fast_slam_gz

1#include "fast_slam_gz_plugins/fake_landmarks.hpp"
2
3#include <sstream>
4#include <geometry_msgs/msg/pose.hpp>
5
6GZ_ADD_PLUGIN(FakeLandmarks, gz::sim::System, FakeLandmarks::ISystemConfigure, FakeLandmarks::ISystemPostUpdate)
7
8class FakeLandmarksPrivate{
9public:
10    gz::sim::Model model{gz::sim::kNullEntity};
11
12    rclcpp::Node::SharedPtr ros_node_ptr;
13    rclcpp::Publisher<geometry_msgs::msg::PoseArray>::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 FakeLandmarksPrivate::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    geometry_msgs::msg::PoseArray 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::Pose pose;
35        pose.orientation.w = 1;
36
37
38        double dx = landmark_pose.Pos().X() - gt_pose.Pos().X();
39        double dy = landmark_pose.Pos().Y() - gt_pose.Pos().Y();
40
41        auto  distance = std::hypot(dx, dy);
42
43        if(distance < threshold){
44            double yaw = gt_pose.Rot().Yaw();
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            msg.poses.push_back(pose);
48        }
49    }
50
51    ros_landmark_pub->publish(msg);
52}
53
54FakeLandmarks::FakeLandmarks(): data_ptr(std::make_unique<FakeLandmarksPrivate>()){}
55
56void FakeLandmarks::Configure(const gz::sim::Entity &_entity, const std::shared_ptr<const sdf::Element> &_sdf, gz::sim::EntityComponentManager &_ecm,
57                   gz::sim::EventManager &/*_event_mgr*/){
58
59    GZ_PROFILE("FakeLandmarks::Configure");
60
61    data_ptr->model = gz::sim::Model(_entity);
62
63    // init if ros is not up
64    if (!rclcpp::ok()) {
65        rclcpp::init(0, nullptr);
66    }
67
68    std::string pose_array_topic;
69    if (!_sdf->HasElement("landmark_topic")){
70        gzmsg << "SDF missing tag <landmark_topic>, initializing to default landmark_topic topic";
71        pose_array_topic = "landmark_topic";
72    } else {
73        pose_array_topic = _sdf->Get<std::string>("landmark_topic");
74    }
75
76    if (!_sdf->HasElement("ros_frame")){
77        gzmsg << "SDF missing tag <ros_frame>, initializing to default base_footprint";
78        data_ptr->ros_frame = "base_footprint";
79    } else {
80        data_ptr->ros_frame = _sdf->Get<std::string>("ros_frame");
81    }
82
83    if (!_sdf->HasElement("threshold")){
84        gzmsg << "SDF missing tag <threshold>, initializing to default 10";
85        data_ptr->threshold = 10.;
86    } else {
87        data_ptr->threshold = _sdf->Get<double>("threshold");
88    }
89
90    std::string landmark_identifier;
91    if (!_sdf->HasElement("landmark_identifier")){
92        gzmsg << "SDF missing tag <landmark_identifier>, initializing to default landmark";
93        landmark_identifier = "landmark";
94    } else {
95        landmark_identifier = _sdf->Get<std::string>("landmark_identifier");
96    }
97
98    // get first landmark
99    size_t first_landmark = 0;
100    for(size_t i=0; i<10000; i++){
101        std::stringstream ss;
102        ss << landmark_identifier << "_" << i << "_link";
103        if(_ecm.EntityByComponents(gz::sim::components::Name(ss.str()), gz::sim::components::Link())){
104            first_landmark = i;
105            break;
106        }
107    }
108
109    gz::sim::Entity landmark_entity = 1;
110    while(landmark_entity){
111        std::stringstream ss;
112        ss << landmark_identifier << "_" << first_landmark << "_link";
113        landmark_entity = _ecm.EntityByComponents(gz::sim::components::Name(ss.str()), gz::sim::components::Link());
114        gz::math::Pose3d landmark_pose = gz::sim::worldPose(landmark_entity, _ecm);
115        data_ptr->all_landmark_poses.push_back(landmark_pose);
116        first_landmark++;
117    }
118
119
120
121    data_ptr->ros_node_ptr = rclcpp::Node::make_shared("gz_fake_landmarks");
122    data_ptr->ros_landmark_pub = data_ptr->ros_node_ptr->create_publisher<geometry_msgs::msg::PoseArray>(pose_array_topic, 1);
123
124}
125
126void FakeLandmarks::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm){
127    GZ_PROFILE("FakeLandmarks::PostUpdate");
128    data_ptr->getLandmarkMeasurements(_info, _ecm);
129}