[FetchIt!] [BUG] RobotStatePublisher for Schunk machine breaks robot's /odom transform

[ben-fido]

Description: You have defined the Schunk machine as a second robot with its own robot_state_publisher. However, this is not configured correctly and it breaks the TF tree for the Fetch robot's /base_link to /odom transform.

I'm not sure why this was working previously, but it should not have been.

Steps to reproduce: Launch the simulation environment:

$ roslaunch fetchit_challenge main_arena_montreal2019_highlights.launch

Check this TF transform:

$ rosrun tf tf_echo /base_link /odom

You should see:

At time 0.0
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

where the timestamp is stuck at zero.
Also, if you generate the diagram of TF frames, you can see that the Schunk machine is connected directly to the robot's /base_link, which is incorrect.

Then, remove the robot_state_publisher in shunk_machine_control.launch and repeat.
You should see the correct output:

At time 9.537
- Translation: [0.000, 0.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, 0.000, 1.000]
            in RPY (radian) [0.000, -0.000, 0.000]
            in RPY (degree) [0.000, -0.000, 0.000]

where the timestamp is changing.

Screenshot: This shows the TF tree with bad connection:

Expected behavior: Each robot can have a /base_link but the TF frames need to be namespaced, like:

/fetch_robot1/base_link/
/schunk_machine/base_link/

and then the two robots are connected in the TF tree via the /map frame.

See the example in the first image on this page:
https://answers.ros.org/question/258948/why-tf-trees-for-multi-robot-system-not-showing/

Or you could try a hard-coded approach and write a simple node that publishes a static TF with the Schunk's location.

System configuration:

• fetch_gazebo version: Latest master branch
• Ubuntu version: 18.04
• ROS version: Melodic

.

[moriarty]

@RDaneelOlivav it's probably best to write a simple node that publishes the TF for the Schunk machine location

[ben-fido]

Fixed in PR https://github.com/fetchrobotics/fetch_gazebo/pull/88

I followed this guide:
http://www.theconstructsim.com/ros-qa-130-how-to-launch-multiple-robots-in-gazebo-simulator/
and put schunk robot in its own namespace.
But the fetch robot still has no namespace, to keep things simple and not break anyones codes.

ROS Nodes:

/robot_state_publisher                 <-- Fetch robot
/schunk_machine/robot_state_publisher

ROS Params:

/robot_description
/schunk_machine/robot_description
/schunk_machine/lathe_joint_velocity_controller/pid/p
/schunk_machine/lathe_joint_velocity_controller/pid/i
/schunk_machine/lathe_joint_velocity_controller/pid/d

ROS Topics:

/joint_states
/schunk_machine/joint_states
/schunk_machine/lathe_joint_velocity_controller/command

You can make the spindle rotate:

rostopic pub -1 /schunk_machine/lathe_joint_velocity_controller/command std_msgs/Float64 '{data: 1.0}'

The jaws won't open until we modify the Collada model of that part.

.

[moriarty]

It doesn't rotate, it only opens and closes the jaws.

Submit the PR and GitHub should automatically assign @RDaneelOlivav to do the code review https://github.com/fetchrobotics/fetch_gazebo/blob/gazebo9/.github/CODEOWNERS after that we'll merge it

[RDaneelOlivav]

Let me check ASAP

[RDaneelOlivav]

Ok Just to define the functionality of the SHUNK machine should be to open and close the gripper of the SHUNK machine when used the action. And then what? Does it have to be able to retain the part? Or its just a fake dummy exercise? Could someone define exactly what the SHUNK machine will do in the competition exactly? @moriarty

[moriarty]

The Schunk Machine should open and close the gripper of the Schunk machine. The large gear, is transferred from the robot, to the Schunk machine and then back to the robot.

The Schunk Gripper, is a Schunk pneumatic chuck, so sometimes we call it a chuck and sometimes we call it a gripper.

Here is one possible time line:

1. Team calls open on Schunk Machine Action to ensure the gripper is open
2. Team grasps large gear
3. Team places large gear in chuck (Schunk gripper) of machine
4. Team calls action to close Schunk gripper the machine retains the part
5. Team slides door close (for bonus points)
6. Team grasp small gear and places it in kit (because there is a time delay before the schunk action can open the gripper- simulating it doing something)
7. Team opens the door (for bonus points)
8. Team grasps the large gear from the machine
9. Team calls the action to open the gripper the machine releases the part
10. Team places large gear in the kit

[RDaneelOlivav]

Ok I've pushed an update of the chunk machine ( https://github.com/fetchrobotics/fetch_gazebo/pull/89), with new updated action server decoupled from simulation or real thing. Also updated the model, colours, and added chuck open and close system, with integration with the action server. Now if you spawn the Shunk machine it should just be there operational. To use it just publish the goal in the goal topic.

[ben-fido]

This thread can be closed.
The thread about the machine is issue https://github.com/fetchrobotics/fetch_gazebo/issues/87