Skip to content

Commit

Permalink
allow converting from/to PoseWithCovarianceStamped (gazebosim#381)
Browse files Browse the repository at this point in the history 
* allow converting from/to PoseWithCovarianceStamped

Signed-off-by: Alaa El Jawad <[email protected]>
  • Loading branch information
@ejalaa12
ejalaa12 authored May 1, 2023
1 parent b9694c1 commit 4e9cadc
Show file tree
Hide file tree
Showing 7 changed files with 117 additions and 55 deletions.
111 changes: 56 additions & 55 deletions ros_gz_bridge/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,61 +5,62 @@ between ROS and Gazebo Transport.

The following message types can be bridged for topics:

| ROS type | Gazebo Transport Type |
|------------------------------------|:----------------------------------:|
| builtin_interfaces/Time | gz.msgs.Time |
| geometry_msgs/Point | gz.msgs.Vector3d |
| geometry_msgs/Pose | gz.msgs.Pose |
| geometry_msgs/msg/PoseArray | gz.msgs.Pose_V |
| geometry_msgs/PoseStamped | gz.msgs.Pose |
| geometry_msgs/PoseWithCovariance | gz.msgs.PoseWithCovariance |
| geometry_msgs/Quaternion | gz.msgs.Quaternion |
| geometry_msgs/Transform | gz.msgs.Pose |
| geometry_msgs/TransformStamped | gz.msgs.Pose |
| geometry_msgs/Twist | gz.msgs.Twist |
| geometry_msgs/TwistWithCovariance | gz.msgs.TwistWithCovariance |
| geometry_msgs/Vector3 | gz.msgs.Vector3d |
| geometry_msgs/Wrench | gz.msgs.Wrench |
| gps_msgs/GPSFix | gz.msgs.NavSat |
| nav_msgs/Odometry | gz.msgs.Odometry |
| nav_msgs/Odometry | gz.msgs.OdometryWithCovariance |
| rcl_interfaces/ParameterValue | gz.msgs.Any |
| ros_gz_interfaces/Contact | gz.msgs.Contact |
| ros_gz_interfaces/Contacts | gz.msgs.Contacts |
| ros_gz_interfaces/Dataframe | gz.msgs.Dataframe |
| ros_gz_interfaces/Entity | gz.msgs.Entity |
| ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V |
| ros_gz_interfaces/GuiCamera | gz.msgs.GUICamera |
| ros_gz_interfaces/JointWrench | gz.msgs.JointWrench |
| ros_gz_interfaces/Light | gz.msgs.Light |
| ros_gz_interfaces/ParamVec | gz.msgs.Param |
| ros_gz_interfaces/ParamVec | gz.msgs.Param_V |
| ros_gz_interfaces/StringVec | gz.msgs.StringMsg_V |
| ros_gz_interfaces/TrackVisual | gz.msgs.TrackVisual |
| ros_gz_interfaces/VideoRecord | gz.msgs.VideoRecord |
| rosgraph_msgs/Clock | gz.msgs.Clock |
| sensor_msgs/BatteryState | gz.msgs.BatteryState |
| sensor_msgs/CameraInfo | gz.msgs.CameraInfo |
| sensor_msgs/FluidPressure | gz.msgs.FluidPressure |
| sensor_msgs/Image | gz.msgs.Image |
| sensor_msgs/Imu | gz.msgs.IMU |
| sensor_msgs/JointState | gz.msgs.Model |
| sensor_msgs/Joy | gz.msgs.Joy |
| sensor_msgs/LaserScan | gz.msgs.LaserScan |
| sensor_msgs/MagneticField | gz.msgs.Magnetometer |
| sensor_msgs/NavSatFix | gz.msgs.NavSat |
| sensor_msgs/PointCloud2 | gz.msgs.PointCloudPacked |
| std_msgs/Bool | gz.msgs.Boolean |
| std_msgs/ColorRGBA | gz.msgs.Color |
| std_msgs/Empty | gz.msgs.Empty |
| std_msgs/Float32 | gz.msgs.Float |
| std_msgs/Float64 | gz.msgs.Double |
| std_msgs/Header | gz.msgs.Header |
| std_msgs/Int32 | gz.msgs.Int32 |
| std_msgs/String | gz.msgs.StringMsg |
| std_msgs/UInt32 | gz.msgs.UInt32 |
| tf2_msgs/TFMessage | gz.msgs.Pose_V |
| trajectory_msgs/JointTrajectory | gz.msgs.JointTrajectory |
| ROS type | Gazebo Transport Type |
| --------------------------------------- | :----------------------------- |
| builtin_interfaces/Time | gz.msgs.Time |
| geometry_msgs/Point | gz.msgs.Vector3d |
| geometry_msgs/Pose | gz.msgs.Pose |
| geometry_msgs/msg/PoseArray | gz.msgs.Pose_V |
| geometry_msgs/PoseStamped | gz.msgs.Pose |
| geometry_msgs/PoseWithCovariance | gz.msgs.PoseWithCovariance |
| geometry_msgs/PoseWithCovarianceStamped | gz.msgs.PoseWithCovariance |
| geometry_msgs/Quaternion | gz.msgs.Quaternion |
| geometry_msgs/Transform | gz.msgs.Pose |
| geometry_msgs/TransformStamped | gz.msgs.Pose |
| geometry_msgs/Twist | gz.msgs.Twist |
| geometry_msgs/TwistWithCovariance | gz.msgs.TwistWithCovariance |
| geometry_msgs/Vector3 | gz.msgs.Vector3d |
| geometry_msgs/Wrench | gz.msgs.Wrench |
| gps_msgs/GPSFix | gz.msgs.NavSat |
| nav_msgs/Odometry | gz.msgs.Odometry |
| nav_msgs/Odometry | gz.msgs.OdometryWithCovariance |
| rcl_interfaces/ParameterValue | gz.msgs.Any |
| ros_gz_interfaces/Contact | gz.msgs.Contact |
| ros_gz_interfaces/Contacts | gz.msgs.Contacts |
| ros_gz_interfaces/Dataframe | gz.msgs.Dataframe |
| ros_gz_interfaces/Entity | gz.msgs.Entity |
| ros_gz_interfaces/msg/Float32Array | gz.msgs.Float_V |
| ros_gz_interfaces/GuiCamera | gz.msgs.GUICamera |
| ros_gz_interfaces/JointWrench | gz.msgs.JointWrench |
| ros_gz_interfaces/Light | gz.msgs.Light |
| ros_gz_interfaces/ParamVec | gz.msgs.Param |
| ros_gz_interfaces/ParamVec | gz.msgs.Param_V |
| ros_gz_interfaces/StringVec | gz.msgs.StringMsg_V |
| ros_gz_interfaces/TrackVisual | gz.msgs.TrackVisual |
| ros_gz_interfaces/VideoRecord | gz.msgs.VideoRecord |
| rosgraph_msgs/Clock | gz.msgs.Clock |
| sensor_msgs/BatteryState | gz.msgs.BatteryState |
| sensor_msgs/CameraInfo | gz.msgs.CameraInfo |
| sensor_msgs/FluidPressure | gz.msgs.FluidPressure |
| sensor_msgs/Image | gz.msgs.Image |
| sensor_msgs/Imu | gz.msgs.IMU |
| sensor_msgs/JointState | gz.msgs.Model |
| sensor_msgs/Joy | gz.msgs.Joy |
| sensor_msgs/LaserScan | gz.msgs.LaserScan |
| sensor_msgs/MagneticField | gz.msgs.Magnetometer |
| sensor_msgs/NavSatFix | gz.msgs.NavSat |
| sensor_msgs/PointCloud2 | gz.msgs.PointCloudPacked |
| std_msgs/Bool | gz.msgs.Boolean |
| std_msgs/ColorRGBA | gz.msgs.Color |
| std_msgs/Empty | gz.msgs.Empty |
| std_msgs/Float32 | gz.msgs.Float |
| std_msgs/Float64 | gz.msgs.Double |
| std_msgs/Header | gz.msgs.Header |
| std_msgs/Int32 | gz.msgs.Int32 |
| std_msgs/String | gz.msgs.StringMsg |
| std_msgs/UInt32 | gz.msgs.UInt32 |
| tf2_msgs/TFMessage | gz.msgs.Pose_V |
| trajectory_msgs/JointTrajectory | gz.msgs.JointTrajectory |

And the following for services:

Expand Down
13 changes: 13 additions & 0 deletions ros_gz_bridge/include/ros_gz_bridge/convert/geometry_msgs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
Expand Down Expand Up @@ -109,6 +110,18 @@ convert_ros_to_gz(
const geometry_msgs::msg::PoseWithCovariance & ros_msg,
gz::msgs::PoseWithCovariance & gz_msg);

template<>
void
convert_gz_to_ros(
const gz::msgs::PoseWithCovariance & gz_msg,
geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg);

template<>
void
convert_ros_to_gz(
const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg,
gz::msgs::PoseWithCovariance & gz_msg);

template<>
void
convert_gz_to_ros(
Expand Down
1 change: 1 addition & 0 deletions ros_gz_bridge/ros_gz_bridge/mappings.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@
Mapping('PoseArray', 'Pose_V'),
Mapping('PoseStamped', 'Pose'),
Mapping('PoseWithCovariance', 'PoseWithCovariance'),
Mapping('PoseWithCovarianceStamped', 'PoseWithCovariance'),
Mapping('Quaternion', 'Quaternion'),
Mapping('Transform', 'Pose'),
Mapping('TransformStamped', 'Pose'),
Expand Down
21 changes: 21 additions & 0 deletions ros_gz_bridge/src/convert/geometry_msgs.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,6 +165,27 @@ convert_gz_to_ros(
}
}

template<>
void
convert_ros_to_gz(
const geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg,
gz::msgs::PoseWithCovariance & gz_msg)
{
convert_ros_to_gz(ros_msg.header, (*gz_msg.mutable_pose()->mutable_header()));
convert_ros_to_gz(ros_msg.pose, gz_msg);
}

template<>
void
convert_gz_to_ros(
const gz::msgs::PoseWithCovariance & gz_msg,
geometry_msgs::msg::PoseWithCovarianceStamped & ros_msg)
{
convert_gz_to_ros(gz_msg.pose().header(), ros_msg.header);
convert_gz_to_ros(gz_msg, ros_msg.pose);
}


template<>
void
convert_ros_to_gz(
Expand Down
1 change: 1 addition & 0 deletions ros_gz_bridge/test/utils/gz_test_msg.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -323,6 +323,7 @@ void createTestMsg(gz::msgs::PoseWithCovariance & _msg)
{
createTestMsg(*_msg.mutable_pose()->mutable_position());
createTestMsg(*_msg.mutable_pose()->mutable_orientation());
createTestMsg(*_msg.mutable_pose()->mutable_header());
for (int i = 0; i < 36; i++) {
_msg.mutable_covariance()->add_data(i);
}
Expand Down
12 changes: 12 additions & 0 deletions ros_gz_bridge/test/utils/ros_test_msg.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -374,6 +374,18 @@ void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovariance
}
}

void createTestMsg(geometry_msgs::msg::PoseWithCovarianceStamped & _msg)
{
createTestMsg(_msg.header);
createTestMsg(_msg.pose);
}

void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped> & _msg)
{
compareTestMsg(std::make_shared<geometry_msgs::msg::PoseWithCovariance>(_msg->pose));
compareTestMsg(std::make_shared<std_msgs::msg::Header>(_msg->header));
}

void createTestMsg(geometry_msgs::msg::PoseStamped & _msg)
{
createTestMsg(_msg.header);
Expand Down
13 changes: 13 additions & 0 deletions ros_gz_bridge/test/utils/ros_test_msg.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,7 @@
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_array.hpp>
#include <geometry_msgs/msg/pose_with_covariance.hpp>
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
Expand Down Expand Up @@ -256,6 +257,18 @@ void createTestMsg(geometry_msgs::msg::PoseWithCovariance & _msg);
/// \param[in] _msg The message to compare.
void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovariance> & _msg);

/// \brief Compare a message with the populated for testing.
/// \param[in] _msg The message to compare.
void compareTestMsg(const geometry_msgs::msg::PoseWithCovarianceStamped & _msg);

/// \brief Create a message used for testing.
/// \param[out] _msg The message populated.
void createTestMsg(geometry_msgs::msg::PoseWithCovarianceStamped & _msg);

/// \brief Compare a message with the populated for testing.
/// \param[in] _msg The message to compare.
void compareTestMsg(const std::shared_ptr<geometry_msgs::msg::PoseWithCovarianceStamped> & _msg);

/// \brief Create a message used for testing.
/// \param[out] _msg The message populated.
void createTestMsg(geometry_msgs::msg::PoseStamped & _msg);
Expand Down

0 comments on commit 4e9cadc

Please sign in to comment.