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 
42 namespace fawkes {
43 class Mutex;
44 }
45 
46 class AmclThread;
47 
49  public fawkes::LoggingAspect,
52  public fawkes::ROSAspect
53 {
54 public:
55  AmclROSThread();
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() */
69 protected:
70  virtual void
71  run()
72  {
73  Thread::run();
74  }
75 
76 private:
77  void initial_pose_received(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
78 
79 private:
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
amcl_hyp_t
Pose hypothesis.
Definition: amcl_thread.h:51
AmclThread
Thread to perform Adaptive Monte Carlo Localization.
Definition: amcl_thread.h:77
AmclROSThread::publish_map
void publish_map(const std::string &global_frame_id, const map_t *map)
Publish map to ROS.
Definition: ros_thread.cpp:142
AmclROSThread::AmclROSThread
AmclROSThread()
Constructor.
Definition: ros_thread.cpp:39
AmclROSThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: ros_thread.h:71
AmclROSThread
Thread for ROS integration of the Adaptive Monte Carlo Localization.
Definition: ros_thread.h:53
fawkes::LocalizationInterface
LocalizationInterface Fawkes BlackBoard Interface.
Definition: LocalizationInterface.h:34
AmclROSThread::~AmclROSThread
virtual ~AmclROSThread()
Destructor.
Definition: ros_thread.cpp:44
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
fawkes
Fawkes library namespace.
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
AmclROSThread::init
virtual void init()
Initialize the thread.
Definition: ros_thread.cpp:49
fawkes::ROSAspect
Thread aspect to get access to a ROS node handle.
Definition: ros.h:39
AmclROSThread::publish_pose
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
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
AmclROSThread::finalize
virtual void finalize()
Finalize the thread.
Definition: ros_thread.cpp:61
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
AmclROSThread::loop
virtual void loop()
Code to execute in the thread.
Definition: ros_thread.cpp:72
AmclROSThread::publish_pose_array
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