Fawkes API  Fawkes Development Version
ros_thread.cpp
1 /***************************************************************************
2  * ros_thread.cpp - Thread to interact with ROS for amcl plugin
3  *
4  * Created: Mon Jun 22 17:50:24 2015
5  * Copyright 2012-2015 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 /* Based on amcl_node.cpp from the ROS amcl package
22  * Copyright (c) 2008, Willow Garage, Inc.
23  */
24 
25 #include "ros_thread.h"
26 
27 #include <geometry_msgs/PoseArray.h>
28 #include <nav_msgs/OccupancyGrid.h>
29 #include <ros/node_handle.h>
30 
31 using namespace fawkes;
32 
33 /** @class AmclROSThread "ros_thread.h"
34  * Thread for ROS integration of the Adaptive Monte Carlo Localization.
35  * @author Tim Niemueller
36  */
37 
38 /** Constructor. */
39 AmclROSThread::AmclROSThread() : Thread("AmclROSThread", Thread::OPMODE_WAITFORWAKEUP)
40 {
41 }
42 
43 /** Destructor. */
45 {
46 }
47 
48 void
50 {
51  pose_pub_ = rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2);
52  particlecloud_pub_ = rosnode->advertise<geometry_msgs::PoseArray>("particlecloud", 2);
53  initial_pose_sub_ =
54  rosnode->subscribe("initialpose", 2, &AmclROSThread::initial_pose_received, this);
55  map_pub_ = rosnode->advertise<nav_msgs::OccupancyGrid>("map", 1, true);
56 
58 }
59 
60 void
62 {
63  blackboard->close(loc_if_);
64 
65  pose_pub_.shutdown();
66  particlecloud_pub_.shutdown();
67  initial_pose_sub_.shutdown();
68  map_pub_.shutdown();
69 }
70 
71 void
73 {
74 }
75 
76 /** Publish pose array to ROS.
77  * @param global_frame_id Name of the global frame
78  * @param set sample set to publish
79  */
80 void
81 AmclROSThread::publish_pose_array(const std::string &global_frame_id, const pf_sample_set_t *set)
82 {
83  geometry_msgs::PoseArray cloud_msg;
84  cloud_msg.header.stamp = ros::Time::now();
85  cloud_msg.header.frame_id = global_frame_id;
86  cloud_msg.poses.resize(set->sample_count);
87  for (int i = 0; i < set->sample_count; i++) {
88  tf::Quaternion q(tf::create_quaternion_from_yaw(set->samples[i].pose.v[2]));
89  cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
90  cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
91  cloud_msg.poses[i].position.z = 0.;
92  cloud_msg.poses[i].orientation.x = q.x();
93  cloud_msg.poses[i].orientation.y = q.y();
94  cloud_msg.poses[i].orientation.z = q.z();
95  cloud_msg.poses[i].orientation.w = q.w();
96  }
97 
98  particlecloud_pub_.publish(cloud_msg);
99 }
100 
101 /** Publish pose with covariance to ROS.
102  * @param global_frame_id Name of the global frame
103  * @param amcl_hyp AMCL hypothesis to finish, i.e. the converged pose
104  * @param covariance covariance associated with the pose
105  */
106 void
107 AmclROSThread::publish_pose(const std::string &global_frame_id,
108  const amcl_hyp_t & amcl_hyp,
109  const double covariance[36])
110 {
111  geometry_msgs::PoseWithCovarianceStamped p;
112  // Fill in the header
113  p.header.frame_id = global_frame_id;
114  p.header.stamp = ros::Time();
115  // Copy in the pose
116  p.pose.pose.position.x = amcl_hyp.pf_pose_mean.v[0];
117  p.pose.pose.position.y = amcl_hyp.pf_pose_mean.v[1];
118  tf::Quaternion q(tf::Vector3(0, 0, 1), amcl_hyp.pf_pose_mean.v[2]);
119  p.pose.pose.orientation.x = q.x();
120  p.pose.pose.orientation.y = q.y();
121  p.pose.pose.orientation.z = q.z();
122  p.pose.pose.orientation.w = q.w();
123 
124  // Copy in the covariance
125  for (int i = 0; i < 2; i++) {
126  for (int j = 0; j < 2; j++) {
127  // Report the overall filter covariance, rather than the
128  // covariance for the highest-weight cluster
129  p.pose.covariance[6 * i + j] = covariance[6 * i + j];
130  }
131  }
132  p.pose.covariance[6 * 5 + 5] = covariance[6 * 5 + 5];
133 
134  pose_pub_.publish(p);
135 }
136 
137 /** Publish map to ROS.
138  * @param global_frame_id Name of the global frame
139  * @param map map to publish
140  */
141 void
142 AmclROSThread::publish_map(const std::string &global_frame_id, const map_t *map)
143 {
144  nav_msgs::OccupancyGrid msg;
145  msg.info.map_load_time = ros::Time::now();
146  msg.header.stamp = ros::Time::now();
147  msg.header.frame_id = global_frame_id;
148 
149  msg.info.width = map->size_x;
150  msg.info.height = map->size_y;
151  msg.info.resolution = map->scale;
152  msg.info.origin.position.x = map->origin_x - (map->size_x / 2) * map->scale;
153  msg.info.origin.position.y = map->origin_y - (map->size_y / 2) * map->scale;
154  msg.info.origin.position.z = 0.0;
155  tf::Quaternion q(tf::create_quaternion_from_yaw(0));
156  msg.info.origin.orientation.x = q.x();
157  msg.info.origin.orientation.y = q.y();
158  msg.info.origin.orientation.z = q.z();
159  msg.info.origin.orientation.w = q.w();
160 
161  // Allocate space to hold the data
162  msg.data.resize((size_t)msg.info.width * msg.info.height);
163 
164  for (unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
165  if (map->cells[i].occ_state == +1) {
166  msg.data[i] = +100;
167  } else if (map->cells[i].occ_state == -1) {
168  msg.data[i] = 0;
169  } else {
170  msg.data[i] = -1;
171  }
172  }
173 
174  map_pub_.publish(msg);
175 }
176 
177 void
178 AmclROSThread::initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
179 {
180  fawkes::Time msg_time(msg->header.stamp.sec, msg->header.stamp.nsec / 1000);
181 
182  const double *covariance = msg->pose.covariance.data();
183  const double rotation[] = {msg->pose.pose.orientation.x,
184  msg->pose.pose.orientation.y,
185  msg->pose.pose.orientation.z,
186  msg->pose.pose.orientation.w};
187  const double translation[] = {msg->pose.pose.position.x,
188  msg->pose.pose.position.y,
189  msg->pose.pose.position.z};
190 
191  std::string frame = msg->header.frame_id;
192  if (!frame.empty() && frame[0] == '/')
193  frame = frame.substr(1);
194 
197  ipm->set_frame(frame.c_str());
198  ipm->set_translation(translation);
199  ipm->set_rotation(rotation);
200  ipm->set_covariance(covariance);
201 
202  try {
203  loc_if_->msgq_enqueue(ipm);
204  } catch (Exception &e) {
205  logger->log_error(name(), "Failed to set pose, exception follows");
206  logger->log_error(name(), e);
207  }
208 }
amcl_hyp_t
Pose hypothesis.
Definition: amcl_thread.h:49
AmclROSThread::publish_map
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
Definition: ros_thread.cpp:142
AmclROSThread::AmclROSThread
AmclROSThread()
Constructor.
Definition: ros_thread.cpp:39
fawkes::ROSAspect::rosnode
LockPtr< ros::NodeHandle > rosnode
Definition: ros.h:46
fawkes::LocalizationInterface::SetInitialPoseMessage
Definition: LocalizationInterface.h:62
fawkes::LocalizationInterface::SetInitialPoseMessage::set_rotation
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
Definition: LocalizationInterface.cpp:294
fawkes::Thread::name
const char * name() const
Definition: thread.h:99
fawkes::LoggingAspect::logger
Logger * logger
Definition: logging.h:50
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
fawkes::Logger::log_error
virtual void log_error(const char *component, const char *format,...)=0
fawkes::LocalizationInterface
Definition: LocalizationInterface.h:37
AmclROSThread::~AmclROSThread
virtual ~AmclROSThread()
Destructor.
Definition: ros_thread.cpp:44
fawkes::LocalizationInterface::SetInitialPoseMessage::set_covariance
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
Definition: LocalizationInterface.cpp:438
fawkes
AmclROSThread::init
virtual void init()
Initialize the thread.
Definition: ros_thread.cpp:49
AmclROSThread::publish_pose
void publish_pose(const std::string &global_frame_id, const amcl_hyp_t &amcl_hyp, const double last_covariance[36])
Publish pose with covariance to ROS.
Definition: ros_thread.cpp:107
fawkes::LocalizationInterface::SetInitialPoseMessage::set_frame
void set_frame(const char *new_frame)
Set frame value.
Definition: LocalizationInterface.cpp:229
fawkes::Time
Definition: time.h:96
fawkes::Thread
Definition: thread.h:44
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
Definition: blackboard.h:47
AmclROSThread::finalize
virtual void finalize()
Finalize the thread.
Definition: ros_thread.cpp:61
amcl_hyp_t::pf_pose_mean
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:54
fawkes::BlackBoard::open_for_reading
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
fawkes::Interface::msgq_enqueue
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:882
fawkes::LocalizationInterface::SetInitialPoseMessage::set_translation
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
Definition: LocalizationInterface.cpp:360
AmclROSThread::loop
virtual void loop()
Code to execute in the thread.
Definition: ros_thread.cpp:72
AmclROSThread::publish_pose_array
void publish_pose_array(const std::string &global_frame_id, const pf_sample_set_t *set)
Publish pose array to ROS.
Definition: ros_thread.cpp:81
fawkes::Exception
Definition: exception.h:39