ros-industrial / easy_manipulation_deployment

A ROS2 package that allows rapid digitization of robotic work cells and ease of efficient path-planning of manipulators.
Apache License 2.0
48 stars 13 forks source link

EMD not generating grasp tasks or any visual output #15

Open me1kox opened 2 years ago

me1kox commented 2 years ago

Hello,

I have been attempting to get the EMD and EPD packages working with a realsense d435. So far I have followed the steps defined in the documentation but I have ran into a number of issues.

When the grasp planner demo node is launched listening to /camera/depth/color/points, this is the only output received as the called back function to start the grasp planning does not seem to get called:

[INFO] [launch]: All log files can be found below /home/ubuntu20/.ros/log/2022-06-29-11-52-24-218969-ubuntu20-Precision-M4800-22062 

[INFO] [launch]: Default logging verbosity is set to INFO 

/home/ubuntu20/workcell_ws/install/run_grasp_planner/share/run_grasp_planner/config/params_2f.yaml 

[INFO] [demo_node-1]: process started with pid [22064] 

[demo_node-1] [INFO] [1656499944.675345363] [DemoNode]: Direct Workflow Enabled 

[demo_node-1] [INFO] [1656499944.810300476] [GraspScene]: Listening to: /camera/depth/color/points... 

[demo_node-1] [INFO] [1656499944.812710945] [GraspScene]: waiting.... 

When I switch over to using the EPD workflow i.e. with the grasp planning node listening to /processor/epd_localize_output, the planning callback works but I can 't seem to get the planner to generate a task no matter where I place objects. This is a sample of the terminal output I usually get:

[INFO] [launch]: All log files can be found below /home/ubuntu20/.ros/log/2022-06-29-12-11-01-246684-ubuntu20-Precision-M4800-25555 

[INFO] [launch]: Default logging verbosity is set to INFO 

/home/ubuntu20/workcell_ws/install/run_grasp_planner/share/run_grasp_planner/config/params_2f.yaml 

[INFO] [demo_node-1]: process started with pid [25557] 

[demo_node-1] [INFO] [1656501061.694681164] [DemoNode]: EPD Workflow Enabled 

[demo_node-1] [INFO] [1656501061.694805583] [DemoNode]: EPD Localization Enabled 

[demo_node-1] [INFO] [1656501061.837432601] [GraspScene]: Listening to: /processor/epd_localize_output... 

[demo_node-1] [INFO] [1656501061.839727629] [GraspScene]: Waiting for EPD Service 

[demo_node-1] [INFO] [1656501290.260912813] [GraspScene]: EPD Service found. Sending trigger to EPD pipeline 

[demo_node-1] [INFO] [1656501290.261048308] [GraspScene]: Client Not started 

[demo_node-1] [INFO] [1656501290.262123302] [GraspScene]: waiting.... 

[demo_node-1] [INFO] [1656501296.397878382] [GraspScene]: Perception input received! 

[demo_node-1] [INFO] [1656501297.203460521] [GraspScene]: Processing Objects detected by EPD... 

[demo_node-1] Failed to find match for field 'rgb'. 

[demo_node-1] [INFO] [1656501314.094860045] [GraspScene]: EPD detected 1 objects. 

[demo_node-1] [INFO] [1656501314.095139409] [GraspScene]: Loading finger gripper robotiq_2f 

[demo_node-1] [INFO] [1656501314.095465434] [GraspScene]: All End Effectors Loaded 

[demo_node-1] [ERROR] [1656501314.095651786] [GraspScene]: For Object person, no grasp methods can be found with end effector  

[demo_node-1] [INFO] [1656501314.095745040] [GraspScene]: Grasp planning time for  0 [ms]  

[demo_node-1] [ERROR] [1656501314.095870232] [GraspScene]: For Object person, no grasp methods can be found with any given  end effectors provided.  

[demo_node-1] [ERROR] [1656501314.095920300] [GraspScene]: No grasp tasks generated, Skipping request to grasp execution... 

[demo_node-1] Point Cloud Viewer Visualization 

[demo_node-1] [INFO] [1656501314.099369194] [GraspScene]: Grasp Planning complete. 

[demo_node-1] [INFO] [1656501314.099475783] [GraspScene]: Waiting for EPD Service 

[demo_node-1] [INFO] [1656501314.099594356] [GraspScene]: EPD Service found. Sending trigger to EPD pipeline 

[demo_node-1] [INFO] [1656501314.099680227] [GraspScene]: EPD Pipeline triggering complete. STATUS: SUCCESS!! 

[demo_node-1] [INFO] [1656501314.102183085] [GraspScene]: Perception input received! 

[demo_node-1] [INFO] [1656501315.089224773] [GraspScene]: Processing Objects detected by EPD... 

[demo_node-1] Failed to find match for field 'rgb'. 

[demo_node-1] [INFO] [1656501332.318244615] [GraspScene]: EPD detected 1 objects. 

[demo_node-1] [INFO] [1656501332.318390624] [GraspScene]: Loading finger gripper robotiq_2f 

[demo_node-1] [INFO] [1656501332.318495101] [GraspScene]: All End Effectors Loaded 

[demo_node-1] [ERROR] [1656501332.318567457] [GraspScene]: For Object person, no grasp methods can be found with end effector  

[demo_node-1] [INFO] [1656501332.318591613] [GraspScene]: Grasp planning time for  0 [ms]  

[demo_node-1] [ERROR] [1656501332.318616739] [GraspScene]: For Object person, no grasp methods can be found with any given  end effectors provided.  

[demo_node-1] [ERROR] [1656501332.318630863] [GraspScene]: No grasp tasks generated, Skipping request to grasp execution... 

[demo_node-1] Point Cloud Viewer Visualization 

[demo_node-1] [INFO] [1656501332.321877738] [GraspScene]: Grasp Planning complete. 

[demo_node-1] [INFO] [1656501332.321927732] [GraspScene]: Waiting for EPD Service 

[demo_node-1] [INFO] [1656501332.322016662] [GraspScene]: EPD Service found. Sending trigger to EPD pipeline 

[demo_node-1] [INFO] [1656501332.322088749] [GraspScene]: EPD Pipeline already Ongoing 

[demo_node-1] [INFO] [1656501332.324916224] [GraspScene]: Perception input received! 

[demo_node-1] [INFO] [1656501333.091255498] [GraspScene]: Processing Objects detected by EPD... 

[demo_node-1] Failed to find match for field 'rgb'. 

[demo_node-1] Failed to find match for field 'rgb'. 

[demo_node-1] [INFO] [1656501347.102724450] [GraspScene]: EPD detected 2 objects. 

[demo_node-1] [INFO] [1656501347.102842790] [GraspScene]: Loading finger gripper robotiq_2f 

[demo_node-1] [INFO] [1656501347.102937021] [GraspScene]: All End Effectors Loaded 

[demo_node-1] [ERROR] [1656501347.102969182] [GraspScene]: For Object person, no grasp methods can be found with end effector  

[demo_node-1] [INFO] [1656501347.102985402] [GraspScene]: Grasp planning time for  0 [ms]  

[demo_node-1] [ERROR] [1656501347.103008371] [GraspScene]: For Object person, no grasp methods can be found with any given  end effectors provided.  

[demo_node-1] [INFO] [1656501347.103033153] [GraspScene]: Loading finger gripper robotiq_2f 

[demo_node-1] [INFO] [1656501347.103070935] [GraspScene]: All End Effectors Loaded 

[demo_node-1] [ERROR] [1656501347.103088510] [GraspScene]: For Object cell phone, no grasp methods can be found with end effector  

[demo_node-1] [INFO] [1656501347.103100108] [GraspScene]: Grasp planning time for  0 [ms]  

[demo_node-1] [ERROR] [1656501347.103113463] [GraspScene]: For Object cell phone, no grasp methods can be found with any given  end effectors provided.  

[demo_node-1] [ERROR] [1656501347.103123589] [GraspScene]: No grasp tasks generated, Skipping request to grasp execution... 

[demo_node-1] Point Cloud Viewer Visualization 

[demo_node-1] Point Cloud Viewer Visualization 

[demo_node-1] [INFO] [1656501347.106437492] [GraspScene]: Grasp Planning complete. 

[demo_node-1] [INFO] [1656501347.106492144] [GraspScene]: Waiting for EPD Service 

[demo_node-1] [INFO] [1656501347.106650890] [GraspScene]: EPD Service found. Sending trigger to EPD pipeline 

[demo_node-1] [INFO] [1656501347.106693552] [GraspScene]: EPD Pipeline already Ongoing 

[demo_node-1] [INFO] [1656501347.108276275] [GraspScene]: Perception input received! 

[demo_node-1] [INFO] [1656501347.898290498] [GraspScene]: Processing Objects detected by EPD... 

[demo_node-1] Failed to find match for field 'rgb'. 

The EPD node seems to be running as expected provided there is an object in view and I have confirmed that in both cases, all relevant nodes are subscribing to the correct topics. However, I fail to receive any visual output such as plan animations or grasp visualization markers.

I have also notice that with the grasp execution launch, the ros2_control_node terminates after throwing an instance of 'std::runtime_error':

[ros2_control_node-4] terminate called after throwing an instance of 'std::runtime_error' 

[ros2_control_node-4]   what():  no ros2_control tag  

Is this because ROS 2 drivers for the UR robot are needed? I have tried integrating the latest ROS 2 UR drivers with workcell builder but they don't seem to play well with the application.

Any information or assistance with the issues identified above will be greatly appreciated. If any additional information is required, feel free to get in touch.

tanjpg commented 2 years ago

Hi, Apologies for the late response. For the first issue of using the grasp planner with the raw camera output, it seems like there is an issue with the subscriber's callback not being triggered in the first place. Can you help to check if the message type, topic name, etc is correct for both the publisher and the subscriber's end?

For the EPD workflow, what the grasp planner output is saying is that the planner is unable to get any grasp plans with the current detected object. this could be due to multiple reasons, many of which might be rectified through tweaking of various parameters in the yaml file. Do take a look at the documentation to see which parameter might need to be changed. Alternatively you can post your configuration file here first and I will take a look.

For the last issue, the Workcell Builder is currently heavily in development, and ros2 control aspects are not added in yet. you would need to manually add in ros2 control aspects to the workcell builder output's URDF. An example of ros2 control file would be this. I'm not sure how familiar you are with ros2 control, but do let me know if you face issues doing so.