RVIZ does not show current state of the physical robot

[Rasmus Hasle Andersen]

Hi all,

I have encountered an issues where the moveit plugins for rviz does not show the current state of the robot.
The current state is published in the /joint_states topic and I can visualize this correctly using the regular RobotModel plugin in rviz.

Any suggestion of what the problem might be?

[Ioan Sucan]

Rasmus,

The state the rviz plugin shows is what is published as part of the planning scene by move_group. That usually means that info from /joint_states is processed by move_group and part of the planning scene, so you see the current state, but if move_group is not running, or you are not publishing scenes, or are not listening to the correct topic in the rviz plugin, the current state will not show.

Ioan

[Rasmus Hasle Andersen]

Thank you Ioan.
Then i just need to figure what has changed since the last time I used moveit!.

Regards Rasmus

[Rasmus Hasle Andersen]

I finally got around to investigate this issue, but I cannot seem to solve it.
For the record I'm using a Youbot with two arms. The joint states are published on /base/joint_states, /arm_1/joint_states and /arm_2/joint_states, but how do I make move_group listen to all of these?

I have tried to simply remap the three topics to /joint_states. That worked for me previously, but for some reason it does not work now.
I also tried to remap just one topic (ie. /arm_1/joint_states to /joint_states, but that did not work either. I then get the message: "The complete state of the robot is not yet known." which somehow tells me the MoveIt! is listening to the correct topic, and thereby should be configured correctly, right?

I do not where to look for possible errors, hence all suggestions are appreciated.

Kind regards
Rasmus

[Tobias Fromm]

Rasmus,

what I did for a similar issue (arm and gripper joint states to be
combined coming from separate nodes) may also work for you: Simply hack
yourself a small node which gets /base/joint_states, /arm_1/joint_states
and /arm_2/joint_states as inputs and outputs the combined /joint_states.
I don't know whether there's a cleaner solution, but it works fine. As
soon as /joint_states publishes all the states MoveIt listens for (hence
not popping up "The complete state of the robot is not yet known."
anymore), the robot status should be updated in RViz.

Cheers,
Tobi

--
Tobias Fromm
Robotics Group
Jacobs University

Research I, Room 38 +49-421-200-3106
Campus Ring 1 t.f...@jacobs-university.de
28759 Bremen, Germany http://robotics.jacobs-university.de

[Rasmus Hasle Andersen]

Hi Tobias,

I tried to remap both base and arm joint_states to the same topic, but without luck. The "The complete state of the robot is not yet known." message is not being shown when both topics are mapped, but is shown when I remap only of the topics.

Any other suggestions?

Thanks.

[Tobias Fromm]

I have no idea whether it works to remap both topic into /joint_states, that sounds rather like both messages are
published alternately. What I did is something like

sensor_msgs::JointState completeState;
for(unsigned int i = 0; i < armState->position.size(); ++i){
completeState.position.push_back(armState->position[i]);
completeState.name.push_back(armState->name[i]);
}
completeState.position.push_back(-gripperPosition);
completeState.position.push_back(gripperPosition);
completeState.name.push_back("gripper_left");
completeState.name.push_back("gripper_right");
completeState.header = armState->header;
joint_state_publisher.publish(completeState);

inside a joint state callback. Maybe that helps?

Cheers,
Tobi

[Martin Günther]

On Wed, 20 Nov 2013 14:12:08 +0100
Tobias Fromm <t.f...@jacobs-university.de> wrote:

> I have no idea whether it works to remap both topic
> into /joint_states, that sounds rather like both messages are
> published alternately. What I did is something like
>
> sensor_msgs::JointState completeState;
> for(unsigned int i = 0; i < armState->position.size(); ++i){
> completeState.position.push_back(armState->position[i]);
> completeState.name.push_back(armState->name[i]);
> }
> completeState.position.push_back(-gripperPosition);
> completeState.position.push_back(gripperPosition);
> completeState.name.push_back("gripper_left");
> completeState.name.push_back("gripper_right");
> completeState.header = armState->header;
> joint_state_publisher.publish(completeState);
>
> inside a joint state callback. Maybe that helps?

Hi all,

a long while ago, on Diamondback IIRC, some Willow nodes didn't like
it if you remapped different joint state publishers to
the /joint_state topic, because it indeed leads to the different
messages being published alternately, and the node in question assumed
that all messages contained the full joint state. But it doesn't sound
as if MoveIt had that bug, since the warning about "no full joint state
received" only appears when Rasmus doesn't remap all required topics
to /joint_states, which is correct.

Anyway, to work around that problem, I created a "joint_state_muxer"
that does exactly what Tobias is suggesting:

https://github.com/uos/uos_tools/tree/hydro/joint_state_muxer

You can include the following snippet into your launch files (replace
the list of joint state topics with your topics):

<node pkg="joint_state_muxer" type="joint_state_muxer" name="joint_state_muxer" output="screen">
<rosparam param="source_list">[kurtana_joint_states, katana_joint_states, wheel_joint_states]</rosparam>
</node>

On Hydro, you just need the Deb package ros-hydro-urdfdom-py. On groovy, you
need:

git clone -b groovy 'git://github.com/laas/robot_model_py.git' 'robot_model_py'

As I wrote before, this probably won't fix the problem, but it's
worth a try.

Cheers,
Martin

[Rasmus Hasle Andersen]

Hi Tobias and Martin (et al.)

Thank you for your help!

I have now modified the youbot driver to publish all joint_states to a single topic, which has fixed the problem.
As I mentioned I previously had MoveIt! running while I simply remapped all joint_state publishes to the same topic (/joint_states) and it worked. But somehow it doesn't any more.
Hence it seems that a recent update to MoveIt! has introduced the "Willow node syndrom" as Martin described.

I have installed from source using the 'hydro-devel' branch.

Again, thank you!

Regard Rasmus

[Evan Sonderegger]

Hi all,

Sorry to be reviving an old thread, but I'm stumped on this same issue.

I'm using a single arm YouBot on Ros-Hydro, with the drivers and controllers from the WPI-RAIL youbot_common package and a moveit configuration I made with the moveit setup assistant.

The state of the physical robot is getting published to joint_states and can be visualized properly in RVIZ by adding a RobotModel display. Also, moveit seems to be aware of the current state of the physical robot, because when I run the moveit_commander_cmdline.py tool and tell it to use the arm group, typing "current" will correctly output the physical state.

However, in RVIZ when I add a RobotState display, it just shows the initial state of the robot from the urdf. By default, the value for Robot State Topic is display_robot_state, a topic that doesn't appear to be published by anything. Is this topic necessary for the RVIZ moveit plugin to work? If so, how have people set up publishers for it?

Thank you in advance,

Evan Sonderegger

[Sachin Chitta]

Hi Evan,

The Rviz State Display was designed for displaying a single state of the robot, mostly for debugging purposes. You are supposed to provide the entire state to this plugin on the display_robot_state topic - e.g. if you had an ik pose for reaching something, you could provide the whole robot state to this plugin and display your ik pose.

You do not need this plugin to work with MoveIt! - you only need the MoveIt! Rviz Motion Planning Plugin.

Sachin

[Evan Sonderegger]

Hi Sachin,

Thank you for your reply. My mistake was that I was starting Rviz manually via "rosrun rviz rviz" instead of via the launch files in the MoveIt config. As a result, I was monitoring the wrong topic and trying to figure out how to create something that would publish to display_robot_state. As soon as I figured out the MoveIt was already publishing on /move_group/monitored_planning_scene, I was able to create and execute plans from the Rviz plugins.

Right now, for me, all three MoveIt Rviz plugins (RobotState, PlanningScene, and MotionPlanning) default to monitoring the "display_robot_state" topic. Do you think it would be a good idea to switch the default to be "/move_group/monitored_planning_scene"?

Thanks,

Evan