/
/
/
1#include "fast_slam_ros/fast_slam_ros.hpp"
2
3FastSlamRos::FastSlamRos():
4 rclcpp::Node("fast_slam_ros"),
5 p_set_(10, Eigen::Vector3d::Zero(), 0.01 * Eigen::Matrix3d::Identity())
6 {
7
8 std::cout << "setting up \n";
9 odom_frame_ = "odom";
10 robot_frame_ = "base_footprint";
11 map_frame_ = "map";
12
13 tf_buffer_ = std::make_unique<tf2_ros::Buffer>(rclcpp::Node::get_clock());
14 tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
15 tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);
16
17 particle_pub_ = rclcpp::Node::create_publisher<geometry_msgs::msg::PoseArray>("particles", 1);
18 landmark_pub_ = rclcpp::Node::create_publisher<geometry_msgs::msg::PoseArray>("map", 1);
19 path_pub_ = rclcpp::Node::create_publisher<nav_msgs::msg::Path>("path", 1);
20 distribution_pub_ = rclcpp::Node::create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("distribution", 1);
21
22 landmark_maesurement_sub_ = rclcpp::Node::create_subscription<geometry_msgs::msg::PoseArray>("/landmarks", 10, std::bind(&FastSlamRos::landmarkCallback, this, std::placeholders::_1));
23 odom_sub_ = rclcpp::Node::create_subscription<nav_msgs::msg::Odometry>("/odom", 10, std::bind(&FastSlamRos::odomCallback, this, std::placeholders::_1));
24
25 timer_ = rclcpp::Node::create_wall_timer(500ms, std::bind(&FastSlamRos::timerCallback, this));
26 path_.header.frame_id = map_frame_;
27}
28
29FastSlamRos::~FastSlamRos(){}
30
31void FastSlamRos::broadCastTransform(){
32 geometry_msgs::msg::TransformStamped transform_ro;
33
34 bool success = false;
35 while(!success){
36 try{
37 transform_ro = tf_buffer_->lookupTransform(robot_frame_, odom_frame_, rclcpp::Time(0), rclcpp::Duration::from_seconds(5));
38 success = true;
39 }
40 catch(tf2::LookupException &ex){
41
42 }
43 }
44
45 tf2::Transform odom_in_robot(tf2::Quaternion(0, 0, transform_ro.transform.rotation.z, transform_ro.transform.rotation.w),
46 tf2::Vector3(transform_ro.transform.translation.x, transform_ro.transform.translation.y, 0));
47
48 auto fast_slam_pose = p_set_.getLatestPoseEstimate();
49 tf2::Quaternion pose_q;
50 pose_q.setRPY(0, 0, fast_slam_pose(2));
51
52 tf2::Transform robot_in_map(pose_q, tf2::Vector3(fast_slam_pose(0), fast_slam_pose(1), 0));
53 tf2::Transform odom_in_map = robot_in_map * odom_in_robot;
54
55 geometry_msgs::msg::TransformStamped t_out;
56 t_out.child_frame_id = odom_frame_;
57 t_out.header.frame_id = map_frame_;
58 t_out.header.stamp = rclcpp::Node::get_clock()->now();
59 t_out.transform.translation.x = odom_in_map.getOrigin().getX();
60 t_out.transform.translation.y = odom_in_map.getOrigin().getY();
61 t_out.transform.rotation.z = odom_in_map.getRotation().getZ();
62 t_out.transform.rotation.w = odom_in_map.getRotation().getW();
63 if(success){
64 tf_broadcaster_->sendTransform(t_out);
65 }
66}
67
68void FastSlamRos::publishDistribution(){
69 auto latest_pose = p_set_.getLatestPoseEstimate();
70 auto latest_covariance = p_set_.getLatestCovarianceEstimate();
71 geometry_msgs::msg::PoseWithCovarianceStamped distribution_msg;
72 distribution_msg.header.frame_id = map_frame_;
73 distribution_msg.header.stamp = rclcpp::Node::get_clock()->now();
74
75 tf2::Quaternion pose_q;
76 pose_q.setRPY(0, 0, latest_pose(2));
77
78 distribution_msg.pose.pose.position.x = latest_pose(0);
79 distribution_msg.pose.pose.position.y = latest_pose(1);
80 distribution_msg.pose.pose.orientation.w = pose_q.getW();
81 distribution_msg.pose.pose.orientation.z = pose_q.getZ();
82
83 distribution_msg.pose.covariance[0] = latest_covariance(0,0);
84 distribution_msg.pose.covariance[1] = latest_covariance(0,1);
85 distribution_msg.pose.covariance[5] = latest_covariance(0,2);
86 distribution_msg.pose.covariance[6] = latest_covariance(1,0);
87 distribution_msg.pose.covariance[7] = latest_covariance(1,1);
88 distribution_msg.pose.covariance[11] = latest_covariance(1,2);
89 distribution_msg.pose.covariance[30] = latest_covariance(2,0);
90 distribution_msg.pose.covariance[31] = latest_covariance(2,1);
91 distribution_msg.pose.covariance[35] = latest_covariance(2,2);
92
93 distribution_pub_->publish(distribution_msg);
94}
95
96void FastSlamRos::publishParticles(){
97 auto particle_poses = p_set_.getAllParticlePoseEstimates();
98
99 geometry_msgs::msg::PoseArray particles;
100 particles.header.frame_id = "map";
101 particles.header.stamp = rclcpp::Node::get_clock()->now();
102
103 for(auto &pose : particle_poses){
104 geometry_msgs::msg::Pose pose_msg;
105 tf2::Quaternion particle_pose_q;
106 particle_pose_q.setRPY(0,0,pose(2));
107 pose_msg.position.x = pose(0);
108 pose_msg.position.y = pose(1);
109 pose_msg.orientation.w = particle_pose_q.getW();
110 pose_msg.orientation.z = particle_pose_q.getZ();
111 particles.poses.push_back(pose_msg);
112 }
113
114 particle_pub_->publish(particles);
115}
116
117void FastSlamRos::publishMap(){
118 // get map of the best particle
119 auto map_pair = p_set_.getBestParticle();
120
121 geometry_msgs::msg::PoseArray map;
122 map.header.stamp = rclcpp::Node::get_clock()->now();
123 map.header.frame_id = "map";
124
125 for(auto &landmark : map_pair.second){
126 geometry_msgs::msg::Pose pose_msg;
127 pose_msg.position.x = landmark->landmark_pose(0);
128 pose_msg.position.y = landmark->landmark_pose(1);
129 pose_msg.orientation.w = 1;
130
131 map.poses.push_back(pose_msg);
132 }
133
134 landmark_pub_->publish(map);
135}
136
137void FastSlamRos::publishPath(){
138 auto latest = p_set_.getLatestPoseEstimate();
139
140 path_.header.stamp = rclcpp::Node::get_clock()->now();
141
142 geometry_msgs::msg::PoseStamped new_pose;
143 tf2::Quaternion pose_q;
144 pose_q.setRPY(0,0,latest(2));
145
146 new_pose.header = path_.header;
147 new_pose.pose.position.x = latest(0);
148 new_pose.pose.position.y = latest(1);
149 new_pose.pose.orientation.w = pose_q.getW();
150 new_pose.pose.orientation.z = pose_q.getZ();
151
152 path_.poses.push_back(new_pose);
153
154 path_pub_->publish(path_);
155}
156
157double FastSlamRos::getYaw(tf2::Quaternion q){
158 tf2::Matrix3x3 m(q);
159 double roll, pitch, yaw;
160 m.getRPY(roll, pitch, yaw);
161 return yaw;
162}
163
164void FastSlamRos::timerCallback(){
165 StateVectorDerivative u;
166
167 rclcpp::Time at_time = latest_landmarks_.header.stamp;
168
169 geometry_msgs::msg::TransformStamped transform;
170 bool success = false;
171 while(!success){
172 try{
173 transform = tf_buffer_->lookupTransform(odom_frame_, robot_frame_, at_time, rclcpp::Duration::from_seconds(5));
174 success = true;
175 }
176 catch(tf2::LookupException &ex){
177
178 }
179 }
180
181 if(initialized_){
182 tf2::Quaternion q, q_prev, q_trans;
183 q.setX(0.0);
184 q.setY(0.0);
185 q.setW(transform.transform.rotation.w);
186 q.setZ(transform.transform.rotation.z);
187
188 q_prev.setX(0.0);
189 q_prev.setY(0.0);
190 q_prev.setW(last_transform_.transform.rotation.w);
191 q_prev.setZ(last_transform_.transform.rotation.z);
192 q_trans = q * q_prev.inverse();
193 double d_yaw = getYaw(q_trans);
194
195
196 u << transform.transform.translation.x - last_transform_.transform.translation.x,
197 transform.transform.translation.y - last_transform_.transform.translation.y,
198 d_yaw;
199
200
201 auto delta_time = std::chrono::nanoseconds((at_time - time_old_).nanoseconds());
202
203 std::shared_ptr<fastslam::MeasurementSet> m_set = std::make_shared<fastslam::MeasurementSet>();
204
205 uint32_t i = 1;
206 for(auto &pose : latest_landmarks_.poses){
207 Eigen::Vector2d lm_input;
208 lm_input << pose.position.x, pose.position.y;
209 std::shared_ptr<fastslam::Measurement> m_landmark = std::make_shared<fastslam::LandmarkXYMeasurement>(i, lm_input);
210 m_set->addMeasurement(m_landmark);
211 i++;
212 }
213
214 p_set_.updateParticleSet(m_set, u, delta_time);
215
216 publishPath();
217 publishMap();
218 publishParticles();
219 publishDistribution();
220
221 broadCastTransform();
222 }
223
224 last_transform_ = transform;
225 last_pose_ = current_pose_;
226 initialized_ = true;
227 time_old_ = at_time;
228}
229
230void FastSlamRos::landmarkCallback(const std::shared_ptr<geometry_msgs::msg::PoseArray> msg) {
231 latest_landmarks_.header = msg->header;
232 latest_landmarks_.poses = msg->poses;
233}
234
235void FastSlamRos::odomCallback(const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
236 current_pose_ = msg->pose.pose;
237}