Suggestions on the joint_states topic when using a floating joint?

[Suh-Shin hwang]

Hello all,

When reading the concepts page (http://moveit.ros.org/documentation/concepts/) I noticed that I need to have a node that publishes on the joint_states publish.

By looking at the joint_states message, it seems to me that it is designed to handle only a single DOF. However, right now I am working on a quadrotor, and the most obvious choice of a joint relative to the quadrotor and the world is a floating (6DOF) joint. Now how would I publish this topic to allow MoveIt to know where the robot is? Does anyone have a suggestion regarding this issue?

Best,

Suh-Shin

[Michael Ferguson]

That is correct, joint_states is only for the single DOF joints. For floating joints, you need to broadcast a TF transform between the two links (likely something like "base_link" and "odom" or "world"). The other component here is robot_state_publisher, which converts joint_states + URDF into TF transforms.

-Fergs

[Suh-Shin hwang]

Hi Fergs,

Looking at the robot_state_publisher package (http://wiki.ros.org/robot_state_publisher), it also subscribes to messages of type JointState, which is also a single-DOF. In this case then, will both the robot_state_publisher and a joint_states be useless in this situation? If MoveIt does not receive messages from either of these topics, is it effectively oblivious about the robot pose in the world? (Hence causing my "replan" functionality to not work? Is this the case?)

I have written simple node that publishes the current pose state into TF, will MoveIt use this TF information? I see the quadrotor being updated in the RViz plugin as the quadrotor is moving in Gazebo.

Best,

Suh-Shin

[Rohan Bhargava]

I am having the same problem. I don't think MoveIt uses the TF information to update the state.  It specifically listens to /joint_state topic.

I have been using the move group api to set the start state based on the odometry topic. This doesn't help much with Rviz but it helps me in planning trajectories to the goal state from my current state

Cheers

Rohan

[Dave Hershberger]

I think you both are seeing a bug/behavior I've seen before.

This line in current_state_monitor:

https://github.com/ros-planning/moveit_ros/blob/hydro-devel/planning/planning_scene_monitor/src/current_state_monitor.cpp#L357

is where it asks TF for the location of the base relative to the world.  That line is inside the jointStateCallback() function, which means current_state_monitor (and thus move_group) only sees changes in the base pose when it gets an update about other joint values.

This is not usually a problem when running on a real robot with an arm, because there is a robot state publisher node of some sort publishing the joint values at a high rate, like 100Hz.

For a quad-copter with no active joints other than the floating virtual joint, it would definitely be a problem.

A quick workaround is to make a node that publishes an empty sensor_msgs/JointState message at some high rate.

A slower fix is to change current_state_monitor.cpp to monitor TF independently from joint values.

Dave

[Suh-Shin hwang]

Hi Dave,

Right now I'm using a modified version of the demo.launch that was generated with the setup assistant. Within this launch file, I see that there is already a joint_state_publisher and also a robot_state_publisher. Should I assume that these two nodes are already doing the job of publishing the messages (they're empty now) thus causing the robot_state_ to be reading the TF?

I did a trial with the launch file without the joint_state_publisher nor the robot_state_publisher. I first launched the simulation and then had the quadrotor take off and hover. I then launched the .launch file and I noticed in RViz that the quadrotor is in the origin (0, 0, 0) instead of in the air. Once I manually executed rosrun joint_state_publisher joint_state_publisher, the quadrotor within RViz pops into a pose matching that of the simulator. I also ran a test where I ran some commands to move the quadrotor in the simulator, and RViz updates accordingly.

If this is happening, does this mean that the jointStateCallback() function is repeatedly being called and hence updating  MoveIt's knowledge of the robot?

I also did a rostopic echo /joint_states command prior to creating a node which publishes empty Joint_States messages at 100 Hz and compared the output to the rostopic echo after I launched the node and I saw no difference.

Also a bit of a side question: does MoveIt knowing the joint_states have anything to do with the replanning capabilities? I'm having trouble allowing MoveIt! to replan.

[Dave Hershberger]

Suh-Shin, it sounds like you are already doing the right thing with respect to the problem I mentioned.

I don't know enough about the replanning code to say.  I would think it would be related.

Dave

[Ankit Vora]

How did you guys eventually solve the issue ? I'm having the exact same problem where my quadrotor has to publish /joint_states for the move_group to execute trajectories.

Let me know if you have a solution.

[Ridhwan Luthra]

I had the same problem, what I find is useful for drones is just use moveit for motion planning and you get the waypoints in /move_group/display_planned_path. Subscribe to those in a node and build your own way of moving the drone in whatever way you want.