30 #include "amcl_thread.h"
31 #include "amcl_utils.h"
33 #include <utils/math/angle.h>
34 #include <core/threading/mutex.h>
35 #include <core/threading/mutex_locker.h>
40 # include <ros/node_handle.h>
41 # include <geometry_msgs/PoseArray.h>
43 # include <nav_msgs/OccupancyGrid.h>
47 using namespace fawkes;
49 static double normalize(
double z) {
50 return atan2(sin(z), cos(z));
53 static double angle_diff(
double a,
double b) {
58 d2 = 2 * M_PI - fabs(d1);
61 if (fabs(d1) < fabs(d2))
72 std::vector<std::pair<int,int> > AmclThread::free_space_indices;
81 conf_mutex_ =
new Mutex();
92 fawkes::amcl::read_map_config(
config, cfg_map_file_, cfg_resolution_, cfg_origin_x_,
93 cfg_origin_y_, cfg_origin_theta_, cfg_occupied_thresh_,
99 map_ = fawkes::amcl::read_map(cfg_map_file_.c_str(),
100 cfg_origin_x_, cfg_origin_y_, cfg_resolution_,
101 cfg_occupied_thresh_, cfg_free_thresh_, free_space_indices);
102 map_width_ = map_->size_x;
103 map_height_ = map_->size_y;
106 map_width_, map_height_, free_space_indices.size(),
107 map_width_ * map_height_,
108 (float)free_space_indices.size() / (float)(map_width_ * map_height_) * 100.);
110 sent_first_transform_ =
false;
111 latest_tf_valid_ =
false;
117 initial_pose_hyp_ = NULL;
118 first_map_received_ =
false;
119 first_reconfigure_call_ =
true;
124 init_cov_[0] = 0.5 * 0.5;
125 init_cov_[1] = 0.5 * 0.5;
126 init_cov_[2] = (M_PI / 12.0) * (M_PI / 12.0);
144 laser_likelihood_max_dist_ =
154 if (angle_min_idx_ > 359) {
155 throw Exception(
"Angle min index out of bounds");
162 if (angle_max_idx_ > 359) {
163 throw Exception(
"Angle max index out of bounds");
166 angle_max_idx_ = 359;
168 angle_range_ = (
unsigned int)abs(angle_max_idx_ - angle_min_idx_);
169 angle_min_ =
deg2rad(angle_min_idx_);
174 resample_interval_ =
config->
get_uint(CFG_PREFIX
"resample_interval");
181 std::string tmp_model_type;
184 if (tmp_model_type ==
"beam")
185 laser_model_type_ = ::amcl::LASER_MODEL_BEAM;
186 else if (tmp_model_type ==
"likelihood_field")
187 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
190 "Unknown laser model type \"%s\"; "
191 "defaulting to likelihood_field model",
192 tmp_model_type.c_str());
193 laser_model_type_ = ::amcl::LASER_MODEL_LIKELIHOOD_FIELD;
197 if (tmp_model_type ==
"diff")
198 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
199 else if (tmp_model_type ==
"omni")
200 odom_model_type_ = ::amcl::ODOM_MODEL_OMNI;
203 "Unknown odom model type \"%s\"; defaulting to diff model",
204 tmp_model_type.c_str());
205 odom_model_type_ = ::amcl::ODOM_MODEL_DIFF;
232 transform_tolerance_ =
config->
get_float(CFG_PREFIX
"transform_tolerance");
234 if (min_particles_ > max_particles_) {
236 "You've set min_particles to be less than max particles, "
237 "this isn't allowed so they'll be set to be equal.");
238 max_particles_ = min_particles_;
243 pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
244 (pf_init_model_fn_t) AmclThread::uniform_pose_generator,
247 pf_init_model(pf_, (pf_init_model_fn_t)AmclThread::uniform_pose_generator,
250 pf_->pop_err = pf_err_;
255 pf_vector_t pf_init_pose_mean = pf_vector_zero();
263 pf_init_pose_mean.v[0] = init_pose_[0];
264 pf_init_pose_mean.v[1] = init_pose_[1];
265 pf_init_pose_mean.v[2] = init_pose_[2];
267 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
268 pf_init_pose_cov.m[0][0] = init_cov_[0];
269 pf_init_pose_cov.m[1][1] = init_cov_[1];
270 pf_init_pose_cov.m[2][2] = init_cov_[2];
271 pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
280 odom_ = new ::amcl::AMCLOdom();
282 if (odom_model_type_ == ::amcl::ODOM_MODEL_OMNI)
283 odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
285 odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);
288 laser_ = new ::amcl::AMCLLaser(max_beams_, map_);
290 if (laser_model_type_ == ::amcl::LASER_MODEL_BEAM) {
291 laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_,
295 "Initializing likelihood field model; "
296 "this can take some time on large maps...");
297 laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
298 laser_likelihood_max_dist_);
302 laser_pose_set_ = set_laser_pose();
305 last_move_time_->
stamp();
309 rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>(
"amcl_pose", 2);
311 rosnode->advertise<geometry_msgs::PoseArray>(
"particlecloud", 2);
313 rosnode->subscribe(
"initialpose", 2,
314 &AmclThread::initial_pose_received,
this);
316 map_pub_ = rosnode->advertise<nav_msgs::OccupancyGrid>(
"map", 1,
true);
326 pos3d_if_->
set_frame(global_frame_id_.c_str());
329 apply_initial_pose();
336 if (!laser_pose_set_) {
337 if (set_laser_pose()) {
338 laser_pose_set_ =
true;
339 apply_initial_pose();
351 float* laser_distances = laser_if_->
distances();
361 if (!get_odom_pose(odom_pose, pose.v[0], pose.v[1], pose.v[2],
362 &latest, base_frame_id_))
365 "associated with laser scan");
369 pf_vector_t delta = pf_vector_zero();
374 delta.v[0] = pose.v[0] - pf_odom_pose_.v[0];
375 delta.v[1] = pose.v[1] - pf_odom_pose_.v[1];
376 delta.v[2] = angle_diff(pose.v[2], pf_odom_pose_.v[2]);
382 fabs(delta.v[0]) > d_thresh_ ||
383 fabs(delta.v[1]) > d_thresh_ ||
384 fabs(delta.v[2]) > a_thresh_;
387 last_move_time_->
stamp();
397 laser_update_ =
true;
398 }
else if ((now - last_move_time_) <= t_thresh_) {
399 laser_update_ =
true;
403 bool force_publication =
false;
407 pf_odom_pose_ = pose;
413 laser_update_ =
true;
414 force_publication =
true;
417 }
else if (pf_init_ && laser_update_) {
422 ::amcl::AMCLOdomData odata;
431 odom_->UpdateAction(pf_, (::amcl::AMCLSensorData*) &odata);
437 bool resampled =
false;
442 ::amcl::AMCLLaserData ldata;
443 ldata.sensor = laser_;
444 ldata.range_count = angle_range_ + 1;
452 q.setEulerZYX(angle_min_, 0.0, 0.0);
454 q.setEulerZYX(angle_min_ + angle_increment_, 0.0, 0.0);
469 double angle_min = tf::get_yaw(min_q);
470 double angle_increment = tf::get_yaw(inc_q) - angle_min;
473 angle_increment = fmod(angle_increment + 5 * M_PI, 2 * M_PI) - M_PI;
476 if (laser_max_range_ > 0.0)
477 ldata.range_max = (float) laser_max_range_;
479 ldata.range_max = HUGE;
481 if (laser_min_range_ > 0.0)
482 range_min = (float) laser_min_range_;
486 ldata.ranges =
new double[ldata.range_count][2];
489 for (
int i = 0; i < ldata.range_count; ++i) {
490 unsigned int idx = (angle_min_idx_ + i) % maxlen_dist;
493 if (laser_distances[idx] <= range_min)
494 ldata.ranges[i][0] = ldata.range_max;
496 ldata.ranges[i][0] = laser_distances[idx];
499 ldata.ranges[i][1] = angle_min + (idx * angle_increment);
503 laser_->UpdateSensor(pf_, (::amcl::AMCLSensorData*) &ldata);
506 "exception follows");
510 laser_update_ =
false;
512 pf_odom_pose_ = pose;
515 if (!(++resample_count_ % resample_interval_)) {
517 pf_update_resample(pf_);
522 pf_sample_set_t* set = (pf_->sets) + pf_->current_set;
527 geometry_msgs::PoseArray cloud_msg;
528 cloud_msg.header.stamp = ros::Time::now();
529 cloud_msg.header.frame_id = global_frame_id_;
530 cloud_msg.poses.resize(set->sample_count);
531 for (
int i = 0; i < set->sample_count; i++) {
532 tf::Quaternion q(tf::create_quaternion_from_yaw(set->samples[i].pose.v[2]));
533 cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
534 cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
535 cloud_msg.poses[i].position.z = 0.;
536 cloud_msg.poses[i].orientation.x = q.x();
537 cloud_msg.poses[i].orientation.y = q.y();
538 cloud_msg.poses[i].orientation.z = q.z();
539 cloud_msg.poses[i].orientation.w = q.w();
542 particlecloud_pub_.publish(cloud_msg);
546 if (resampled || force_publication) {
548 double max_weight = 0.0;
549 int max_weight_hyp = -1;
550 std::vector<amcl_hyp_t> hyps;
551 hyps.resize(pf_->sets[pf_->current_set].cluster_count);
552 for (
int hyp_count = 0;
553 hyp_count < pf_->sets[pf_->current_set].cluster_count;
556 pf_vector_t pose_mean;
557 pf_matrix_t pose_cov;
558 if (!pf_get_cluster_stats(pf_, hyp_count, &weight, &pose_mean,
565 hyps[hyp_count].weight = weight;
566 hyps[hyp_count].pf_pose_mean = pose_mean;
567 hyps[hyp_count].pf_pose_cov = pose_cov;
569 if (hyps[hyp_count].weight > max_weight) {
570 max_weight = hyps[hyp_count].weight;
571 max_weight_hyp = hyp_count;
575 if (max_weight > 0.0) {
587 geometry_msgs::PoseWithCovarianceStamped p;
589 p.header.frame_id = global_frame_id_;
590 p.header.stamp = ros::Time();
592 p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
593 p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
594 tf::Quaternion q(tf::Vector3(0,0,1),
595 hyps[max_weight_hyp].pf_pose_mean.v[2]);
596 p.pose.pose.orientation.x = q.x();
597 p.pose.pose.orientation.y = q.y();
598 p.pose.pose.orientation.z = q.z();
599 p.pose.pose.orientation.w = q.w();
601 pf_sample_set_t* set = pf_->sets + pf_->current_set;
602 for (
int i = 0; i < 2; i++) {
603 for (
int j = 0; j < 2; j++) {
607 last_covariance_[6 * i + j] = set->cov.m[i][j];
614 last_covariance_[6 * 5 + 5] = set->cov.m[2][2];
616 pose_pub_.publish(p);
631 tmp_tf(tf::create_quaternion_from_yaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
632 tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
633 hyps[max_weight_hyp].pf_pose_mean.v[1], 0.0));
636 latest, base_frame_id_);
638 tmp_tf_stamped, odom_to_map);
641 "Failed to subtract base to odom transform");
646 tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
647 tf::Point(odom_to_map.getOrigin()));
648 latest_tf_valid_ =
true;
652 Time transform_expiration =
653 (*laser_if_->
timestamp() + transform_tolerance_);
655 transform_expiration,
656 global_frame_id_, odom_frame_id_);
663 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
664 tf::Quaternion map_att = map_pose.getRotation();
666 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
667 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
678 sent_first_transform_ =
true;
682 }
else if (latest_tf_valid_) {
685 Time transform_expiration =
686 (*laser_if_->
timestamp() + transform_tolerance_);
688 transform_expiration,
689 global_frame_id_, odom_frame_id_);
697 tf::Pose map_pose = latest_tf_.inverse() * odom_pose;
698 tf::Quaternion map_att = map_pose.getRotation();
700 double trans[3] = {map_pose.getOrigin().x(), map_pose.getOrigin().y(), 0};
701 double rot[4] = { map_att.x(), map_att.y(), map_att.z(), map_att.w() };
745 delete initial_pose_hyp_;
746 initial_pose_hyp_ = NULL;
748 delete last_move_time_;
754 pose_pub_.shutdown();
755 particlecloud_pub_.shutdown();
756 initial_pose_sub_.shutdown();
763 double& y,
double& yaw,
768 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1),
769 tf::Vector3(0, 0, 0)), t, f);
774 "Failed to compute odom pose, skipping scan (%s)", e.
what());
777 x = odom_pose.getOrigin().x();
778 y = odom_pose.getOrigin().y();
780 odom_pose.getBasis().getEulerZYX(yaw, pitch, roll);
791 AmclThread::publish_map()
793 nav_msgs::OccupancyGrid msg;
794 msg.info.map_load_time = ros::Time::now();
795 msg.header.stamp = ros::Time::now();
796 msg.header.frame_id =
"map";
798 msg.info.width = map_width_;
799 msg.info.height = map_height_;
800 msg.info.resolution = cfg_resolution_;
801 msg.info.origin.position.x = 0.;
802 msg.info.origin.position.y = 0.;
803 msg.info.origin.position.z = 0.0;
804 tf::Quaternion q(tf::create_quaternion_from_yaw(0.));
805 msg.info.origin.orientation.x = q.x();
806 msg.info.origin.orientation.y = q.y();
807 msg.info.origin.orientation.z = q.z();
808 msg.info.origin.orientation.w = q.w();
811 msg.data.resize(msg.info.width * msg.info.height);
813 for (
unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
814 if (map_->cells[i].occ_state == +1) {
816 }
else if (map_->cells[i].occ_state == -1) {
823 map_pub_.publish(msg);
830 AmclThread::set_laser_pose()
835 ident(tf::Transform(tf::Quaternion(0, 0, 0, 1), tf::Vector3(0, 0, 0)),
836 &now, laser_frame_id_);
852 laser_frame_id_.c_str(), base_frame_id_.c_str());
874 pf_vector_t laser_pose_v;
875 laser_pose_v.v[0] = laser_pose.getOrigin().x();
876 laser_pose_v.v[1] = laser_pose.getOrigin().y();
879 laser_pose_v.v[2] = tf::get_yaw(laser_pose.getRotation());
880 laser_->SetLaserPose(laser_pose_v);
882 "Received laser's pose wrt robot: %.3f %.3f %.3f",
883 laser_pose_v.v[0], laser_pose_v.v[1], laser_pose_v.v[2]);
889 AmclThread::apply_initial_pose()
891 if (initial_pose_hyp_ != NULL && map_ != NULL) {
893 "(cov: %.3f %.3f %.3f, %.3f %.3f %.3f, %.3f %.3f %.3f)",
915 AmclThread::uniform_pose_generator(
void* arg)
917 map_t* map = (map_t*) arg;
918 #if NEW_UNIFORM_SAMPLING
919 unsigned int rand_index = drand48() * free_space_indices.size();
920 std::pair<int,int> free_point = free_space_indices[rand_index];
922 p.v[0] = MAP_WXGX(map, free_point.first);
923 p.v[1] = MAP_WYGY(map, free_point.second);
924 p.v[2] = drand48() * 2 * M_PI - M_PI;
926 double min_x, max_x, min_y, max_y;
927 min_x = (map->size_x * map->scale) / 2.0 - map->origin_x;
928 max_x = (map->size_x * map->scale) / 2.0 + map->origin_x;
929 min_y = (map->size_y * map->scale) / 2.0 - map->origin_y;
930 max_y = (map->size_y * map->scale) / 2.0 + map->origin_y;
934 p.v[0] = min_x + drand48() * (max_x - min_x);
935 p.v[1] = min_y + drand48() * (max_y - min_y);
936 p.v[2] = drand48() * 2 * M_PI - M_PI;
939 i = MAP_GXWX(map, p.v[0]);
940 j = MAP_GYWY(map, p.v[1]);
941 if (MAP_VALID(map,i,j) && (map->cells[MAP_INDEX(map,i,j)].occ_state == -1))
951 AmclThread::initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
954 if(msg->header.frame_id ==
"") {
957 "You should always supply a frame_id.");
962 "initial poses must be in the global frame, \"%s\"",
963 msg->header.frame_id.c_str(),
964 global_frame_id_.c_str());
970 msg->header.stamp.nsec / 1000);
977 base_frame_id_, msg_time,
978 global_frame_id_, tx_odom);
984 if(sent_first_transform_)
986 "in time (%s)", e.
what());
987 tx_odom.setIdentity();
993 tf::Pose pose_old, pose_new;
995 tf::Transform(tf::Quaternion(msg->pose.pose.orientation.x,
996 msg->pose.pose.orientation.y,
997 msg->pose.pose.orientation.z,
998 msg->pose.pose.orientation.w),
999 tf::Vector3(msg->pose.pose.position.x,
1000 msg->pose.pose.position.y,
1001 msg->pose.pose.position.z));
1002 pose_new = tx_odom.inverse() * pose_old;
1007 pose_new.getOrigin().x(),
1008 pose_new.getOrigin().y(),
1009 tf::get_yaw(pose_new));
1011 pf_vector_t pf_init_pose_mean = pf_vector_zero();
1012 pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
1013 pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
1014 pf_init_pose_mean.v[2] = tf::get_yaw(pose_new);
1015 pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
1017 for(
int i=0; i<2; i++) {
1018 for(
int j=0; j<2; j++) {
1019 pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j];
1022 pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5];
1024 delete initial_pose_hyp_;
1027 initial_pose_hyp_->
pf_pose_cov = pf_init_pose_cov;
1028 apply_initial_pose();
1030 last_move_time_->
stamp();
Laser360Interface Fawkes BlackBoard Interface.
void set_frame(const char *new_frame)
Set frame value.
virtual void log_info(const char *component, const char *format,...)=0
Log informational message.
A class for handling time.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Thread class encapsulation of pthreads.
void write()
Write from local copy into BlackBoard memory.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Logger * logger
This is the Logger member used to access the logger.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier)=0
Open interface for writing.
Clock * clock
By means of this member access to the clock is given.
pf_vector_t pf_pose_mean
Mean of pose esimate.
Thread aspect to use blocked timing.
const Time * timestamp() const
Get timestamp of last write.
virtual void init()
Initialize the thread.
Position3DInterface Fawkes BlackBoard Interface.
virtual const char * what() const
Get primary string.
void set_visibility_history(const int32_t new_visibility_history)
Set visibility_history value.
Base class for exceptions in Fawkes.
void read()
Read from BlackBoard into local copy.
pf_matrix_t pf_pose_cov
Covariance of pose estimate.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
const char * name() const
Get name of thread.
bool changed() const
Check if data has been changed.
Wrapper class to add time stamp and frame ID to base types.
int32_t visibility_history() const
Get visibility_history value.
float * distances() const
Get distances value.
virtual void finalize()
Finalize the thread.
virtual ~AmclThread()
Destructor.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual Interface * open_for_reading(const char *interface_type, const char *identifier)=0
Open interface for reading.
Time & stamp()
Set this time to the current time.
virtual unsigned int get_uint(const char *path)=0
Get value from configuration which is of type unsigned int.
A frame could not be looked up.
Mutex mutual exclusion lock.
float deg2rad(float deg)
Convert an angle given in degrees to radians.
size_t maxlenof_distances() const
Get maximum length of distances value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual float get_float(const char *path)=0
Get value from configuration which is of type float.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
virtual void loop()
Code to execute in the thread.
virtual void close(Interface *interface)=0
Close interface.