Draft: adding namespaces to create3

hugo-tardiou commented 2 years ago

Description

Hi, A colleague of mine contacted the turtlebot4 repository to see if it was possible to do multi robot simulation with it. From what @roni-kreinin said it is not possible yet. turtlebot4 issue We started working on it, starting from the create3, and we're hoping somebody can give us some help as we are running into issues.

Type of change

[ ] Bug fix (non-breaking change which fixes an issue)
[ ] New feature (non-breaking change which adds functionality)

How Has This Been Tested?

We modified files to make every namespace generic and this works if you spawn 1 robot including namespace cmd_vel, but as soon as you try to spawn a 2nd one it segment faults at launch

z1g4kEQ

It's difficult to find best practices for multi-robot simulation, so we're hoping the way we started is good. It has only been tested for ignition, I can test for Classic once it works, as well as verify it doesn't break anything else.

# Run this command
ros2 launch irobot_create_ignition_bringup create3_multi.launch.py

Checklist

[ ] I have performed a self-review of my own code
[ ] I have commented my code, particularly in hard-to-understand areas
[ ] I have made corresponding changes to the documentation

alsora commented 2 years ago

I opened a PR targeting your branch with a couple of fixes https://github.com/hugo-tardiou/create3_sim/pull/1/files For some reason, my rviz window does not show the robot model and it complains about missing connections in the tf tree

hugo-tardiou commented 2 years ago

Thanks, i merged it, we'll investigate why but we did see those.

Another issue we saw is on the teleop GUI the /namespace/cmd_vel does not publish anything to the specific topic, so it does not work on namespace'd robots. Might be an error on the plugin? Works fine if using the default empty namespace

hugo-tardiou commented 2 years ago

@alsora I looked into it, I believe you are speaking in the case where the namespace is not empty?

Setting the namespace before /robot_description as below shows it properly in the rviz. Should a modification be done for the rviz file? We think no, because I don't think there is a dynamic way to modify the .rviz file for this.

I do not see any error in the tf_tree can you show more details?

( I believe the wheel transform errors were present before this PR? )

Screenshot from 2022-05-04 17-45-32

alsora commented 2 years ago

Yes, the wheels are a known issue. I will have to investigate my issue or do a clean build, but it;s unrelated to these changes.

Looking at the navigation stack rviz launch file, they are doing some replacement/substitutions when working with a namespace https://github.com/ros-planning/navigation2/blob/main/nav2_bringup/launch/rviz_launch.py#L63-L78

hugo-tardiou commented 2 years ago

Oh very cute, we'll implement that.

hugo-tardiou commented 2 years ago

@alsora in the latest commits:

We fixed the rviz files as discussed above
We namespace'd the dock and the cmd_vel in the gui
We (re)added the namespaces /** to the common_bringup config file as it seems to be the only way to make sure the topics inside of it are namespace properly, as well as a ReplaceString ( this function comes from nav2_common, we can move/import the function since its from from a different project ) Without this change, several topics were still namespace-less which is not ideal, I hope how we did it is ok with you

Edit:

Another issue I saw is replacing the string of a /topic to /namespace/topic, in a namespace-less scenario will give //topic which is why I believe nav2 adds <robot_namespace> in their rviz file. This is an issue for us in the /config/*.yaml files

alsora commented 2 years ago

Hi, sorry if we still haven't been able to help with this PR. Anyhow, I just noticed this bug & its fix in rclcpp https://github.com/ros2/rclcpp/pull/1839 This may explain the problems we had setting the namespace and the parameters.

alsora commented 2 years ago

@lchico these packages shouldn't be needed. The simulator should not have any dependency on the navigation stack.

I didn't notice this dependency, where do you see it coming from?

lchico commented 2 years ago

nav2

@alsora, When I tried to run:

ros2 launch irobot_create_gazebo_bringup create3_gazebo.launch.py

I got:

[INFO] [launch]: Default logging verbosity is set to INFO
Task exception was never retrieved
future: <Task finished name='Task-2' coro=<LaunchService._process_one_event() done, defined at /opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py:226> exception=ModuleNotFoundError("No module named 'nav2_common'")>
Traceback (most recent call last):
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 228, in _process_one_event
    await self.__process_event(next_event)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_service.py", line 248, in __process_event
    visit_all_entities_and_collect_futures(entity, self.__context))
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 45, in visit_all_entities_and_collect_futures
    futures_to_return += visit_all_entities_and_collect_futures(sub_entity, context)
  [Previous line repeated 3 more times]
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/utilities/visit_all_entities_and_collect_futures_impl.py", line 38, in visit_all_entities_and_collect_futures
    sub_entities = entity.visit(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/action.py", line 108, in visit
    return self.execute(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/actions/include_launch_description.py", line 127, in execute
    launch_description = self.__launch_description_source.get_launch_description(context)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_description_source.py", line 84, in get_launch_description
    self._get_launch_description(self.__expanded_location)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_description_sources/python_launch_description_source.py", line 51, in _get_launch_description
    return get_launch_description_from_python_launch_file(location)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_description_sources/python_launch_file_utilities.py", line 62, in get_launch_description_from_python_launch_file
    launch_file_module = load_python_launch_file_as_module(python_launch_file_path)
  File "/opt/ros/galactic/lib/python3.8/site-packages/launch/launch_description_sources/python_launch_file_utilities.py", line 37, in load_python_launch_file_as_module
    loader.exec_module(mod)
  File "<frozen importlib._bootstrap_external>", line 848, in exec_module
  File "<frozen importlib._bootstrap>", line 219, in _call_with_frames_removed
  File "/catkin_ws/install/irobot_create_common_bringup/share/irobot_create_common_bringup/launch/create3_nodes.launch.py", line 13, in <module>
    from nav2_common.launch import ReplaceString
ModuleNotFoundError: No module named 'nav2_common'

After install this:

sudo apt install ros-galactic-navigation2

I don`t have that error.

lchico commented 2 years ago

The namespace looks like it's working:

ros2 topic list 
/clock
/joint_states
/parameter_events
/robot1/battery_state
/robot1/clicked_point
/robot1/cmd_audio
/robot1/cmd_lightring
/robot1/cmd_vel
/robot1/diffdrive_controller/cmd_vel_unstamped
/robot1/dock
/robot1/dynamic_joint_states
/robot1/goal_pose
/robot1/hazard_detection
/robot1/initialpose
/robot1/interface_buttons
/robot1/ir_intensity
/robot1/joint_states
/robot1/kidnap_status
/robot1/odom
/robot1/robot_description
/robot1/sim_ground_truth_dock_pose
/robot1/sim_ground_truth_pose
/robot1/slip_status
/robot1/standard_dock_description
/robot1/stop_status
/robot1/wheel_status
/robot1/wheel_ticks
/robot1/wheel_vels
/rosout
/standard_dock_description
/tf
/tf_static

alsora commented 2 years ago

Oh ok, well this irobot_create_common_bringup/launch/create3_nodes.launch.py line 13 should be modified to not depend on the nav stack. We should copy the utility in our scripts.

alsora commented 2 years ago

Ok, now I'm reading again the comments I see that the latest changes make it work, but we had to use /**: in the node names in the various params.yaml files.

This requires that we keep running every node in a separate process otherwise their parameters will collide. But I think that at this point we can proceed with the PR assuming that we remove the dependency on the nav2 code

hugo-tardiou commented 2 years ago

We started working on this PR a bit as we got some new leads on how to fix our issue.

As for the nav2 utility yes we can copy it. Can we just copy the whole file? link. Not sure how the open-source aspect works. We might also need to rebase this PR to the latest branch on main at some point. We needed some fixes that were not present during this PR's creation.

alsora commented 2 years ago

Yes, you can copy the file. Please maintain the original copyright notice and add a comment saying

# This file has been copied from https://github.com/ros-planning/navigation2 for use as part of the iRobot Create 3 simulator.

rjcausarano commented 2 years ago

@lchico mentioned that sudo apt install ros-galactic-navigation2 is needed to make it work. Was this addressed? @alsora @hugo-tardiou

rjcausarano commented 2 years ago

@hugo-tardiou @alsora Are we going to fix the failing test? or is that out of scope?

alsora commented 2 years ago

Yes the nav2 utility must be copied here before being able to merge this PR (I really don't want this package to have a dependency on the whole navigation stack just because of a python launch file utility).

Similarly, also linter tests should be fixed. @hugo-tardiou you should be able to check them locally using colcon test

hugo-tardiou commented 2 years ago

@rjcausarano Yes sorry, we cannot advance on this PR until the issue has been resolved in the gz_ros_control repo Link related to the ignition plugin not handling multiple calls for each spawn correctly.

I'll try to clean navigation2 issue, lint and probably rebase on the latest commit this branch when I can.

But this PR is stuck until the gz plugin has been fixed, so it has not been a priority for us yet

hugo-tardiou commented 2 years ago

@alsora @rjcausarano We updated the code with mostly these diffs:

Rebase for the latest changes on main
No longer needs the ReplaceString from nav2 package
Lint
We copied how nav2 handled namespaces Nodes, mostly visible in create3_node.launch.py, therefore we have two nodes where only one is called if there is a namespace present or not. @alsora can you please confirm this doesn't bother you.

We're still working on the ignition plugin to fix the segfault in gz_ros2_control caused when calling a second robot for ignition.

alsora commented 2 years ago

That looks ok. However, I wonder if an alternative (and easier to maintain) solution is possible.

Right now the parameter files include topic names such as /my_topic. IMO, if we remove the / from the topic name in the yaml file, then we don't need to replace that.

A topic name such as my_topic (without the /) should automatically inherit the namespace of the node.

iRobotEducation / create3_sim