25 #include "ros_thread.h"
27 #include <geometry_msgs/PoseArray.h>
28 #include <nav_msgs/OccupancyGrid.h>
29 #include <ros/node_handle.h>
51 pose_pub_ =
rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>(
"amcl_pose", 2);
52 particlecloud_pub_ =
rosnode->advertise<geometry_msgs::PoseArray>(
"particlecloud", 2);
54 rosnode->subscribe(
"initialpose", 2, &AmclROSThread::initial_pose_received,
this);
55 map_pub_ =
rosnode->advertise<nav_msgs::OccupancyGrid>(
"map", 1,
true);
66 particlecloud_pub_.shutdown();
67 initial_pose_sub_.shutdown();
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();
98 particlecloud_pub_.publish(cloud_msg);
109 const double covariance[36])
111 geometry_msgs::PoseWithCovarianceStamped p;
113 p.header.frame_id = global_frame_id;
114 p.header.stamp = ros::Time();
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();
125 for (
int i = 0; i < 2; i++) {
126 for (
int j = 0; j < 2; j++) {
129 p.pose.covariance[6 * i + j] = covariance[6 * i + j];
132 p.pose.covariance[6 * 5 + 5] = covariance[6 * 5 + 5];
134 pose_pub_.publish(p);
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;
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();
162 msg.data.resize((
size_t)msg.info.width * msg.info.height);
164 for (
unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
165 if (map->cells[i].occ_state == +1) {
167 }
else if (map->cells[i].occ_state == -1) {
174 map_pub_.publish(msg);
178 AmclROSThread::initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg)
180 fawkes::Time msg_time(msg->header.stamp.sec, msg->header.stamp.nsec / 1000);
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};
191 std::string frame = msg->header.frame_id;
192 if (!frame.empty() && frame[0] ==
'/')
193 frame = frame.substr(1);
virtual ~AmclROSThread()
Destructor.
virtual void loop()
Code to execute in the thread.
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
AmclROSThread()
Constructor.
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.
virtual void finalize()
Finalize the thread.
virtual void init()
Initialize the thread.
void publish_pose_array(const std::string &global_frame_id, const pf_sample_set_t *set)
Publish pose array to ROS.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
unsigned int msgq_enqueue(Message *message, bool proxy=false)
Enqueue message at end of queue.
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.
LockPtr< ros::NodeHandle > rosnode
Central ROS node handle.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
A class for handling time.
Fawkes library namespace.
pf_vector_t pf_pose_mean
Mean of pose esimate.