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 <ros/node_handle.h>
28 #include <geometry_msgs/PoseArray.h>
29 #include <nav_msgs/OccupancyGrid.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. */
40  : Thread("AmclROSThread", Thread::OPMODE_WAITFORWAKEUP)
41 {
42 }
43 
44 /** Destructor. */
46 {
47 }
48 
49 void
51 {
52  pose_pub_ =
53  rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>("amcl_pose", 2);
54  particlecloud_pub_ =
55  rosnode->advertise<geometry_msgs::PoseArray>("particlecloud", 2);
56  initial_pose_sub_ =
57  rosnode->subscribe("initialpose", 2,
58  &AmclROSThread::initial_pose_received, this);
59  map_pub_ = rosnode->advertise<nav_msgs::OccupancyGrid>("map", 1, true);
60 
61  loc_if_ =
63 }
64 
65 
66 void
68 {
69  blackboard->close(loc_if_);
70 
71  pose_pub_.shutdown();
72  particlecloud_pub_.shutdown();
73  initial_pose_sub_.shutdown();
74  map_pub_.shutdown();
75 }
76 
77 
78 void
80 {
81 }
82 
83 
84 
85 /** Publish pose array to ROS.
86  * @param global_frame_id Name of the global frame
87  * @param set sample set to publish
88  */
89 void
90 AmclROSThread::publish_pose_array(const std::string &global_frame_id,
91  const pf_sample_set_t* set)
92 {
93  geometry_msgs::PoseArray cloud_msg;
94  cloud_msg.header.stamp = ros::Time::now();
95  cloud_msg.header.frame_id = global_frame_id;
96  cloud_msg.poses.resize(set->sample_count);
97  for (int i = 0; i < set->sample_count; i++) {
98  tf::Quaternion q(tf::create_quaternion_from_yaw(set->samples[i].pose.v[2]));
99  cloud_msg.poses[i].position.x = set->samples[i].pose.v[0];
100  cloud_msg.poses[i].position.y = set->samples[i].pose.v[1];
101  cloud_msg.poses[i].position.z = 0.;
102  cloud_msg.poses[i].orientation.x = q.x();
103  cloud_msg.poses[i].orientation.y = q.y();
104  cloud_msg.poses[i].orientation.z = q.z();
105  cloud_msg.poses[i].orientation.w = q.w();
106  }
107 
108  particlecloud_pub_.publish(cloud_msg);
109 }
110 
111 
112 /** Publish pose with covariance to ROS.
113  * @param global_frame_id Name of the global frame
114  * @param amcl_hyp AMCL hypothesis to finish, i.e. the converged pose
115  * @param covariance covariance associated with the pose
116  */
117 void
118 AmclROSThread::publish_pose(const std::string &global_frame_id,
119  const amcl_hyp_t &amcl_hyp,
120  const double covariance[36])
121 {
122  geometry_msgs::PoseWithCovarianceStamped p;
123  // Fill in the header
124  p.header.frame_id = global_frame_id;
125  p.header.stamp = ros::Time();
126  // Copy in the pose
127  p.pose.pose.position.x = amcl_hyp.pf_pose_mean.v[0];
128  p.pose.pose.position.y = amcl_hyp.pf_pose_mean.v[1];
129  tf::Quaternion q(tf::Vector3(0,0,1), amcl_hyp.pf_pose_mean.v[2]);
130  p.pose.pose.orientation.x = q.x();
131  p.pose.pose.orientation.y = q.y();
132  p.pose.pose.orientation.z = q.z();
133  p.pose.pose.orientation.w = q.w();
134 
135  // Copy in the covariance
136  for (int i = 0; i < 2; i++) {
137  for (int j = 0; j < 2; j++) {
138  // Report the overall filter covariance, rather than the
139  // covariance for the highest-weight cluster
140  p.pose.covariance[6*i+j] = covariance[6 * i + j];
141  }
142  }
143  p.pose.covariance[6 * 5 + 5] = covariance[6 * 5 + 5];
144 
145  pose_pub_.publish(p);
146 }
147 
148 /** Publish map to ROS.
149  * @param global_frame_id Name of the global frame
150  * @param map map to publish
151  */
152 void
153 AmclROSThread::publish_map(const std::string &global_frame_id,
154  const map_t *map)
155 {
156  nav_msgs::OccupancyGrid msg;
157  msg.info.map_load_time = ros::Time::now();
158  msg.header.stamp = ros::Time::now();
159  msg.header.frame_id = global_frame_id;
160 
161  msg.info.width = map->size_x;
162  msg.info.height = map->size_y;
163  msg.info.resolution = map->scale;
164  msg.info.origin.position.x = map->origin_x - (map->size_x / 2) * map->scale;
165  msg.info.origin.position.y = map->origin_y - (map->size_y / 2) * map->scale;
166  msg.info.origin.position.z = 0.0;
167  tf::Quaternion q(tf::create_quaternion_from_yaw(0));
168  msg.info.origin.orientation.x = q.x();
169  msg.info.origin.orientation.y = q.y();
170  msg.info.origin.orientation.z = q.z();
171  msg.info.origin.orientation.w = q.w();
172 
173  // Allocate space to hold the data
174  msg.data.resize(msg.info.width * msg.info.height);
175 
176  for (unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
177  if (map->cells[i].occ_state == +1) {
178  msg.data[i] = +100;
179  } else if (map->cells[i].occ_state == -1) {
180  msg.data[i] = 0;
181  } else {
182  msg.data[i] = -1;
183  }
184  }
185 
186  map_pub_.publish(msg);
187 }
188 
189 void
190 AmclROSThread::initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
191 {
192  fawkes::Time msg_time(msg->header.stamp.sec,
193  msg->header.stamp.nsec / 1000);
194 
195  const double *covariance = msg->pose.covariance.data();
196  const double rotation[] = {msg->pose.pose.orientation.x,
197  msg->pose.pose.orientation.y,
198  msg->pose.pose.orientation.z,
199  msg->pose.pose.orientation.w};
200  const double translation[] = {msg->pose.pose.position.x,
201  msg->pose.pose.position.y,
202  msg->pose.pose.position.z};
203 
204  std::string frame = msg->header.frame_id;
205  if (! frame.empty() && frame[0] == '/') frame = frame.substr(1);
206 
209  ipm->set_frame(frame.c_str());
210  ipm->set_translation(translation);
211  ipm->set_rotation(rotation);
212  ipm->set_covariance(covariance);
213 
214  try {
215  loc_if_->msgq_enqueue(ipm);
216  } catch (Exception &e) {
217  logger->log_error(name(), "Failed to set pose, exception follows");
218  logger->log_error(name(), e);
219  }
220 }
LocalizationInterface Fawkes BlackBoard Interface.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
virtual void finalize()
Finalize the thread.
Definition: ros_thread.cpp:67
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
Definition: ros_thread.cpp:153
Fawkes library namespace.
virtual void loop()
Code to execute in the thread.
Definition: ros_thread.cpp:79
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:44
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:55
AmclROSThread()
Constructor.
Definition: ros_thread.cpp:39
Base class for exceptions in Fawkes.
Definition: exception.h:36
Pose hypothesis.
Definition: amcl_thread.h:51
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
Definition: thread.h:95
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual ~AmclROSThread()
Destructor.
Definition: ros_thread.cpp:45
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
Definition: interface.cpp:903
virtual void init()
Initialize the thread.
Definition: ros_thread.cpp:50
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
void set_frame(const char *new_frame)
Set frame value.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:48
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:90
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:118
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual void close(Interface *interface)=0
Close interface.