fix iss470

[OmkarKabadagi5823]

Description

Fixes Issue: #470

Issue:

1. Robot loads in an invalid state.
2. Error stating unable to plan goal

Changes made:
motion_planning_pipeline_tutorial.cpp:

1. Set Robot State to ready state. (previously in collision)
   planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
2. Remove re-declarations of robot_state and joint_model_group.

motion_planning_pipeline_tutorial.launch:

1. Fix virtual_joint_broadcaster_0 args
2. Add /source_list param to joint_state_publisher
3. Include move_group.launch
4. Add a relay node to remap /joint_states to /joint_states_desired (later topic required by move_group node)

How changes fix the issue:
Setting the robot to ready state brings the joint configurations to a valid state. Though this fixes the state and planning issue, this still does not fix the visual of robot in RViz as moveit is not subscribing to /joint_states. The changes in launch file fixes this issue.

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[JStech]

I built this branch and melodic/devel (for comparison) and ran this tutorial. This branch prevents the "Unable to find a valid state nearby the start state" warning, but in both cases the tutorial immediately fails with this:

Waiting to continue: Press 'next' in the RvizVisualToolsGui window to start the demo... continuing
[ERROR] [1624118871.611841295]: Found empty JointState message
[ERROR] [1624118871.611883870]: Found empty JointState message
[ERROR] [1624118871.612070890]: Found empty JointState message
[motion_planning_pipeline_tutorial-8] process has died [pid 26584, exit code -11, cmd /home/john/ws_moveit_tutorials/devel/lib/moveit_tutorials/motion_planning_pipeline_tutorial __name:=motion_planning_pipeline_tutorial __log:=/home/john/.ros/log/634a98e2-d118-11eb-b9b0-94e6f7abd3d0/motion_planning_pipeline_tutorial-8.log].
log file: /home/john/.ros/log/634a98e2-d118-11eb-b9b0-94e6f7abd3d0/motion_planning_pipeline_tutorial-8*.log

Maybe I'm missing something? I'm on melodic-devel on MoveIt also. I'm just launching the tutorial with roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch and then clicking "Next"

[OmkarKabadagi5823]

I don't remember facing any such issue. But I'll look into it once I am done with my examinations.

[JStech]

I tried MoveIt 1.0.8 and 1.0.7. The "Found empty JointState message" error gets printed in both, but 1.0.7 doesn't segfault. I also verified that the bug occurs with moveit_tutorials on melodic_devel using MoveIt 1.0.7, but doesn't with this fix.

I don't know what's going on (gonna investigate some), but it isn't a problem with this PR.

Update: I think the segfault was just a bad mix of branches checked out in my workspace between moveit, moveit_msgs, and moveit_resources. I got everything on melodic_devel and it works fine.

[AndyZe]

I see the issue of the robot starting in a collision state. But, this PR doesn't fix it, for me.

Let's see if we can figure out how to set the robot state from launch file.

[AndyZe]

This is definitely a positive change 👍

[AndyZe]

I believe you can set the initial joint values with zeros tag, for joint_state_publisher. See http://wiki.ros.org/joint_state_publisher#Example_YAML_file_for_dependent_joints_and_zeros_parameters

[OmkarKabadagi5823]

This line should have removed the collosion confict planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

[AndyZe]

This line should have removed the collosion confict planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

It does fix the conflict in terms of allowing the tutorial to continue, but I still think it's confusing that the robot suddenly jumps to a good state.

Best to fix the initial joint angles via launch file, I think.