fast_slam_gz

1#include "fast_slam_gz_plugins/fake_landmark_lines.hpp"
2
3#include <sstream>
4#include <geometry_msgs/msg/point.hpp>
5#include <fast_slam_ros_msgs/msg/line_landmark.hpp>
6
7GZ_ADD_PLUGIN(FakeLandmarkLines, gz::sim::System, FakeLandmarkLines::ISystemConfigure, FakeLandmarkLines::ISystemPostUpdate)
8
9struct GzLine{
10    double distance;
11    double heading;
12};
13
14double angleMod(double angle){
15    auto x = fmod(angle + M_PI, 2 * M_PI);
16    if (x < 0) x += 2 * M_PI;
17    return x - M_PI;
18}
19
20class FakeLandmarkLinesPrivate{
21public:
22    gz::sim::Model model{gz::sim::kNullEntity};
23
24    rclcpp::Node::SharedPtr ros_node_ptr;
25    rclcpp::Publisher<fast_slam_ros_msgs::msg::LineLandmarkArray>::SharedPtr ros_landmark_pub;
26
27    std::vector<gz::sim::Entity> landmark_entities;
28    std::vector<GzLine> all_landmark_lines;
29
30    double threshold;
31
32    std::string ros_frame;
33
34    void getLandmarkMeasurements(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm);
35};
36
37void FakeLandmarkLinesPrivate::getLandmarkMeasurements(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm){
38    gz::math::Pose3d gt_pose = gz::sim::worldPose(model.Entity(), _ecm);
39
40    fast_slam_ros_msgs::msg::LineLandmarkArray msg;
41    
42    msg.header.frame_id = ros_frame;
43    msg.header.stamp = rclcpp::Time(_info.simTime.count());
44
45    for(const auto &landmark_line :  all_landmark_lines){
46        fast_slam_ros_msgs::msg::LineLandmark line;
47
48        line.heading = angleMod(landmark_line.heading - gt_pose.Rot().Yaw());
49        line.distance = landmark_line.distance + std::cos(landmark_line.heading) * gt_pose.Pos().X() - std::sin(landmark_line.heading) * gt_pose.Pos().Y();
50
51
52        msg.line_landmarks.push_back(line);
53    }
54
55    ros_landmark_pub->publish(msg);
56}
57
58FakeLandmarkLines::FakeLandmarkLines(): data_ptr(std::make_unique<FakeLandmarkLinesPrivate>()){}
59
60void FakeLandmarkLines::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("FakeLandmarkLines::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
118        double distance = std::hypot(landmark_pose.Pos().X(), landmark_pose.Pos().Y());
119        double heading = std::atan2(landmark_pose.Pos().Y(), landmark_pose.Pos().X());
120
121        heading = angleMod(heading + 0.5*M_PI);
122
123        GzLine line;
124        line.distance = distance;
125        line.heading = heading;
126        data_ptr->all_landmark_lines.push_back(line);
127    }
128
129    std::stringstream node_name_stream;
130    node_name_stream << "gz_fake_landmark_" << landmark_identifier;
131
132    data_ptr->ros_node_ptr = rclcpp::Node::make_shared(node_name_stream.str());
133    data_ptr->ros_landmark_pub = data_ptr->ros_node_ptr->create_publisher<fast_slam_ros_msgs::msg::LineLandmarkArray>(pose_array_topic, 1);
134
135}
136
137void FakeLandmarkLines::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm){
138    GZ_PROFILE("FakeLandmarkLines::PostUpdate");
139    data_ptr->getLandmarkMeasurements(_info, _ecm);
140}