Real platforms pose from simulation

[manfer33]

Hi, during the simulation I do not find any topic with information about the real pose of UAV, USV or vessels in the world. The only topic related (/quadrotor_1/pose) just shows each rotor position respect to the airframe.

Can it be a problem of my compilation?

Best regards

[iche033]

hi, yes that's expected behavior. The simulator does not publish any ground truth information of the vehicles in simulation via the provided ROS2 topics. For local development and testing, you can change the publish_model_pose param to true to get ground truth: ~https://github.com/osrf/mbzirc/blob/main/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb#L218~ https://github.com/osrf/mbzirc/blob/v4/mbzirc_ign/models/mbzirc_quadrotor/model.sdf.erb#L249

[manfer33]

Thank you @iche033, I found the same parameter for the USV model. Can we get the ground truth information of the vessels spawned too?

[iche033]

It's more involved to get the ground truth data from the vessels since the sim currently does not publish any.

You'll need to first add the pose-publisher system to the vessel model, then create a bridge to bridge the pose data to the ros2 side.

Here is a example patch from a quick test off of main:

patch

```diff diff --git a/mbzirc_ign/worlds/simple_demo.sdf b/mbzirc_ign/worlds/simple_demo.sdf index 20c7edd..ba52ccd 100644 --- a/mbzirc_ign/worlds/simple_demo.sdf +++ b/mbzirc_ign/worlds/simple_demo.sdf @@ -184,9 +184,23 @@ - Vessel A + Vessel_A 25 25 0.3 0 0.0 -1.0 https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Vessel A + + + true + true + false + false + true + true + true + true + 1 + + diff --git a/mbzirc_ros/launch/competition.launch.py b/mbzirc_ros/launch/competition.launch.py index c9194cf..83b88f8 100644 --- a/mbzirc_ros/launch/competition.launch.py +++ b/mbzirc_ros/launch/competition.launch.py @@ -25,6 +25,8 @@ from launch_ros.actions import Node import os +import mbzirc_ign.bridges + def launch(context, *args, **kwargs): ign_args = LaunchConfiguration('ign_args').perform(context) @@ -56,10 +58,25 @@ def launch(context, *args, **kwargs): arguments=['/mbzirc/phase@std_msgs/msg/String@ignition.msgs.StringMsg'], ) + vessel_a_bridges = [ + mbzirc_ign.bridges.pose('Vessel_A'), + mbzirc_ign.bridges.pose_static('Vessel_A'), + ] + ros2_ign_vessel_a_bridge = Node( + package='ros_ign_bridge', + executable='parameter_bridge', + output='screen', + namespace='vessel_a', + arguments=[bridge.argument() for bridge in vessel_a_bridges], + remappings=[bridge.remapping() for bridge in vessel_a_bridges], + ) + return [ign_gazebo, ros2_ign_score_bridge, ros2_ign_run_clock_bridge, - ros2_ign_phase_bridge] + ros2_ign_phase_bridge, + ros2_ign_vessel_a_bridge + ] def generate_launch_description(): return LaunchDescription([ ``` Then launch by running: ``` ros2 launch mbzirc_ros competition.launch.py ign_args:="-v 4 -r simple_demo.sdf" ```

[swadhinagrawal]

In case of the Oberon7 arm that is provided to us, there is only relative pose that is available and the base link of the arm shows null values for positions of the base of the arm. So when we mount the arm on the usv, is there any possibility of getting the actual ground truths?

[iche033]

are you looking for the ground truth of the arm's base in world frame? If so, it's always be at a fixed position w.r.t the USV: https://github.com/osrf/mbzirc/blob/v4/mbzirc_ign/models/mbzirc_usv_base/model.sdf#L607

Since you can get the ground truth of the USV (by setting publish_model_pose to true), you should be able to transform the arm to the world frame.

As for pose of other links in the arm, you'll need to compute them by looking at the joint states published in /usv/arm/joint_states

[holaros]

Hi @iche033 @manfer33 I am using the simulator for research and if I want the USV ground truth, how do I get it? I have set the publish_model_pose to true. Which topic should I subscribe to? I don't see any topic that is publishing the ground truth of the USV. I can see the groundtruth of each entity like the left_engine_link, right_propeller_link etc. but I cannot find for the whole USV together. Is there no way I can get that?

This is the output from ros2 topic list

/clock
/mbzirc/phase
/mbzirc/run_clock
/mbzirc/score
/mbzirc/target/stream/status
/parameter_events
/rosout
/usv/imu/data
/usv/left/thrust/cmd_thrust
/usv/left/thrust/joint/cmd_pos
/usv/pose
/usv/pose_static
/usv/right/thrust/cmd_thrust
/usv/right/thrust/joint/cmd_pos
/usv/rx

I am essentially trying to make the USV go from one waypoint to the other because of which I need the pose. Any help will be much appreciated. Thanks!

[iche033]

After you made the change, make sure to build and install the workspace again. It should come through the /usv/pose topic. Do you also see a /tf topic? The ground truth should also be published there as well.

[holaros]

Hi @iche033 , thank you for your reply. So, I built and installed the workspace using: IGNITION_VERSION=fortress colcon build --merge-install and then source install/setup.zsh (is there anything else that needs to be done?)

And then, when I run the simulator and spawn the usv using

ros2 launch mbzirc_ros competition_local.launch.py ign_args:="-v 4 -r coast.sdf"                                

ros2 launch mbzirc_ign spawn.launch.py name:=usv world:=coast model:=usv x:=-1462 y:=-16.5 z:=0.3 R:=0 P:=0 Y:=0

it spawns the usv. and then when I do ros2 topic echo /usv/pose I get a stream of messages, copy pasted below is one part of it:

---
transforms:
- header:
    stamp:
      sec: 2
      nanosec: 248000000
    frame_id: usv
  child_frame_id: usv/right_propeller_link
  transform:
    translation:
      x: -0.1510000000000673
      y: -1.3480001120762672
      z: -0.7159999999999993
    rotation:
      x: 1.3231858595218996e-14
      y: -1.3231659896302898e-14
      z: 0.7071048067408525
      w: 0.7071087556267289
- header:
    stamp:
      sec: 2
      nanosec: 248000000
    frame_id: usv
  child_frame_id: usv/right_engine_link
  transform:
    translation:
      x: -2.2737367544323206e-13
      y: -1.3480000000000025
      z: -0.5459999999999994
    rotation:
      x: 2.445817974954449e-19
      y: -6.728328461536955e-20
      z: 0.7071048067408527
      w: 0.707108755626729
- header:
    stamp:
      sec: 2
      nanosec: 248000000
    frame_id: usv
  child_frame_id: usv/left_propeller_link
  transform:
    translation:
      x: -0.1510000000000673
      y: 1.3479998879237325
      z: -0.7160000000000011
    rotation:
      x: 1.3231424914143206e-14
      y: -1.3232093577999073e-14
      z: 0.7071048067408524
      w: 0.707108755626729
- header:
    stamp:
      sec: 2
      nanosec: 248000000
    frame_id: usv
  child_frame_id: usv/left_engine_link
  transform:
    translation:
      x: -2.2737367544323206e-13
      y: 1.347999999999999
      z: -0.5460000000000012
    rotation:
      x: 2.4458138390513864e-19
      y: 3.6639758437883223e-19
      z: 0.7071048067408527
      w: 0.707108755626729
---

I dont see the groundtruth pose in this. Am I doing something wrong?

Your help will be much appreciated. Thanks!

[holaros]

Hi @iche033 , so I tried on a different device altogether with the simulation setup from scratch. The issue is still persistent. Essentially, I don't see the groundtruth pose to subscribe to.

[iche033]

can you try commenting these lines out, rebuild, and run again? https://github.com/osrf/mbzirc/blob/ec66f35c1fc925f18565c03251bd5f12f5b6b4e3/mbzirc_ign/src/mbzirc_ign/launch.py#L139-L141

see if you now get the ground truth in either the /model/usv/pose or /model/usv/pose_static topic

[holaros]

@iche033 so, when I try this, I can see the child_frame_id: usv on the usv/pose_static topic. But this is published at a much lower rate right? is there a way I can increase this (so that I can use this to go from one waypoint to the other)?

[iche033]

the pose_static topic is published at 1 hz, defined here: https://github.com/osrf/mbzirc/blob/ec66f35c1fc925f18565c03251bd5f12f5b6b4e3/mbzirc_ign/models/usv/model.sdf.erb#L361

I think you should be able to bump up that value to the rate you want.