/
/
/
1#include "fast_slam_gz_plugins/fake_landmark_poses.hpp"
2
3#include <sstream>
4#include <geometry_msgs/msg/pose.hpp>
5#include <tf2/LinearMath/Quaternion.h>
6
7GZ_ADD_PLUGIN(FakeLandmarkPoses, gz::sim::System, FakeLandmarkPoses::ISystemConfigure, FakeLandmarkPoses::ISystemPostUpdate)
8
9class FakeLandmarkPosesPrivate{
10public:
11 gz::sim::Model model{gz::sim::kNullEntity};
12
13 rclcpp::Node::SharedPtr ros_node_ptr;
14 rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr ros_landmark_pub;
15
16 std::vector<gz::sim::Entity> landmark_entities;
17 std::vector<gz::math::Pose3d> all_landmark_poses;
18
19 double threshold;
20
21 std::string ros_frame;
22
23 void getLandmarkMeasurements(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm);
24};
25
26void FakeLandmarkPosesPrivate::getLandmarkMeasurements(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm){
27 gz::math::Pose3d gt_pose = gz::sim::worldPose(model.Entity(), _ecm);
28
29 geometry_msgs::msg::PoseArray msg;
30
31 msg.header.frame_id = ros_frame;
32 msg.header.stamp = rclcpp::Time(_info.simTime.count());
33
34 for(const auto &landmark_pose : all_landmark_poses){
35 geometry_msgs::msg::Pose pose;
36
37 double dx = landmark_pose.Pos().X() - gt_pose.Pos().X();
38 double dy = landmark_pose.Pos().Y() - gt_pose.Pos().Y();
39
40 auto distance = std::hypot(dx, dy);
41
42 if(distance < threshold){
43 double yaw = gt_pose.Rot().Yaw();
44 auto rot_diff = gt_pose.CoordRotationSub(landmark_pose.Rot());
45 pose.position.x = std::cos(yaw) * dx + std::sin(yaw) * dy;
46 pose.position.y = -std::sin(yaw) * dx + std::cos(yaw) * dy;
47
48 pose.orientation.z = rot_diff.Z();
49 pose.orientation.w = rot_diff.W();
50
51 msg.poses.push_back(pose);
52 }
53 }
54
55 ros_landmark_pub->publish(msg);
56}
57
58FakeLandmarkPoses::FakeLandmarkPoses(): data_ptr(std::make_unique<FakeLandmarkPosesPrivate>()){}
59
60void FakeLandmarkPoses::Configure(const gz::sim::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, gz::sim::EntityComponentManager &_ecm,
61 gz::sim::EventManager &/*_event_mgr*/){
62
63 GZ_PROFILE("FakeLandmarkPoses::Configure");
64
65 data_ptr->model = gz::sim::Model(_entity);
66
67 // init if ros is not up
68 if (!rclcpp::ok()) {
69 rclcpp::init(0, nullptr);
70 }
71
72 std::string pose_array_topic;
73 if (!_sdf->HasElement("landmark_topic")){
74 gzmsg << "SDF missing tag <landmark_topic>, initializing to default landmark_topic topic";
75 pose_array_topic = "landmark_topic";
76 } else {
77 pose_array_topic = _sdf->Get<std::string>("landmark_topic");
78 }
79
80 if (!_sdf->HasElement("ros_frame")){
81 gzmsg << "SDF missing tag <ros_frame>, initializing to default base_footprint";
82 data_ptr->ros_frame = "base_footprint";
83 } else {
84 data_ptr->ros_frame = _sdf->Get<std::string>("ros_frame");
85 }
86
87 if (!_sdf->HasElement("threshold")){
88 gzmsg << "SDF missing tag <threshold>, initializing to default 10";
89 data_ptr->threshold = 10.;
90 } else {
91 data_ptr->threshold = _sdf->Get<double>("threshold");
92 }
93
94 std::string landmark_identifier;
95 if (!_sdf->HasElement("landmark_identifier")){
96 gzmsg << "SDF missing tag <landmark_identifier>, initializing to default landmark";
97 landmark_identifier = "landmark";
98 } else {
99 landmark_identifier = _sdf->Get<std::string>("landmark_identifier");
100 }
101
102
103 bool found_first_landmark = false;
104 for(size_t i=0; i< std::numeric_limits<size_t>::max(); i++){
105 std::stringstream ss;
106 ss << landmark_identifier << "_" << i << "_link";
107
108 auto landmark_entity = _ecm.EntityByComponents(gz::sim::components::Name(ss.str()));
109 if(!landmark_entity && !found_first_landmark)
110 continue;
111 else if(!landmark_entity && found_first_landmark)
112 break;
113 else if(landmark_entity && !found_first_landmark)
114 found_first_landmark = true;
115
116 gz::math::Pose3d landmark_pose = gz::sim::worldPose(landmark_entity, _ecm);
117 data_ptr->all_landmark_poses.push_back(landmark_pose);
118 }
119
120 std::stringstream node_name_stream;
121 node_name_stream << "gz_fake_landmark_" << landmark_identifier;
122
123 data_ptr->ros_node_ptr = rclcpp::Node::make_shared(node_name_stream.str());
124 data_ptr->ros_landmark_pub = data_ptr->ros_node_ptr->create_publisher<geometry_msgs::msg::PoseArray>(pose_array_topic, 1);
125
126}
127
128void FakeLandmarkPoses::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm){
129 GZ_PROFILE("FakeLandmarkPoses::PostUpdate");
130 data_ptr->getLandmarkMeasurements(_info, _ecm);
131}