Error when setting up Gazebo with MoveIt! and Stretch

Continuing the discussion from Error when setting up Stretch simulation on Gazebo:

Now that I have the initial demo running with the teleop_twist, I was trying to run the MoveIt! demo per the ReadMe. Everything works very well on the MoveIt! interface, but when I try to execute a plan, I receive the following error outputs from $ roslaunch stretch_moveit_config demo_gazebo.launch:

[ INFO] [1624391236.290471975, 16.464000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391236.290540103, 16.464000000]: Failed to validate trajectory: couldn’t receive full current joint state within 1s
[ INFO] [1624391236.312759822, 16.469000000]: ABORTED: Solution found but controller failed during execution

Since I don’t have a joystick, I made a simple launch file for the teleop_twist_keyboard, but I didn’t see anything that I thought would affect the MoveIt! operation.

I am just learning Gazebo and MoveIt! so I really appreciate the assistance with the setup!

There is much more in the terminal output prior to those errors, but none of it seemed relevant. Just in case, it's here. $ roslaunch stretch_moveit_config demo_gazebo.launch ... logging to /home/hcilab/.ros/log/88c7b84e-d392-11eb-978b-d0577b4196c8/roslaunch-hcilab-HP-Pavilion-24585.log Checking log directory for disk usage. This may take a while. Press Ctrl-C to interrupt Done checking log file disk usage. Usage is <1GB.

started roslaunch server http://127.0.0.1:44501/

SUMMARY

PARAMETERS

  • /move_group/allow_trajectory_execution: True
  • /move_group/capabilities:
  • /move_group/controller_list: [{‘default’: True…
  • /move_group/disable_capabilities:
  • /move_group/jiggle_fraction: 0.05
  • /move_group/joint_state_controller/publish_rate: 50
  • /move_group/joint_state_controller/type: joint_state_contr…
  • /move_group/max_range: 5.0
  • /move_group/max_safe_path_cost: 1
  • /move_group/moveit_controller_manager: moveit_simple_con…
  • /move_group/moveit_manage_controllers: True
  • /move_group/octomap_resolution: 0.025
  • /move_group/planner_configs/AnytimePathShortening/hybridize: True
  • /move_group/planner_configs/AnytimePathShortening/max_hybrid_paths: 24
  • /move_group/planner_configs/AnytimePathShortening/num_planners: 4
  • /move_group/planner_configs/AnytimePathShortening/planners:
  • /move_group/planner_configs/AnytimePathShortening/shortcut: True
  • /move_group/planner_configs/AnytimePathShortening/type: geometric::Anytim…
  • /move_group/planner_configs/BFMT/balanced: 0
  • /move_group/planner_configs/BFMT/cache_cc: 1
  • /move_group/planner_configs/BFMT/extended_fmt: 1
  • /move_group/planner_configs/BFMT/heuristics: 1
  • /move_group/planner_configs/BFMT/nearest_k: 1
  • /move_group/planner_configs/BFMT/num_samples: 1000
  • /move_group/planner_configs/BFMT/optimality: 1
  • /move_group/planner_configs/BFMT/radius_multiplier: 1.0
  • /move_group/planner_configs/BFMT/type: geometric::BFMT
  • /move_group/planner_configs/BKPIECE/border_fraction: 0.9
  • /move_group/planner_configs/BKPIECE/failed_expansion_score_factor: 0.5
  • /move_group/planner_configs/BKPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/BKPIECE/range: 0.0
  • /move_group/planner_configs/BKPIECE/type: geometric::BKPIECE
  • /move_group/planner_configs/BiEST/range: 0.0
  • /move_group/planner_configs/BiEST/type: geometric::BiEST
  • /move_group/planner_configs/BiTRRT/cost_threshold: 1e300
  • /move_group/planner_configs/BiTRRT/frountier_node_ratio: 0.1
  • /move_group/planner_configs/BiTRRT/frountier_threshold: 0.0
  • /move_group/planner_configs/BiTRRT/init_temperature: 100
  • /move_group/planner_configs/BiTRRT/range: 0.0
  • /move_group/planner_configs/BiTRRT/temp_change_factor: 0.1
  • /move_group/planner_configs/BiTRRT/type: geometric::BiTRRT
  • /move_group/planner_configs/EST/goal_bias: 0.05
  • /move_group/planner_configs/EST/range: 0.0
  • /move_group/planner_configs/EST/type: geometric::EST
  • /move_group/planner_configs/FMT/cache_cc: 1
  • /move_group/planner_configs/FMT/extended_fmt: 1
  • /move_group/planner_configs/FMT/heuristics: 0
  • /move_group/planner_configs/FMT/nearest_k: 1
  • /move_group/planner_configs/FMT/num_samples: 1000
  • /move_group/planner_configs/FMT/radius_multiplier: 1.1
  • /move_group/planner_configs/FMT/type: geometric::FMT
  • /move_group/planner_configs/KPIECE/border_fraction: 0.9
  • /move_group/planner_configs/KPIECE/failed_expansion_score_factor: 0.5
  • /move_group/planner_configs/KPIECE/goal_bias: 0.05
  • /move_group/planner_configs/KPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/KPIECE/range: 0.0
  • /move_group/planner_configs/KPIECE/type: geometric::KPIECE
  • /move_group/planner_configs/LBKPIECE/border_fraction: 0.9
  • /move_group/planner_configs/LBKPIECE/min_valid_path_fraction: 0.5
  • /move_group/planner_configs/LBKPIECE/range: 0.0
  • /move_group/planner_configs/LBKPIECE/type: geometric::LBKPIECE
  • /move_group/planner_configs/LBTRRT/epsilon: 0.4
  • /move_group/planner_configs/LBTRRT/goal_bias: 0.05
  • /move_group/planner_configs/LBTRRT/range: 0.0
  • /move_group/planner_configs/LBTRRT/type: geometric::LBTRRT
  • /move_group/planner_configs/LazyPRM/range: 0.0
  • /move_group/planner_configs/LazyPRM/type: geometric::LazyPRM
  • /move_group/planner_configs/LazyPRMstar/type: geometric::LazyPR…
  • /move_group/planner_configs/PDST/type: geometric::PDST
  • /move_group/planner_configs/PRM/max_nearest_neighbors: 10
  • /move_group/planner_configs/PRM/type: geometric::PRM
  • /move_group/planner_configs/PRMstar/type: geometric::PRMstar
  • /move_group/planner_configs/ProjEST/goal_bias: 0.05
  • /move_group/planner_configs/ProjEST/range: 0.0
  • /move_group/planner_configs/ProjEST/type: geometric::ProjEST
  • /move_group/planner_configs/RRT/goal_bias: 0.05
  • /move_group/planner_configs/RRT/range: 0.0
  • /move_group/planner_configs/RRT/type: geometric::RRT
  • /move_group/planner_configs/RRTConnect/range: 0.0
  • /move_group/planner_configs/RRTConnect/type: geometric::RRTCon…
  • /move_group/planner_configs/RRTstar/delay_collision_checking: 1
  • /move_group/planner_configs/RRTstar/goal_bias: 0.05
  • /move_group/planner_configs/RRTstar/range: 0.0
  • /move_group/planner_configs/RRTstar/type: geometric::RRTstar
  • /move_group/planner_configs/SBL/range: 0.0
  • /move_group/planner_configs/SBL/type: geometric::SBL
  • /move_group/planner_configs/SPARS/dense_delta_fraction: 0.001
  • /move_group/planner_configs/SPARS/max_failures: 1000
  • /move_group/planner_configs/SPARS/sparse_delta_fraction: 0.25
  • /move_group/planner_configs/SPARS/stretch_factor: 3.0
  • /move_group/planner_configs/SPARS/type: geometric::SPARS
  • /move_group/planner_configs/SPARStwo/dense_delta_fraction: 0.001
  • /move_group/planner_configs/SPARStwo/max_failures: 5000
  • /move_group/planner_configs/SPARStwo/sparse_delta_fraction: 0.25
  • /move_group/planner_configs/SPARStwo/stretch_factor: 3.0
  • /move_group/planner_configs/SPARStwo/type: geometric::SPARStwo
  • /move_group/planner_configs/STRIDE/degree: 16
  • /move_group/planner_configs/STRIDE/estimated_dimension: 0.0
  • /move_group/planner_configs/STRIDE/goal_bias: 0.05
  • /move_group/planner_configs/STRIDE/max_degree: 18
  • /move_group/planner_configs/STRIDE/max_pts_per_leaf: 6
  • /move_group/planner_configs/STRIDE/min_degree: 12
  • /move_group/planner_configs/STRIDE/min_valid_path_fraction: 0.2
  • /move_group/planner_configs/STRIDE/range: 0.0
  • /move_group/planner_configs/STRIDE/type: geometric::STRIDE
  • /move_group/planner_configs/STRIDE/use_projected_distance: 0
  • /move_group/planner_configs/TRRT/frountierNodeRatio: 0.1
  • /move_group/planner_configs/TRRT/frountier_threshold: 0.0
  • /move_group/planner_configs/TRRT/goal_bias: 0.05
  • /move_group/planner_configs/TRRT/init_temperature: 10e-6
  • /move_group/planner_configs/TRRT/k_constant: 0.0
  • /move_group/planner_configs/TRRT/max_states_failed: 10
  • /move_group/planner_configs/TRRT/min_temperature: 10e-10
  • /move_group/planner_configs/TRRT/range: 0.0
  • /move_group/planner_configs/TRRT/temp_change_factor: 2.0
  • /move_group/planner_configs/TRRT/type: geometric::TRRT
  • /move_group/planning_plugin: ompl_interface/OM…
  • /move_group/planning_scene_monitor/publish_geometry_updates: True
  • /move_group/planning_scene_monitor/publish_planning_scene: True
  • /move_group/planning_scene_monitor/publish_state_updates: True
  • /move_group/planning_scene_monitor/publish_transforms_updates: True
  • /move_group/request_adapters: default_planner_r…
  • /move_group/sensors: [{'point_subsampl…
  • /move_group/start_state_max_bounds_error: 0.1
  • /move_group/stretch_arm/default_planner_config: RRTConnect
  • /move_group/stretch_arm/planner_configs: ['AnytimePathShor…
  • /move_group/stretch_gripper/default_planner_config: RRTConnect
  • /move_group/stretch_gripper/planner_configs: ['AnytimePathShor…
  • /move_group/stretch_head/default_planner_config: RRTConnect
  • /move_group/stretch_head/planner_configs: ['AnytimePathShor…
  • /move_group/trajectory_execution/allowed_execution_duration_scaling: 1.2
  • /move_group/trajectory_execution/allowed_goal_duration_margin: 0.5
  • /move_group/trajectory_execution/allowed_start_tolerance: 0.01
  • /robot_description: <?xml version="1…
  • /robot_description_kinematics/stretch_arm/kinematics_solver: kdl_kinematics_pl…
  • /robot_description_kinematics/stretch_arm/kinematics_solver_search_resolution: 0.005
  • /robot_description_kinematics/stretch_arm/kinematics_solver_timeout: 0.005
  • /robot_description_kinematics/stretch_head/kinematics_solver: kdl_kinematics_pl…
  • /robot_description_kinematics/stretch_head/kinematics_solver_search_resolution: 0.005
  • /robot_description_kinematics/stretch_head/kinematics_solver_timeout: 0.1
  • /robot_description_planning/default_acceleration_scaling_factor: 0.1
  • /robot_description_planning/default_velocity_scaling_factor: 0.1
  • /robot_description_planning/joint_limits/joint_arm_l0/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l0/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l0/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l0/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l1/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l1/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l1/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l1/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l2/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l2/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l2/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l2/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_arm_l3/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_arm_l3/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_arm_l3/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_arm_l3/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_gripper_finger_left/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_gripper_finger_right/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_lift/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_lift/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_lift/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_lift/max_velocity: 1
  • /robot_description_planning/joint_limits/joint_wrist_yaw/has_acceleration_limits: False
  • /robot_description_planning/joint_limits/joint_wrist_yaw/has_velocity_limits: True
  • /robot_description_planning/joint_limits/joint_wrist_yaw/max_acceleration: 0
  • /robot_description_planning/joint_limits/joint_wrist_yaw/max_velocity: 1
  • /robot_description_semantic: <?xml version="1…
  • /rosdistro: melodic
  • /rosversion: 1.14.10

NODES
/
move_group (moveit_ros_move_group/move_group)
rviz_hcilab_HP_Pavilion_24585_8593209250659358117 (rviz/rviz)
virtual_joint_broadcaster_0 (tf2_ros/static_transform_publisher)

ROS_MASTER_URI=http://localhost:11311

process[virtual_joint_broadcaster_0-1]: started with pid [24644]
process[move_group-2]: started with pid [24645]
process[rviz_hcilab_HP_Pavilion_24585_8593209250659358117-3]: started with pid [24650]
[ INFO] [1624391209.429076265]: Loading robot model ‘stretch_description’…
[ INFO] [1624391209.430194019]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ INFO] [1624391209.522116464]: rviz version 1.13.17
[ INFO] [1624391209.522199886]: compiled against Qt version 5.9.5
[ INFO] [1624391209.522223436]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1624391209.528010789]: Forcing OpenGl version 0.
[ WARN] [1624391209.726635487, 6.675000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391209.733804592, 6.678000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1624391210.563529605, 6.949000000]: Publishing maintained planning scene on ‘monitored_planning_scene’
[ INFO] [1624391210.567065540, 6.950000000]: MoveGroup debug mode is ON
Starting planning scene monitors…
[ INFO] [1624391210.567109979, 6.950000000]: Starting planning scene monitor
[ INFO] [1624391210.569398280, 6.951000000]: Listening to ‘/planning_scene’
[ INFO] [1624391210.569430219, 6.951000000]: Starting world geometry update monitor for collision objects, attached objects, octomap updates.
[ INFO] [1624391210.572020157, 6.952000000]: Listening to ‘/collision_object’
[ INFO] [1624391210.573589682, 6.952000000]: Listening to ‘/planning_scene_world’ for planning scene world geometry
[ INFO] [1624391210.665396361, 6.982000000]: Stereo is NOT SUPPORTED
[ INFO] [1624391210.665495789, 6.982000000]: OpenGL device: GeForce GTX 960M/PCIe/SSE2
[ INFO] [1624391210.665563776, 6.982000000]: OpenGl version: 4.6 (GLSL 4.6).
[ WARN] [1624391210.684220753, 6.988000000]: Convex hull creation failed
[ WARN] [1624391210.684335455, 6.988000000]: Convex hull creation failed
[ INFO] [1624391212.682383131, 7.730000000]: Listening to ‘/head_mount_kinect/depth_registered/points’ using message filter with target frame 'base_link ’
[ INFO] [1624391212.686262401, 7.731000000]: Listening to ‘/attached_collision_object’ for attached collision objects
Planning scene monitors started.
[ INFO] [1624391212.713561005, 7.741000000]: Initializing OMPL interface using ROS parameters
[ INFO] [1624391212.755362156, 7.756000000]: Using planning interface ‘OMPL’
[ INFO] [1624391212.759363849, 7.758000000]: Param ‘default_workspace_bounds’ was not set. Using default value: 10
[ INFO] [1624391212.759828534, 7.758000000]: Param ‘start_state_max_bounds_error’ was set to 0.1
[ INFO] [1624391212.760221692, 7.758000000]: Param ‘start_state_max_dt’ was not set. Using default value: 0.5
[ INFO] [1624391212.760740347, 7.759000000]: Param ‘start_state_max_dt’ was not set. Using default value: 0.5
[ INFO] [1624391212.761124518, 7.759000000]: Param ‘jiggle_fraction’ was set to 0.05
[ INFO] [1624391212.761504321, 7.759000000]: Param ‘max_sampling_attempts’ was not set. Using default value: 100
[ INFO] [1624391212.761556067, 7.759000000]: Using planning request adapter ‘Add Time Parameterization’
[ INFO] [1624391212.761578265, 7.759000000]: Using planning request adapter ‘Fix Workspace Bounds’
[ INFO] [1624391212.761600973, 7.759000000]: Using planning request adapter ‘Fix Start State Bounds’
[ INFO] [1624391212.761623944, 7.759000000]: Using planning request adapter ‘Fix Start State In Collision’
[ INFO] [1624391212.761641439, 7.759000000]: Using planning request adapter ‘Fix Start State Path Constraints’
[ INFO] [1624391212.984467244, 7.846000000]: Added FollowJointTrajectory controller for stretch_arm_controller
[ INFO] [1624391213.273770230, 7.946000000]: Added FollowJointTrajectory controller for stretch_head_controller
[ INFO] [1624391213.488519744, 8.019000000]: Added FollowJointTrajectory controller for stretch_gripper_controller
[ INFO] [1624391213.488627166, 8.019000000]: Returned 3 controllers in list
[ INFO] [1624391213.501528525, 8.022000000]: Trajectory execution is managing controllers
Loading ‘move_group/ApplyPlanningSceneService’…
Loading ‘move_group/ClearOctomapService’…
Loading ‘move_group/MoveGroupCartesianPathService’…
Loading ‘move_group/MoveGroupExecuteTrajectoryAction’…
Loading ‘move_group/MoveGroupGetPlanningSceneService’…
Loading ‘move_group/MoveGroupKinematicsService’…
Loading ‘move_group/MoveGroupMoveAction’…
Loading ‘move_group/MoveGroupPickPlaceAction’…
Loading ‘move_group/MoveGroupPlanService’…
Loading ‘move_group/MoveGroupQueryPlannersService’…
Loading ‘move_group/MoveGroupStateValidationService’…
[ INFO] [1624391213.562829052, 8.043000000]:


  • MoveGroup using:
  • - ApplyPlanningSceneService
    
  • - ClearOctomapService
    
  • - CartesianPathService
    
  • - ExecuteTrajectoryAction
    
  • - GetPlanningSceneService
    
  • - KinematicsService
    
  • - MoveAction
    
  • - PickPlaceAction
    
  • - MotionPlanService
    
  • - QueryPlannersService
    
  • - StateValidationService
    

[ INFO] [1624391213.562906681, 8.043000000]: MoveGroup context using planning plugin ompl_interface/OMPLPlanner
[ INFO] [1624391213.562937034, 8.043000000]: MoveGroup context initialization complete

You can start planning now!

[ INFO] [1624391214.049193554, 8.220000000]: Loading robot model ‘stretch_description’…
[ INFO] [1624391214.049257922, 8.220000000]: No root/virtual joint specified in SRDF. Assuming fixed joint
[ WARN] [1624391214.326861418, 8.317000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391214.335422661, 8.320000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ INFO] [1624391215.171264846, 8.621000000]: Starting planning scene monitor
[ INFO] [1624391215.173783693, 8.622000000]: Listening to ‘/move_group/monitored_planning_scene’
[ WARN] [1624391216.481111437, 9.065000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391216.481310627, 9.065000000]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF.
[ WARN] [1624391216.487478361, 9.067000000]: Unable to transform object from frame ‘map’ to planning frame ‘base_link’ (Could not find a connection between ‘base_link’ and ‘map’ because they are not part of the same tree.Tf has two or more unconnected trees.)
[ INFO] [1624391216.488590112, 9.067000000]: Constructing new MoveGroup connection for group ‘stretch_arm’ in namespace ‘’
[ INFO] [1624391217.584871065, 9.475000000]: Ready to take commands for planning group stretch_arm.
[ INFO] [1624391217.585017244, 9.476000000]: Looking around: no
[ INFO] [1624391217.585053278, 9.476000000]: Replanning: no
[ INFO] [1624391235.042137248, 16.000000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391235.042203297, 16.000000000]: Failed to fetch current robot state.
[ WARN] [1624391235.042285753, 16.000000000]: Unable to transform object from frame ‘map’ to planning frame ‘base_link’ (Could not find a connection between ‘base_link’ and ‘map’ because they are not part of the same tree.Tf has two or more unconnected trees.)
[ INFO] [1624391235.042391243, 16.000000000]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1624391235.042574288, 16.000000000]: Planning attempt 1 of at most 1
[ INFO] [1624391235.044634186, 16.000000000]: Planner configuration ‘stretch_arm[RRTConnect]’ will use planner ‘geometric::RRTConnect’. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1624391235.045747809, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045799494, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045852315, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.045958093, 16.000000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.072180338, 16.009000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.073048387, 16.009000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.074481000, 16.010000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.078630898, 16.012000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.081267629, 16.013000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.036013 seconds
[ INFO] [1624391235.081541725, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.081720184, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.083426927, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.083548021, 16.013000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.095424976, 16.017000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.097279729, 16.017000000]: stretch_arm[RRTConnect]: Created 8 states (2 start + 6 goal)
[ INFO] [1624391235.099494653, 16.017000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.101233910, 16.018000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.111613714, 16.021000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.030130 seconds
[ INFO] [1624391235.111791181, 16.021000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.111856208, 16.021000000]: stretch_arm[RRTConnect]: Starting planning with 1 states already in datastructure
[ INFO] [1624391235.122429290, 16.024000000]: stretch_arm[RRTConnect]: Created 6 states (2 start + 4 goal)
[ INFO] [1624391235.125721590, 16.025000000]: stretch_arm[RRTConnect]: Created 7 states (2 start + 5 goal)
[ INFO] [1624391235.134678419, 16.026000000]: ParallelPlan::solve(): Solution found by one or more threads in 0.022911 seconds
[ INFO] [1624391235.275166648, 16.079000000]: SimpleSetup: Path simplification took 0.140280 seconds and changed from 5 to 2 states
[ INFO] [1624391235.290303685, 16.085000000]: Disabling trajectory recording
[ INFO] [1624391236.290471975, 16.464000000]: Didn’t receive robot state (joint angles) with recent timestamp within 1 seconds.
Check clock synchronization if your are running ROS across multiple machines!
[ WARN] [1624391236.290540103, 16.464000000]: Failed to validate trajectory: couldn’t receive full current joint state within 1s
[ INFO] [1624391236.312759822, 16.469000000]: ABORTED: Solution found but controller failed during execution

Drive / Joystick should be irrelevant to moveit you are correct on that.

I can’t reproduce this, but it seems to me either joint_states aren’t published correctly from Gazebo or there is something wrong with your clock. I have a few questions to better understand your problem:

  • Can you visualize the robot in Rviz?
  • What is your real time factor in gazebo (located in Left Bottom)?
  • Are you running ROS in multiple machines without clock sync?
  • Are you setting use_sim_time parameter to false in the launch file?

In a relevant note, if you are new to MoveIt! I recommend giving the official tutorials a look as well as Stretch

One thing may work is you can try removing <arg name="use_sim_time" value="$(arg use_sim_time)"/> from gazebo.launch file (should be on line 15).

Thanks for the help, and the tutorials–I’ll be sure to check them out! To answer your questions:

  • The movit rviz window visualizes fine, including the arm movements when I try to execute the plan. However, the rviz window produce by gazebo.launch with rviz:=true produces a broken robot:

  • The real time factor in Gazebo seems to fluctuate between 0.35-0.38 as the simulation is running.

  • I’m just using ROS on one machine

  • I didn’t set use_sim_time in the launch file, so it’s just using the default of “true”

I tried removing <arg name="use_sim_time" value="$(arg use_sim_time)"/> from gazebo.launch file, but it did not affect the simulation that I could tell.

Update: I solved the issue–joint_state_controller was not properly installed (I had thought it was, but I guess not).

Got that fixed, and now MoveIt! works, and the rviz window from gazebo.launch ino longer has a broken robot.

Thanks!

2 Likes

Thanks @lstegner, joint_state_controller is now an explicit dependency of stretch_gazebo. It’ll install automatically when using rosdep. I’ve also added a keyboard_teleop script!

1 Like