fast_slam_gz

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