diff --git a/src/converter.hh b/src/converter.hh index 9b94e52ba5b838233fb9fcc7e730a9ee9934ecc2..968cde474788cb3679bdb1333a0afdff1549a11b 100644 --- a/src/converter.hh +++ b/src/converter.hh @@ -66,6 +66,17 @@ namespace dynamicgraph dst = src.data; } + // Int + SOT_TO_ROS_IMPL(int) + { + dst.data = src; + } + + ROS_TO_SOT_IMPL(int) + { + dst = src.data; + } + // Unsigned SOT_TO_ROS_IMPL(unsigned int) { @@ -229,6 +240,7 @@ namespace dynamicgraph DG_BRIDGE_MAKE_SHPTR_IMPL(bool); DG_BRIDGE_MAKE_SHPTR_IMPL(double); + DG_BRIDGE_MAKE_SHPTR_IMPL(int); DG_BRIDGE_MAKE_SHPTR_IMPL(unsigned int); DG_BRIDGE_MAKE_SHPTR_IMPL(Vector); DG_BRIDGE_MAKE_SHPTR_IMPL(specific::Vector3); diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp index 1120ed1b92500be59cbd7eaaa10e7a7dbcc1f5ca..8646fe540bfc659addc15de5b5f3df29f1609ce5 100644 --- a/src/ros_publish.cpp +++ b/src/ros_publish.cpp @@ -84,6 +84,8 @@ namespace dynamicgraph entity.add<double> (signal, topic); else if (type == "unsigned") entity.add<unsigned int> (signal, topic); + else if (type == "int") + entity.add<int> (signal, topic); else if (type == "matrix") entity.add<Matrix> (signal, topic); else if (type == "vector") diff --git a/src/sot_to_ros.cpp b/src/sot_to_ros.cpp index 87b121426946fc0cc2e590b127bb5b5c47783e59..abc88e3caa096fe7094628c2edc36818bcbc3bfc 100644 --- a/src/sot_to_ros.cpp +++ b/src/sot_to_ros.cpp @@ -5,6 +5,7 @@ namespace dynamicgraph const char* SotToRos<bool>::signalTypeName = "bool"; const char* SotToRos<double>::signalTypeName = "Double"; + const char* SotToRos<int>::signalTypeName = "int"; const char* SotToRos<unsigned int>::signalTypeName = "Unsigned"; const char* SotToRos<Matrix>::signalTypeName = "Matrix"; const char* SotToRos<Vector>::signalTypeName = "Vector"; diff --git a/src/sot_to_ros.hh b/src/sot_to_ros.hh index 02673ec2e1085dcaeb8ea30f3ccd5dbb054b214d..46bdf4c53380fdcd78af82d9f9bd189b2031095c 100644 --- a/src/sot_to_ros.hh +++ b/src/sot_to_ros.hh @@ -8,6 +8,7 @@ # include <std_msgs/Bool.h> # include <std_msgs/Float64.h> # include <std_msgs/UInt32.h> +# include <std_msgs/Int32.h> # include "dynamic_graph_bridge_msgs/Matrix.h" # include "dynamic_graph_bridge_msgs/Vector.h" @@ -121,6 +122,30 @@ namespace dynamicgraph } }; + template <> + struct SotToRos<int> + { + typedef int sot_t; + typedef std_msgs::Int32 ros_t; + typedef std_msgs::Int32ConstPtr 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 (0); + } + + static void setDefault(sot_t& s) + { + s = 0; + } + }; + template <> struct SotToRos<Matrix> {