1 |
|
|
#ifndef DYNAMIC_GRAPH_ROS_PUBLISH_HH |
2 |
|
|
#define DYNAMIC_GRAPH_ROS_PUBLISH_HH |
3 |
|
|
#include <dynamic-graph/command.h> |
4 |
|
|
#include <dynamic-graph/entity.h> |
5 |
|
|
#include <dynamic-graph/signal-time-dependent.h> |
6 |
|
|
#include <realtime_tools/realtime_publisher.h> |
7 |
|
|
#include <ros/ros.h> |
8 |
|
|
|
9 |
|
|
#include <boost/shared_ptr.hpp> |
10 |
|
|
#include <boost/thread/mutex.hpp> |
11 |
|
|
#include <boost/tuple/tuple.hpp> |
12 |
|
|
#include <fstream> |
13 |
|
|
#include <map> |
14 |
|
|
|
15 |
|
|
#include "converter.hh" |
16 |
|
|
#include "sot_to_ros.hh" |
17 |
|
|
|
18 |
|
|
namespace dynamicgraph { |
19 |
|
|
class RosPublish; |
20 |
|
|
|
21 |
|
|
namespace command { |
22 |
|
|
namespace rosPublish { |
23 |
|
|
using ::dynamicgraph::command::Command; |
24 |
|
|
using ::dynamicgraph::command::Value; |
25 |
|
|
|
26 |
|
|
#define ROS_PUBLISH_MAKE_COMMAND(CMD) \ |
27 |
|
|
class CMD : public Command { \ |
28 |
|
|
public: \ |
29 |
|
|
CMD(RosPublish& entity, const std::string& docstring); \ |
30 |
|
|
virtual Value doExecute(); \ |
31 |
|
|
} |
32 |
|
|
|
33 |
|
|
ROS_PUBLISH_MAKE_COMMAND(Add); |
34 |
|
|
|
35 |
|
|
#undef ROS_PUBLISH_MAKE_COMMAND |
36 |
|
|
|
37 |
|
|
} // namespace rosPublish |
38 |
|
|
} // end of namespace command. |
39 |
|
|
|
40 |
|
|
/// \brief Publish dynamic-graph information into ROS. |
41 |
|
|
class RosPublish : public dynamicgraph::Entity { |
42 |
|
|
DYNAMIC_GRAPH_ENTITY_DECL(); |
43 |
|
|
|
44 |
|
|
public: |
45 |
|
|
typedef boost::function<void(int)> callback_t; |
46 |
|
|
|
47 |
|
|
typedef boost::tuple<boost::shared_ptr<dynamicgraph::SignalBase<int> >, |
48 |
|
|
callback_t> |
49 |
|
|
bindedSignal_t; |
50 |
|
|
|
51 |
|
|
static const double ROS_JOINT_STATE_PUBLISHER_RATE; |
52 |
|
|
|
53 |
|
|
RosPublish(const std::string& n); |
54 |
|
|
virtual ~RosPublish(); |
55 |
|
|
|
56 |
|
|
virtual std::string getDocString() const; |
57 |
|
|
void display(std::ostream& os) const; |
58 |
|
|
|
59 |
|
|
void add(const std::string& signal, const std::string& topic); |
60 |
|
|
void rm(const std::string& signal); |
61 |
|
|
std::vector<std::string> list() const; |
62 |
|
|
void clear(); |
63 |
|
|
|
64 |
|
|
int& trigger(int&, int); |
65 |
|
|
|
66 |
|
|
template <typename T> |
67 |
|
|
void sendData( |
68 |
|
|
boost::shared_ptr< |
69 |
|
|
realtime_tools::RealtimePublisher<typename SotToRos<T>::ros_t> > |
70 |
|
|
publisher, |
71 |
|
|
boost::shared_ptr<typename SotToRos<T>::signalIn_t> signal, int time); |
72 |
|
|
|
73 |
|
|
template <typename T> |
74 |
|
|
void add(const std::string& signal, const std::string& topic); |
75 |
|
|
|
76 |
|
|
private: |
77 |
|
|
static const std::string docstring_; |
78 |
|
|
ros::NodeHandle& nh_; |
79 |
|
|
std::map<std::string, bindedSignal_t> bindedSignal_; |
80 |
|
|
dynamicgraph::SignalTimeDependent<int, int> trigger_; |
81 |
|
|
ros::Duration rate_; |
82 |
|
|
ros::Time nextPublication_; |
83 |
|
|
boost::mutex mutex_; |
84 |
|
|
std::ofstream aofs_; |
85 |
|
|
struct timespec nextPublicationRT_; |
86 |
|
|
}; |
87 |
|
|
} // end of namespace dynamicgraph. |
88 |
|
|
|
89 |
|
|
#include "ros_publish.hxx" |
90 |
|
|
#endif //! DYNAMIC_GRAPH_ROS_PUBLISH_HH |