gazebosim / gz-sim

Open source robotics simulator. The latest version of Gazebo.
https://gazebosim.org
Apache License 2.0
708 stars 269 forks source link

JointPositionController topic name is not suitable for ROS2 topic name #447

Open chama1176 opened 4 years ago

chama1176 commented 4 years ago

JointPositionController topic name is "/model//joint///cmd_pos", however ROS2 topic name must not start with a numeric character. (https://design.ros2.org/articles/topic_and_service_names.html)

So topic name "/" leads to error when using the ros_ign_bridge.

chapulina commented 4 years ago

Oh interesting.

Does ROS at least let the user remap that topic to another valid one?

In any case, we should make the topic configurable for that system, so ROS users can set custom topics that match their requirements.

chama1176 commented 4 years ago

ROS remap does not work fine in this case.

In ROS2 Foxy,

ros2 run ros_ign_bridge parameter_bridge /model/robot/joint/joint_name/0/cmd_pos@std_msgs/msg/Float64@ignition.msgs.Double /model/robot/joint/joint_name/0/cmd_pos:=/test

errors:

NameError'
  what():  Invalid topic name: topic name token must not start with a number:
  '/model/robot/joint/joint_name/0/cmd_pos'

When remapping from launch file

<?xml version="1.0"?>
<launch>
  <node pkg="ros_ign_bridge" exec="parameter_bridge" args="/model/robot/joint/joint_name/0/cmd_pos@std_msgs/msg/Float64@ignition.msgs.Double" output="screen">
    <remap from="/model/robot/joint/joint_name/0/cmd_pos" to="/cmd_pos" />
  </node>
</launch>

also fails to parse global arguments.

drfknoble commented 3 years ago

I also came across this error this morning. I was trying to send joint commands from ROS 2 to Ignition Gazebo. I'll have a look at what I can do. If I come up with a solution, I will post here afterwards.

drfknoble commented 3 years ago

Okay, so I came up with a work around to get the plugin to work with ROS 2 via the ros_ign_bridge package's parameter_bridge exectuable. Here's a summary of what I did in case anyone else finds them self in need:

  1. Clone https://github.com/ignitionrobotics/ign-gazebo into ~/.

  2. Edit line 150 of ~/ign-gazebo/src/systems/joint_position_controller/JointPositionController.cc to:

    std::string topic = transport::TopicUtils::AsValidTopic("/model/" +
      this->dataPtr->model.Name(_ecm) + "/joint/" + this->dataPtr->jointName +
      "/_" + std::to_string(this->dataPtr->jointIndex) + "/cmd_pos");

    This will change the package's topic from /model/<name>/joint/<index>/cmd_pos to /model/<name>/joint/_<index>/cmd_pos, which will fix the "topic name" issue.

  3. Run the following commands:

    mkdir -p ~/ign_gazebo/build
    cd ~/ign_gazebo/build
    cmake ..
    cmake --build .
  4. Wait for CMake to complete (this step will take some time). When done, it will have created a lib sub-directory.

  5. Copy the following files from ~/ign_gazebo/build/lib to /usr/lib/x86_64-linux-gnu//usr/lib/x86_64-linux-gnu/ign-gazebo-4/plugins:

    • libignition-gazebo4-joint-position-controller-system.so
    • libignition-gazebo4-joint-position-controller-system.so.4
    • lib/libignition-gazebo4-joint-position-controller-system.so.4.2.0 This will overwrite the existing plugin files with the amended files. You may want to back up the original files (I created an old-plugins folder and copied the original files their before copying the new files over).
  6. Open a terminal. Run the following command:

    ign gazebo robot_world.sdf

    This will start Gazebo Ignition with your SDF world with a model that uses the libignition-gazebo-joint-position-controller-system.so plugin.

  7. Open a new terminal. Run the following command:

    ros2 run ros_ign_bridge parameter_bridge /model/robot_assembly/joint/joint_3/_0/cmd_pos@std_msgs/msg/Float64@ignition.msgs.Double

    This will start the bridge.

  8. Open a new terminal. Run the following command:

    ros2 topic pub /model/robot_assembly/joint/joint_3/_0/cmd_pos std_msgs/msg/Float64 '{data: -0.7804}'

    This will move the robot_assembly's joint_3 to -0.7804 radians.

Here's a before and after picture: image image
image Figure: (1) The commands typed into each terminal; (2) Ignition Gazebo before command; and (3) Ignition Gazebo after command.

I hope this helps anyone else wanting to use the JointController plugin with ROS 2 via ros_ign_bridge.

chapulina commented 3 years ago

Thank you for sharing the workaround, @DrFKNoble .

I opened a PR making the topic configurable for that plugin: https://github.com/ignitionrobotics/ign-gazebo/pull/584

This way, ROS users can specify the topic through the SDF file and don't need to change the source code.

drfknoble commented 3 years ago

@chapulina: No worries, happy to help! I think being able to specify the topic in the SDF will make things a lot easier.

ikim80 commented 3 years ago

Hello Frazer Noble ign gazebo robot_world.sdf

robot_world.sdf Please share the file. IGN Control Set Not Easy https://blog.naver.com/mdc1020/222481409951 Please.....

mdc1020@naver.com

rrrrkkkk commented 9 months ago

I can move the position of https://github.com/gazebosim/gz-sim/blob/ign-gazebo5/examples/worlds/joint_position_controller.sdf with ign topic -t "/rotor_cmd" -m ignition.msgs.Double -p "data: -1.0" command.

But I can't do this with ROS2. I tried the command below. ros2 run ros_ign_bridge parameter_bridge /joint_position_controller_demo/j1/rotor_cmd@std_msgs/msg/Float64@ignition.msgs.Double ros2 topic pub /joint_position_controller_demo/j1/rotor_cmd std_msgs/msg/Float64 '{data: 1.0}' --once

What is wrong?