move_group Failed to validate trajectory after launching second simulation

asked 2019-05-16 03:58:08 -0600

kump gravatar image

updated 2019-05-17 02:20:56 -0600

I am launching two simulations, each on it's own gazebo master and each of those on their own ROS master. The ROS masters are connected with Multimaster-FKIE package where the /gazebo and /gazebo_gui topics are set private. There are two robots in the first simulations, one of them is a robotic arm and uses MoveIt! for movement planning. Each robot exists in it's own namespace.

When I launch the first simulation, the MoveIt! can control the robotic arm in the first simulation. As soon, as I launch the second simulation (on different ROS master), I start getting warnings in the terminal of the first simulation (shown below) and MoveIt is no longer able to control the robotic arm.

image description

This is the error I get from the terminal in which I launch the first simulation:

[ WARN] [1557994112.124412241, 1892.202000000] [file:/tmp/binarydeb/ros-kinetic-moveit-ros-planning-0.9.15/trajectory_execution_manager/src/trajectory_execution_manager.cpp] [node:/panda/move_group] [TrajectoryExecutionManager::validate:955]: Failed to validate trajectory: couldn't receive full current joint state within 1s
[ INFO] [1557994112.124463958, 1892.202000000] [file:/tmp/binarydeb/ros-kinetic-moveit-ros-move-group-0.9.15/src/default_capabilities/execute_trajectory_action_capability.cpp] [node:/panda/move_group] [MoveGroupExecuteTrajectoryAction::executePath:118]: Execution completed: ABORTED
[ WARN] [1557994272.231470399, 2034.540000000] [file:/tmp/binarydeb/ros-kinetic-robot-state-publisher-1.13.6/src/joint_state_listener.cpp] [node:/panda/robot_state_publisher] [JointStateListener::callbackJointState:118]: Received JointState is 1267.643000 seconds old.
[ WARN] [1557994283.482286155, 2044.557000000] [file:/tmp/binarydeb/ros-kinetic-robot-state-publisher-1.13.6/src/joint_state_listener.cpp] [node:/panda/robot_state_publisher] [JointStateListener::callbackJointState:118]: Received JointState is 1266.500000 seconds old.
[ WARN] [1557994294.756474698, 2054.559000000] [file:/tmp/binarydeb/ros-kinetic-robot-state-publisher-1.13.6/src/joint_state_listener.cpp] [node:/panda/robot_state_publisher] [JointStateListener::callbackJointState:118]: Received JointState is 1265.322000 seconds old.

This is an output I get on terminal where I launch the MoveIt! script:

[ INFO] [1557994112.124960255, 608.002000000] [file:/tmp/binarydeb/ros-kinetic-moveit-ros-planning-interface-0.9.15/move_group_interface/src/move_group_interface.cpp] [node:/move_group_commander_wrappers_1557994109441851035] [planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroupInterface::MoveGroupInterfaceImpl::execute:930]: ABORTED: Solution found but controller failed during execution

Before I launch the second simulation, the script finishes successfully and the arm executes the trajectory. I don't understand how can the second simulation have any effect on the moveit controller of the arm in the first simulation. Why is this happening and how can I fix this?


EDIT 1

I think what is happening is, that the two simulations have different time and that is what causes the problem. But when I reset the time in both simulations at the same time to synchronize them the control algorithms seems to stop working. How do people tackle this issue of ROS?

edit retag flag offensive close merge delete

Comments

Any updates yet?? I am facing similar issues

Aash gravatar image Aash  ( 2020-08-05 08:44:19 -0600 )edit

I stopped using the MoveIt! a long time ago, so I do not remember if this particular problem was solved, but what I did with the two simulations running at the same time was implement a LockStep Synchronization plugin that would not let the simulation times of the two simulations to differ more than 1 stepsize (1 ms in my case).

kump gravatar image kump  ( 2020-08-06 01:37:33 -0600 )edit

Also, making the /clock, /gazebo* and /tf* topics private to each ROS master is necessary. Otherwise the simulations get confused and throws lots of warnings. Maybe that was the main solution to the problem. I was working on this a long time ago. Sorry I cannot recall in details.

kump gravatar image kump  ( 2020-08-06 01:49:39 -0600 )edit