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
31using 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. */
39AmclROSThread::AmclROSThread() : Thread("AmclROSThread", Thread::OPMODE_WAITFORWAKEUP)
40{
41}
42
43/** Destructor. */
45{
46}
47
48void
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
60void
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
71void
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 */
80void
81AmclROSThread::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 */
106void
107AmclROSThread::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 */
141void
142AmclROSThread::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
177void
178AmclROSThread::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}
virtual ~AmclROSThread()
Destructor.
Definition: ros_thread.cpp:44
virtual void loop()
Code to execute in the thread.
Definition: ros_thread.cpp:72
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
Definition: ros_thread.cpp:142
AmclROSThread()
Constructor.
Definition: ros_thread.cpp:39
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
virtual void finalize()
Finalize the thread.
Definition: ros_thread.cpp:61
virtual void init()
Initialize the thread.
Definition: ros_thread.cpp:49
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
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_reading(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for reading.
virtual void close(Interface *interface)=0
Close interface.
Base class for exceptions in Fawkes.
Definition: exception.h:36
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
Definition: interface.cpp:915
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
void set_translation(unsigned int index, const double new_translation)
Set translation value at given index.
void set_rotation(unsigned int index, const double new_rotation)
Set rotation value at given index.
void set_frame(const char *new_frame)
Set frame value.
void set_covariance(unsigned int index, const double new_covariance)
Set covariance value at given index.
LocalizationInterface Fawkes BlackBoard Interface.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Definition: ros.h:47
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
A class for handling time.
Definition: time.h:93
Fawkes library namespace.
Pose hypothesis.
Definition: amcl_thread.h:51
pf_vector_t pf_pose_mean
Mean of pose esimate.
Definition: amcl_thread.h:55