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" 33 #include <core/threading/thread.h> 34 #include <aspect/configurable.h> 35 #include <aspect/blackboard.h> 36 #include <aspect/logging.h> 38 #include <interfaces/LocalizationInterface.h> 40 #include <plugins/ros/aspect/ros.h> 41 #include <ros/publisher.h> 42 #include <ros/subscriber.h> 43 #include <geometry_msgs/PoseWithCovarianceStamped.h> 67 const pf_sample_set_t*
set);
70 const double last_covariance[36]);
71 void publish_map(
const std::string &global_frame_id,
76 protected:
virtual void run() { Thread::run();}
79 void initial_pose_received(
const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
82 std::string cfg_pose_ifname_;
86 ros::Publisher pose_pub_;
87 ros::Publisher particlecloud_pub_;
88 ros::Subscriber initial_pose_sub_;
89 ros::Publisher map_pub_;
LocalizationInterface Fawkes BlackBoard Interface.
Thread aspect to access to BlackBoard.
Thread aspect to get access to a ROS node handle.
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.
Thread class encapsulation of pthreads.
virtual void run()
Stub to see name in backtrace for easier debugging.
Thread to perform Adaptive Monte Carlo Localization.
AmclROSThread()
Constructor.
Thread aspect to log output.
virtual ~AmclROSThread()
Destructor.
Thread aspect to access configuration data.
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 for ROS integration of the Adaptive Monte Carlo Localization.
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.