From 0951b1a69b6edc0a35b51eb22cba3d6c547fb09a Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Fri, 5 Oct 2018 17:14:01 +0200
Subject: [PATCH] RosPublish can publish booleans
---
src/converter.hh | 12 ++++++++++++
src/ros_publish.cpp | 4 +++-
src/sot_to_ros.cpp | 1 +
src/sot_to_ros.hh | 25 +++++++++++++++++++++++++
4 files changed, 41 insertions(+), 1 deletion(-)
diff --git a/src/converter.hh b/src/converter.hh
index 7c953b6..9b94e52 100644
--- a/src/converter.hh
+++ b/src/converter.hh
@@ -44,6 +44,17 @@ namespace dynamicgraph
template <typename D, typename S>
void converter (D& dst, const S& src);
+ // Boolean
+ SOT_TO_ROS_IMPL(bool)
+ {
+ dst.data = src;
+ }
+
+ ROS_TO_SOT_IMPL(bool)
+ {
+ dst = src.data;
+ }
+
// Double
SOT_TO_ROS_IMPL(double)
{
@@ -216,6 +227,7 @@ namespace dynamicgraph
} \
struct e_n_d__w_i_t_h__s_e_m_i_c_o_l_o_n
+ DG_BRIDGE_MAKE_SHPTR_IMPL(bool);
DG_BRIDGE_MAKE_SHPTR_IMPL(double);
DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int);
DG_BRIDGE_MAKE_SHPTR_IMPL(Vector);
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 49bf2d2..1120ed1 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -78,7 +78,9 @@ namespace dynamicgraph
const std::string& signal = values[1].value ();
const std::string& topic = values[2].value ();
- if (type == "double")
+ if (type == "boolean")
+ entity.add<bool> (signal, topic);
+ else if (type == "double")
entity.add<double> (signal, topic);
else if (type == "unsigned")
entity.add<unsigned int> (signal, topic);
diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp
index e7a2075..87b1214 100644
--- a/src/sot_to_ros.cpp
+++ b/src/sot_to_ros.cpp
@@ -3,6 +3,7 @@
namespace dynamicgraph
{
+ const char* SotToRos<bool>::signalTypeName = "bool";
const char* SotToRos<double>::signalTypeName = "Double";
const char* SotToRos<unsigned int>::signalTypeName = "Unsigned";
const char* SotToRos<Matrix>::signalTypeName = "Matrix";
diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh
index 7163abd..02673ec 100644
--- a/src/sot_to_ros.hh
+++ b/src/sot_to_ros.hh
@@ -5,6 +5,7 @@
# include <boost/format.hpp>
+# include <std_msgs/Bool.h>
# include <std_msgs/Float64.h>
# include <std_msgs/UInt32.h>
# include "dynamic_graph_bridge_msgs/Matrix.h"
@@ -48,6 +49,30 @@ namespace dynamicgraph
template <typename SotType>
class SotToRos;
+ template <>
+ struct SotToRos<bool>
+ {
+ typedef bool sot_t;
+ typedef std_msgs::Bool ros_t;
+ typedef std_msgs::BoolConstPtr ros_const_ptr_t;
+ typedef dynamicgraph::Signal<sot_t, int> signal_t;
+ typedef dynamicgraph::SignalPtr<sot_t, int> signalIn_t;
+ typedef boost::function<sot_t& (sot_t&, int)> callback_t;
+
+ static const char* signalTypeName;
+
+ template <typename S>
+ static void setDefault(S& s)
+ {
+ s.setConstant (false);
+ }
+
+ static void setDefault(sot_t& s)
+ {
+ s = false;
+ }
+ };
+
template <>
struct SotToRos<double>
{
--
GitLab