How to connect the real robot's joint states to the current state of MoveIt motion planning?
After completing the MoveIt configuration of my custom robot with moveit_setup_assistant
, I modified the source_list of joint_state_publisher
in demo.launch
to synchronize the joint state of the actual robot to joint_state_publisher
.
<launch>
<!-- specify the planning pipeline -->
<arg name="pipeline" default="ompl" />
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- Allow user to specify database location -->
<arg name="db_path" default="$(find th6_moveit_config)/default_warehouse_mongo_db" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- By default, we will load or override the robot_description -->
<arg name="load_robot_description" default="true"/>
<!-- Set execution mode for fake execution controllers -->
<arg name="execution_type" default="interpolate" />
<!--
By default, hide joint_state_publisher's GUI
MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
The latter one maintains and publishes the current joint configuration of the simulated robot.
It also provides a GUI to move the simulated robot around "manually".
This corresponds to moving around the real robot without the use of MoveIt.
-->
<arg name="use_gui" default="false" />
<arg name="use_rviz" default="true" />
<!-- If needed, broadcast static tf for robot root -->
<!-- We do not have a robot connected, so publish fake joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
<rosparam param="source_list">[th6_joint_state]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find th6_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="execution_type" value="$(arg execution_type)"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="pipeline" value="$(arg pipeline)"/>
<arg name="load_robot_description" value="$(arg load_robot_description)"/>
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(find th6_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
<arg name="rviz_config" value="$(find th6_moveit_config)/launch/moveit.rviz"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(find th6_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
<arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
</include>
</launch>
The picture above is the screen after demo.launch
is launched. The model marked with 1 shows the pose of the actual robot read by joint_state_pubsliher
from my custom location update node. But when I use the MotionPlanning
plugin to initiate a motion planning request, the current state of MoveIt has been maintained at the origin (model marked with 2), not the pose of the actual robot. How can I synchronize the pose of the actual robot with ...