Fawkes API Fawkes Development Version
ros_thread.h
1/***************************************************************************
2 * ros_thread.cpp - Thread to interact with ROS for amcl plugin
3 *
4 * Created: Mon Jun 22 17:46:40 2015
5 * Copyright 2012 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#ifndef _PLUGINS_AMCL_ROS_THREAD_H_
22#define _PLUGINS_AMCL_ROS_THREAD_H_
23
24#ifndef HAVE_ROS
25# error "ROS integration requires ROS support of system"
26#endif
27
28#include "amcl_thread.h"
29#include "map/map.h"
30#include "pf/pf.h"
31
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>
41
42namespace fawkes {
43class Mutex;
44}
45
46class AmclThread;
47
53{
54public:
56 virtual ~AmclROSThread();
57
58 virtual void init();
59 virtual void loop();
60 virtual void finalize();
61
62 void publish_pose_array(const std::string &global_frame_id, const pf_sample_set_t *set);
63 void publish_pose(const std::string &global_frame_id,
64 const amcl_hyp_t & amcl_hyp,
65 const double last_covariance[36]);
66 void publish_map(const std::string &global_frame_id, const map_t *map);
67
68 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
69protected:
70 virtual void
72 {
73 Thread::run();
74 }
75
76private:
77 void initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
78
79private:
80 std::string cfg_pose_ifname_;
81
83
84 ros::Publisher pose_pub_;
85 ros::Publisher particlecloud_pub_;
86 ros::Subscriber initial_pose_sub_;
87 ros::Publisher map_pub_;
88};
89
90#endif
Thread for ROS integration of the Adaptive Monte Carlo Localization.
Definition: ros_thread.h:53
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: ros_thread.h:71
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
Thread to perform Adaptive Monte Carlo Localization.
Definition: amcl_thread.h:77
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
LocalizationInterface Fawkes BlackBoard Interface.
Thread aspect to log output.
Definition: logging.h:33
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
Thread class encapsulation of pthreads.
Definition: thread.h:46
Fawkes library namespace.
Pose hypothesis.
Definition: amcl_thread.h:51