25 #include "ros_thread.h" 27 #include <ros/node_handle.h> 28 #include <geometry_msgs/PoseArray.h> 29 #include <nav_msgs/OccupancyGrid.h> 53 rosnode->advertise<geometry_msgs::PoseWithCovarianceStamped>(
"amcl_pose", 2);
55 rosnode->advertise<geometry_msgs::PoseArray>(
"particlecloud", 2);
57 rosnode->subscribe(
"initialpose", 2,
58 &AmclROSThread::initial_pose_received,
this);
59 map_pub_ =
rosnode->advertise<nav_msgs::OccupancyGrid>(
"map", 1,
true);
72 particlecloud_pub_.shutdown();
73 initial_pose_sub_.shutdown();
91 const pf_sample_set_t*
set)
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();
108 particlecloud_pub_.publish(cloud_msg);
120 const double covariance[36])
122 geometry_msgs::PoseWithCovarianceStamped p;
124 p.header.frame_id = global_frame_id;
125 p.header.stamp = ros::Time();
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();
136 for (
int i = 0; i < 2; i++) {
137 for (
int j = 0; j < 2; j++) {
140 p.pose.covariance[6*i+j] = covariance[6 * i + j];
143 p.pose.covariance[6 * 5 + 5] = covariance[6 * 5 + 5];
145 pose_pub_.publish(p);
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;
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();
174 msg.data.resize(msg.info.width * msg.info.height);
176 for (
unsigned int i = 0; i < msg.info.width * msg.info.height; ++i) {
177 if (map->cells[i].occ_state == +1) {
179 }
else if (map->cells[i].occ_state == -1) {
186 map_pub_.publish(msg);
190 AmclROSThread::initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
193 msg->header.stamp.nsec / 1000);
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};
204 std::string frame = msg->header.frame_id;
205 if (! frame.empty() && frame[0] ==
'/') frame = frame.substr(1);
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.
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
Fawkes library namespace.
virtual void loop()
Code to execute in the thread.
A class for handling time.
Thread class encapsulation of pthreads.
Logger * logger
This is the Logger member used to access the logger.
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.
AmclROSThread()
Constructor.
Base class for exceptions in Fawkes.
SetInitialPoseMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
virtual ~AmclROSThread()
Destructor.
unsigned int msgq_enqueue(Message *message)
Enqueue message at end of queue.
virtual void init()
Initialize the thread.
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.
void publish_pose_array(const std::string &global_frame_id, const pf_sample_set_t *set)
Publish pose array to ROS.
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.
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.
virtual void close(Interface *interface)=0
Close interface.