Exercise EXTRA-4.7: Graspable object not detected (Manipulation with ROS2)

iisaac.saito · April 18, 2023, 8:48pm

Graspable object detection by simple_grasping seems to be working IF I run it via bash CLI. In the sample solution C++ code, I see a ROS action client for that as well, so I don’t think I need to run it from CLI along with the exercise code’s executable. However, I don’t think the object is being detected, nor I even see an action client being started.

Btw I found something potentially a bug? Don’t we want to do action_client2->?

  auto action_client2 = rclcpp_action::create_client<Find>(move_group_node, "find_objects");

  if (!action_client->wait_for_action_server(std::chrono::seconds(20))) {
    RCLCPP_ERROR(move_group_node->get_logger(), "Action server not available after waiting");

Almost full output on stdout

user:~/ros2_ws$ ros2 launch get_pose_client test_trajectory.launch.py
[INFO] [launch]: All log files can be found below /home/user/.ros/log/2023-04-18-18-53-44-892764-4_xterm-4513
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [basic_grasping_perception_node-1]: process started with pid [4518]
[INFO] [move_group-2]: process started with pid [4520]
[INFO] [rviz2-3]: process started with pid [4523]
[INFO] [pick_place-4]: process started with pid [4525]
[move_group-2] [WARN] [1681844026.565919382] [move_group.move_group]: MoveGroup launched without ~default_planning_pipeline specifying the namespace for the default planning pipeline configuration
[move_group-2] [WARN] [1681844026.566003827] [move_group.move_group]: Falling back to using the the move_group node namespace (deprecated behavior).
[move_group-2] Parsing robot urdf xml string.
[move_group-2] [INFO] [1681844026.579890791] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[move_group-2] [INFO] [1681844026.582961639] [moveit_robot_model.robot_model]: Loading robot model 'ur3e'...
[move_group-2] [WARN] [1681844026.583518855] [moveit_robot_model.robot_model]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not matchthe URDF frame 'world'
[move_group-2] [INFO] [1681844026.586895581] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[move_group-2] Link ground_plane_box had 0 children
[move_group-2] Link base_link had 2 children
[move_group-2] Link base had 0 children
[move_group-2] Link shoulder_link had 1 children
[move_group-2] Link upper_arm_link had 1 children
[move_group-2] Link forearm_link had 1 children
[move_group-2] Link wrist_1_link had 1 children
[move_group-2] Link wrist_2_link had 1 children
[move_group-2] Link wrist_3_link had 2 children
[move_group-2] Link ee_link had 0 children
[move_group-2] Link tool0 had 1 children
[move_group-2] Link egh_gripper_base_link had 2 children
[move_group-2] Link egh_gripper_left_finger had 0 children
[move_group-2] Link egh_gripper_right_finger had 0 children
[move_group-2] [INFO] [1681844026.832518571] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Publishing maintained planning scene on 'monitored_planning_scene'
[move_group-2] [INFO] [1681844026.832718081] [moveit.ros_planning_interface.moveit_cpp]: Listening to '/joint_states' for joint states
[move_group-2] [INFO] [1681844026.833630228] [moveit_ros.current_state_monitor]: Listening to joint states on topic '/joint_states'
[move_group-2] [INFO] [1681844026.834574366] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/move_group/planning_scene_monitor' for attached collision objects
[move_group-2] [INFO] [1681844026.835111582] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[move_group-2] [INFO] [1681844026.835768787] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/move_group/publish_planning_scene'
[move_group-2] [INFO] [1681844026.835789052] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[move_group-2] [INFO] [1681844026.836533948] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'collision_object'
[move_group-2] [INFO] [1681844026.837099772] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to 'planning_scene_world' for planning scene worldgeometry
[move_group-2] [WARN] [1681844026.837153631] [moveit.ros.occupancy_map_monitor]: Resolution not specified for Octomap. Assuming resolution = 0.1 instead
[move_group-2] [ERROR] [1681844026.837201965] [moveit.ros.occupancy_map_monitor]: No 3D sensor plugin(s) defined for octomap updates
[move_group-2] [INFO] [1681844026.891022087] [moveit.ros_planning_interface.moveit_cpp]: Loading planning pipeline 'move_group'
[rviz2-3] QStandardPaths: XDG_RUNTIME_DIR not set, defaulting to '/tmp/runtime-user'
[move_group-2] [INFO] [1681844028.033863147] [moveit.ros_planning.planning_pipeline]: Using planning interface 'OMPL'
[move_group-2] [INFO] [1681844028.041353879] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.path_tolerance' was not set. Using default value: 0.100000
[move_group-2] [INFO] [1681844028.041389385] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.resample_dt' was not set. Using default value: 0.100000
[move_group-2] [INFO] [1681844028.041399987] [moveit_ros.add_time_optimal_parameterization]: Param 'move_group.min_angle_change' was not set. Using default value: 0.001000
[move_group-2] [INFO] [1681844028.041429553] [moveit_ros.fix_workspace_bounds]: Param 'move_group.default_workspace_bounds' was not set. Using default value: 10.000000
[move_group-2] [INFO] [1681844028.041461799] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_bounds_error' was set to 0.050000
[move_group-2] [INFO] [1681844028.041474649] [moveit_ros.fix_start_state_bounds]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-2] [INFO] [1681844028.041499119] [moveit_ros.fix_start_state_collision]: Param 'move_group.start_state_max_dt' was not set. Using default value: 0.500000
[move_group-2] [INFO] [1681844028.041511807] [moveit_ros.fix_start_state_collision]: Param 'move_group.jiggle_fraction' was not set. Using default value: 0.020000
[move_group-2] [INFO] [1681844028.041536983] [moveit_ros.fix_start_state_collision]: Param 'move_group.max_sampling_attempts' was not set. Using default value: 100
[move_group-2] [INFO] [1681844028.041562388] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Add Time Optimal Parameterization'
[move_group-2] [INFO] [1681844028.041575528] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Workspace Bounds'
[move_group-2] [INFO] [1681844028.041613283] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Bounds'
[move_group-2] [INFO] [1681844028.041624384] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State In Collision'
[move_group-2] [INFO] [1681844028.041631676] [moveit.ros_planning.planning_pipeline]: Using planning request adapter 'Fix Start State Path Constraints'
[move_group-2] [INFO] [1681844028.248823383] [moveit.plugins.moveit_simple_controller_manager]: Added FollowJointTrajectory controller for arm_controller
[move_group-2] [INFO] [1681844028.255208960] [moveit.plugins.moveit_simple_controller_manager]: Added GripperCommand controller for gripper_controller
[move_group-2] [INFO] [1681844028.255377757] [moveit.plugins.moveit_simple_controller_manager]: Returned 2 controllers in list
[move_group-2] [INFO] [1681844028.256621647] [moveit_ros.trajectory_execution_manager]: Trajectory execution is not managing controllers
[move_group-2] [INFO] [1681844028.257182901] [move_group.move_group]: MoveGroup debug mode is ON
[move_group-2] [INFO] [1681844028.307978614] [move_group.move_group]:
[move_group-2]
[move_group-2] ********************************************************
[move_group-2] * MoveGroup using:
[move_group-2] *     - ApplyPlanningSceneService
[move_group-2] *     - ClearOctomapService
[move_group-2] *     - CartesianPathService
[move_group-2] *     - ExecuteTrajectoryAction
[move_group-2] *     - GetPlanningSceneService
[move_group-2] *     - KinematicsService
[move_group-2] *     - MoveAction
[move_group-2] *     - MotionPlanService
[move_group-2] *     - QueryPlannersService
[move_group-2] *     - StateValidationService
[move_group-2] ********************************************************
[move_group-2]
[move_group-2] [INFO] [1681844028.308041609] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[move_group-2] [INFO] [1681844028.308058944] [moveit_move_group_capabilities_base.move_group_context]: MoveGroup context initialization complete
[move_group-2] Loading 'move_group/ApplyPlanningSceneService'...
[move_group-2] Loading 'move_group/ClearOctomapService'...
[move_group-2] Loading 'move_group/MoveGroupCartesianPathService'...
[move_group-2] Loading 'move_group/MoveGroupExecuteTrajectoryAction'...
[move_group-2] Loading 'move_group/MoveGroupGetPlanningSceneService'...
[move_group-2] Loading 'move_group/MoveGroupKinematicsService'...
[move_group-2] Loading 'move_group/MoveGroupMoveAction'...
[move_group-2] Loading 'move_group/MoveGroupPlanService'...
[move_group-2] Loading 'move_group/MoveGroupQueryPlannersService'...
[move_group-2] Loading 'move_group/MoveGroupStateValidationService'...
[move_group-2]
[move_group-2] You can start planning now!
[move_group-2]
[pick_place-4] Parsing robot urdf xml string.
[pick_place-4] [INFO] [1681844028.316331262] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0 seconds
[pick_place-4] [INFO] [1681844028.316703243] [moveit_robot_model.robot_model]: Loading robot model 'ur3e'...
[pick_place-4] [WARN] [1681844028.316728759] [moveit_robot_model.robot_model]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not matchthe URDF frame 'world'
[pick_place-4] [INFO] [1681844028.316740908] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[pick_place-4] Link ground_plane_box had 0 children
[pick_place-4] Link base_link had 2 children
[pick_place-4] Link base had 0 children
[pick_place-4] Link shoulder_link had 1 children
[pick_place-4] Link upper_arm_link had 1 children
[pick_place-4] Link forearm_link had 1 children
[pick_place-4] Link wrist_1_link had 1 children
[pick_place-4] Link wrist_2_link had 1 children
[pick_place-4] Link wrist_3_link had 2 children
[pick_place-4] Link ee_link had 0 children
[pick_place-4] Link tool0 had 1 children
[pick_place-4] Link egh_gripper_base_link had 2 children
[pick_place-4] Link egh_gripper_left_finger had 0 children
[pick_place-4] Link egh_gripper_right_finger had 0 children
[pick_place-4] [INFO] [1681844028.509716315] [move_group_interface]: Ready to take commands for planning group ur_manipulator.
[pick_place-4] [WARN] [1681844028.511247374] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[pick_place-4] [INFO] [1681844028.555099965] [move_group_interface]: Ready to take commands for planning group gripper.
[pick_place-4] [INFO] [1681844028.555846179] [moveit_ros.current_state_monitor]: Listening to joint states on topic 'joint_states'
[rviz2-3] [INFO] [1681844028.780246193] [rviz2]: Stereo is NOT SUPPORTED
[rviz2-3] [INFO] [1681844028.781626625] [rviz2]: OpenGl version: 3.1 (GLSL 1.4)
[pick_place-4] [INFO] [1681844029.056094665] [move_group_demo]: Planning frame: world
[pick_place-4] [INFO] [1681844029.056272949] [move_group_demo]: End-effector link: tool0
[pick_place-4] [INFO] [1681844029.056289956] [move_group_demo]: Available Planning Groups:
[pick_place-4] [INFO] [1681844029.056308387] [move_group_demo]: Get Robot Current State
[pick_place-4] [INFO] [1681844029.056347153] [move_group_demo]: Going Home
[move_group-2] [INFO] [1681844029.057307239] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-2] [INFO] [1681844029.057584792] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[rviz2-3] [INFO] [1681844029.132666346] [rviz2]: Stereo is NOT SUPPORTED
[pick_place-4] [INFO] [1681844029.358569201] [move_group_interface]: Planning request accepted
[move_group-2] [INFO] [1681844030.249660766] [moveit_ros.current_state_monitor]: Didn't received robot state (joint angles) with recent timestamp within 1.000000 seconds.
[move_group-2] Check clock synchronization if your are running ROS across multiple machines!
[move_group-2] [WARN] [1681844030.249715528] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Failed to fetch current robot state.
[move_group-2] [INFO] [1681844030.249906834] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[rviz2-3] Warning: class_loader.impl: SEVERE WARNING!!! A namespace collision has occured with plugin factory for class rviz_default_plugins::displays::InteractiveMarkerDisplay. New factory will OVERWRITE existing one. This situation occurs when libraries containing plugins are directly linked against an executable (the one running right now generating this message). Please separate plugins out into their own library or just don't link against the library and use either class_loader::ClassLoader/MultiLibraryClassLoader to open.
[rviz2-3]          at line 253 in /opt/ros/foxy/include/class_loader/class_loader_core.hpp
[move_group-2] [INFO] [1681844030.667109603] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[move_group-2] [INFO] [1681844030.817376101] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:220 - ur_manipulator/ur_manipulator: Starting planning with 1 states already in datastructure
[move_group-2] [INFO] [1681844030.972134505] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:354 - ur_manipulator/ur_manipulator: Created 4 states (2 start + 2 goal)
[move_group-2] [INFO] [1681844030.972179950] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:138 - Solution found in 0.154962 seconds
[move_group-2] [INFO] [1681844030.983182762] [ompl]: /tmp/binarydeb/ros-foxy-ompl-1.5.0/src/ompl/geometric/src/SimpleSetup.cpp:179 - SimpleSetup: Path simplificationtook 0.010912 seconds and changed from 3 to 2 states
[move_group-2] [INFO] [1681844031.005431687] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was computed succesfully.
[pick_place-4] [INFO] [1681844031.266004315] [move_group_interface]: Planning request complete!
[pick_place-4] [INFO] [1681844031.366286525] [move_group_interface]: time taken to generate plan: 0.165875 seconds
[move_group-2] [INFO] [1681844031.370203470] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Received goal request
[move_group-2] [INFO] [1681844031.370348144] [moveit_move_group_default_capabilities.execute_trajectory_action_capability]: Execution request received
[move_group-2] [INFO] [1681844031.370559943] [moveit_ros.trajectory_execution_manager]: Validating trajectory with allowed_start_tolerance 0.1
[move_group-2] [INFO] [1681844031.374667171] [moveit_ros.trajectory_execution_manager]: Starting trajectory execution ...
[move_group-2] [INFO] [1681844031.374751672] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: sending trajectory to arm_controller
[move_group-2] [INFO] [1681844031.383579866] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: arm_controller started execution
[move_group-2] [INFO] [1681844031.383614712] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Goal request accepted!
[pick_place-4] [INFO] [1681844031.667886271] [move_group_interface]: Execute request accepted
[rviz2-3] [ERROR] [1681844033.536488380] [moveit_ros_visualization.motion_planning_frame]: Action server: /recognize_objects not available
[rviz2-3] [ERROR] [1681844033.811926216] [moveit_rviz_plugin_render_tools.trajectory_visualization]: No robot state or robot model loaded
[rviz2-3] Parsing robot urdf xml string.
[rviz2-3] [INFO] [1681844033.850209170] [moveit_rdf_loader.rdf_loader]: Loaded robot model in 0.0138647 seconds
[rviz2-3] [INFO] [1681844033.850331604] [moveit_robot_model.robot_model]: Loading robot model 'ur3e'...
[rviz2-3] [WARN] [1681844033.850365476] [moveit_robot_model.robot_model]: Skipping virtual joint 'fixed_base' because its child frame 'base_link' does not match the URDF frame 'world'
[rviz2-3] [INFO] [1681844033.850376875] [moveit_robot_model.robot_model]: No root/virtual joint specified in SRDF. Assuming fixed joint
[rviz2-3] Link ground_plane_box had 0 children
[rviz2-3] Link base_link had 2 children
[rviz2-3] Link base had 0 children
[rviz2-3] Link shoulder_link had 1 children
[rviz2-3] Link upper_arm_link had 1 children
[rviz2-3] Link forearm_link had 1 children
[rviz2-3] Link wrist_1_link had 1 children
[rviz2-3] Link wrist_2_link had 1 children
[rviz2-3] Link wrist_3_link had 2 children
[rviz2-3] Link ee_link had 0 children
[rviz2-3] Link tool0 had 1 children
[rviz2-3] Link egh_gripper_base_link had 2 children
[rviz2-3] Link egh_gripper_left_finger had 0 children
[rviz2-3] Link egh_gripper_right_finger had 0 children
[rviz2-3] [INFO] [1681844034.359506702] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Starting planning scene monitor
[rviz2-3] [INFO] [1681844034.362010751] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Listening to '/monitored_planning_scene'
[rviz2-3] [WARN] [1681844034.450571829] [rviz2]: The STL file 'package://ur_e_description/meshes/egh_base.stl' is malformed. It starts with the word 'solid', indicating that it's an ASCII STL file, but it does not contain the word 'endsolid' so it is either a malformed ASCII STL file or it is actually a binary STL file. Trying tointerpret it as a binary STL file instead.
[rviz2-3] [WARN] [1681844034.591495113] [rviz2]: The STL file 'package://ur_e_description/meshes/egh_finger.stl' is malformed. It starts with the word 'solid', indicating that it's an ASCII STL file, but it does not contain the word 'endsolid' so it is either a malformed ASCII STL file or it is actually a binary STL file. Trying to interpret it as a binary STL file instead.
[basic_grasping_perception_node-1] [INFO] [1681844035.834622530] [basic_grasping_perception]: basic_grasping_perception initialized
[rviz2-3] [INFO] [1681844036.513050427] [interactive_marker_display_94343032595984]: Connected on namespace: rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic
[rviz2-3] Link ground_plane_box had 0 children
[rviz2-3] Link base_link had 2 children
[rviz2-3] Link base had 0 children
[rviz2-3] Link shoulder_link had 1 children
[rviz2-3] Link upper_arm_link had 1 children
[rviz2-3] Link forearm_link had 1 children
[rviz2-3] Link wrist_1_link had 1 children
[rviz2-3] Link wrist_2_link had 1 children
[rviz2-3] Link wrist_3_link had 2 children
[rviz2-3] Link ee_link had 0 children
[rviz2-3] Link tool0 had 1 children
[rviz2-3] Link egh_gripper_base_link had 2 children
[rviz2-3] Link egh_gripper_left_finger had 0 children
[rviz2-3] Link egh_gripper_right_finger had 0 children
[rviz2-3] [WARN] [1681844036.604949653] [moveit_robot_model.joint_model_group]: comparing input tip: wrist_3_link to this groups tip: tool0
[rviz2-3] [INFO] [1681844036.617715100] [moveit_ros_visualization.motion_planning_frame]: group ur_manipulator
[rviz2-3] [INFO] [1681844036.617759176] [moveit_ros_visualization.motion_planning_frame]: Constructing new MoveGroup connection for group 'ur_manipulator' in namespace ''
[rviz2-3] [WARN] [1681844036.617893700] [rcl.logging_rosout]: Publisher already registered for provided node name. If this is due to multiple nodes with the same name then all logs for that logger name will go out over the existing publisher. As soon as any node with that name is destructed it will unregister the publisher, preventing any further logs for that name from being published on the rosout topic.
[rviz2-3] [INFO] [1681844036.750572810] [move_group_interface]: Ready to take commands for planning group ur_manipulator.
[rviz2-3] [INFO] [1681844036.750666632] [move_group_interface]: Looking around: no
[rviz2-3] [INFO] [1681844036.750698589] [move_group_interface]: Replanning: no
[rviz2-3] [INFO] [1681844036.878462632] [interactive_marker_display_94343032595984]: Sending request for interactive markers
[rviz2-3] [INFO] [1681844037.139368868] [interactive_marker_display_94343032595984]: Service response received for initialization
[move_group-2] [WARN] [1681844041.838062733] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[move_group-2] [WARN] [1681844041.838127991] [moveit_ros.trajectory_execution_manager]: Controller handle arm_controller reports status RUNNING
[move_group-2] [INFO] [1681844041.838148783] [moveit_ros.trajectory_execution_manager]: Completed trajectory execution with status RUNNING ...

:

[pick_place-4] [INFO] [1681844083.491711224] [move_group_interface]: Execute request success!
[move_group-2] [INFO] [1681844084.205135987] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: Controller arm_controller successfully finished
^C[WARNING] [launch]: user interrupted with ctrl-c (SIGINT)
[pick_place-4] [INFO] [1681844090.917501674] [rclcpp]: signal_handler(signal_value=2)
[rviz2-3] [INFO] [1681844090.917503434] [rclcpp]: signal_handler(signal_value=2)
[basic_grasping_perception_node-1] [INFO] [1681844090.917523112] [rclcpp]: signal_handler(signal_value=2)
[move_group-2] [INFO] [1681844090.917990698] [rclcpp]: signal_handler(signal_value=2)
[move_group-2] [INFO] [1681844090.933777657] [moveit.ros_planning_interface.moveit_cpp]: Deleting MoveItCpp
[INFO] [pick_place-4]: process has finished cleanly [pid 4525]
[pick_place-4] gripper, ur_manipulator,
[rviz2-3] [WARN] [1681844091.150643706] [interactive_marker_display_94343032595984]: Server not available while running, resetting
[move_group-2] [INFO] [1681844091.175162963] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopped publishing maintained planning scene.
[move_group-2] [INFO] [1681844091.175580126] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping world geometry monitor
[move_group-2] [INFO] [1681844091.175740972] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor
[move_group-2] Warning: class_loader.ClassLoader: SEVERE WARNING!!! Attempting to unload library while objects created by this loader exist in the heap! You should delete your objects before attempting to unload the library or destroying the ClassLoader. The library will NOT be unloaded.
[move_group-2]          at line 127 in /tmp/binarydeb/ros-foxy-class-loader-2.0.2/src/class_loader.cpp
[INFO] [move_group-2]: process has finished cleanly [pid 4520]
[move_group-2]
[rviz2-3]
[rviz2-3] >>> [rcutils|error_handling.c:108] rcutils_set_error_state()
[rviz2-3] This error state is being overwritten:
[rviz2-3]
[rviz2-3]   'rcl node's context is invalid, at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/node.c:441'
[rviz2-3]
[rviz2-3] with this new error message:
[rviz2-3]
[rviz2-3]   'publisher's context is invalid, at /tmp/binarydeb/ros-foxy-rcl-1.1.11/src/rcl/publisher.c:423'
[rviz2-3]
[rviz2-3] rcutils_reset_error() should be called after error handling to avoid this.
[rviz2-3] <<<
[rviz2-3] [INFO] [1681844091.412100697] [moveit_ros.planning_scene_monitor.planning_scene_monitor]: Stopping planning scene monitor
[INFO] [rviz2-3]: process has finished cleanly [pid 4523]
[INFO] [basic_grasping_perception_node-1]: process has finished cleanly [pid 4518]
user:~/ros2_ws$

Result of watching Action nodes stays pretty much like this:

: ros2 action info /find_objects
Action: /find_objects
Action clients: 0
Action servers: 1
    /basic_grasping_perception

I’m also checking /object_cloud topic (on both CLI and on RViz) but I don’t see it published during the execution of the sample code (I DO see it published when I run simple_grasping action client call on CLI).

bayodesegun · April 19, 2023, 10:10am

Hi @iisaac.saito
Could you please point us to the particular unit of the course, and section of the notebook, where you have this problem?

iisaac.saito · April 19, 2023, 1:29pm

Sorry, but I’ve already put all the specific info the title of this post. I don’t know how I can be more specific than that. It’s the extra exercise of 4.7 (why not call it exercise 4.8 to avoid confusion?).

bayodesegun · April 19, 2023, 4:40pm

Sorry, I missed the information in the title completely . You could also add a screenshot and tell us the name of the unit.

Anyways, I’ll call the attention of the course maintainer to this.

aezquerrostudent · April 20, 2023, 3:14pm

Hello @iisaac.saito ,

I’ve just updated this unit with a proper solution to this exercise. You should see the new notebooks when you go back to this unit. Hope this solves your issue.

iisaac.saito · April 20, 2023, 7:13pm

Thanks, hm, I gave it a try but I still don’t see graspable objects (via the topic /object_cloud) are not detected. I will keep looking, but you tested it and saw the objects detected, right? @aezquerrostudent

aezquerrostudent · April 21, 2023, 8:22am

Yes, I can confirm it’s working correctly in my tests. The new code is using the pose of the object obtained from perception for the grasp position:

void pregrasp() {

    RCLCPP_INFO(LOGGER, "Pregrasp Position");

    target_pose1.orientation.x = -1.0;
    target_pose1.orientation.y = 0.00;
    target_pose1.orientation.z = 0.00;
    target_pose1.orientation.w = 0.00;
    target_pose1.position.x = pose_x;
    target_pose1.position.y = pose_y;
    target_pose1.position.z = 0.284;
    move_group_arm.setPoseTarget(target_pose1);

    bool success_arm = (move_group_arm.plan(my_plan_arm) ==
                        moveit::planning_interface::MoveItErrorCode::SUCCESS);

    // Execute
    move_group_arm.execute(my_plan_arm);
  }

So, if it doesn’t detect anything the grasp is not going to work. It can happen that the detection is not made correctly (during my tests it happened like 1 time from 10+ tests). In that case, you will see that the pick & place motion is not finished. If this happens, just rerun everything and it should work.

iisaac.saito · April 23, 2023, 10:34am

Somehow I managed to get it to work. I still can’t tell the root cause, but these are what I ended up doing:

First, my mistake, my executable was pointing to a wrong file. Fixed.
I had to reboot param_server processes after unsuccessful runs of exercise 4.7 application, in order for both moveit and the graspable objects detection.

Btw I see exercise 4.7 cpp code on the tutorial page has changed A LOT, in a good way. It is now following the same format that has been used until 4.6 so it’s easy to understand the difference (previously 4.7 code was full-scratch, with only a single main function).