21#ifndef _PLUGINS_AMCL_ROS_THREAD_H_
22#define _PLUGINS_AMCL_ROS_THREAD_H_
25# error "ROS integration requires ROS support of system"
28#include "amcl_thread.h"
32#include <aspect/blackboard.h>
33#include <aspect/configurable.h>
34#include <aspect/logging.h>
35#include <core/threading/thread.h>
36#include <geometry_msgs/PoseWithCovarianceStamped.h>
37#include <interfaces/LocalizationInterface.h>
38#include <plugins/ros/aspect/ros.h>
39#include <ros/publisher.h>
40#include <ros/subscriber.h>
62 void publish_pose_array(
const std::string &global_frame_id,
const pf_sample_set_t *set);
65 const double last_covariance[36]);
66 void publish_map(
const std::string &global_frame_id,
const map_t *map);
77 void initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
80 std::string cfg_pose_ifname_;
84 ros::Publisher pose_pub_;
85 ros::Publisher particlecloud_pub_;
86 ros::Subscriber initial_pose_sub_;
87 ros::Publisher map_pub_;
Thread for ROS integration of the Adaptive Monte Carlo Localization.
virtual void run()
Stub to see name in backtrace for easier debugging.
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.
Thread to perform Adaptive Monte Carlo Localization.
Thread aspect to access to BlackBoard.
Thread aspect to access configuration data.
LocalizationInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Thread aspect to get access to a ROS node handle.
Thread class encapsulation of pthreads.
Fawkes library namespace.