diff --git a/src/converter.hh b/src/converter.hh index 7c953b68221fa3f4a1700f96e1da7452adbf7371..9b94e52ba5b838233fb9fcc7e730a9ee9934ecc2 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 49bf2d22eeb60a78dbc22e2ce5dc67842f65d41c..1120ed1b92500be59cbd7eaaa10e7a7dbcc1f5ca 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 e7a20758905723210165cc3c93a49b8b5d3c44ba..87b121426946fc0cc2e590b127bb5b5c47783e59 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 7163abd3a56785cf7b9ae1b9b60f6466bd76ae1c..02673ec2e1085dcaeb8ea30f3ccd5dbb054b214d 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> {