diff --git a/include/dynamic_graph_bridge/ros.hpp b/include/dynamic_graph_bridge/ros.hpp
index d173ffc7aefbf4686d2de5355443c01477f7a3bc..57797d22fe6eff9cad0688a307335d00329b6b37 100644
--- a/include/dynamic_graph_bridge/ros.hpp
+++ b/include/dynamic_graph_bridge/ros.hpp
@@ -15,8 +15,7 @@
#include "rclcpp/rclcpp.hpp"
#include "std_srvs/srv/empty.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
const std::string DG_ROS_NODE_NAME = "dynamic_graph";
const std::string HWC_ROS_NODE_NAME = "hardware_communication";
@@ -29,7 +28,8 @@ typedef rclcpp::executors::MultiThreadedExecutor RosExecutor;
typedef std::shared_ptr<RosExecutor> RosExecutorPtr;
// Python commands shortcuts
-typedef dynamic_graph_bridge_msgs::srv::RunPythonCommand RunPythonCommandSrvType;
+typedef dynamic_graph_bridge_msgs::srv::RunPythonCommand
+ RunPythonCommandSrvType;
typedef rclcpp::Service<RunPythonCommandSrvType>::SharedPtr
RunPythonCommandServerPtr;
typedef rclcpp::Client<RunPythonCommandSrvType>::SharedPtr
diff --git a/include/dynamic_graph_bridge/ros_parameter.hpp b/include/dynamic_graph_bridge/ros_parameter.hpp
index 688850ce0353159a4c60f1f75cf215e4b07362d8..fda58a3ab0cba0130dcfe04f1b9f5432d69fddfa 100644
--- a/include/dynamic_graph_bridge/ros_parameter.hpp
+++ b/include/dynamic_graph_bridge/ros_parameter.hpp
@@ -4,7 +4,7 @@
#include "rclcpp/node.hpp"
namespace dynamicgraph {
- bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh);
+bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh);
}
#endif /* _ROS_DYNAMIC_GRAPH_PARAMETER_ */
diff --git a/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp b/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
index a5b5459e12cda2f064d5c8071fc882abb66a0b9e..2c44d6016f7da198982f783351e0ffddf24bac22 100644
--- a/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
+++ b/include/dynamic_graph_bridge/ros_python_interpreter_client.hpp
@@ -12,141 +12,124 @@
#include "dynamic_graph_bridge/ros.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/**
* @brief Client of the RosPythonInterpreterServer through rosservices.
*/
-class RosPythonInterpreterClient
-{
-public:
- /**
- * @brief Construct a new RosPythonInterpreterClient object.
- */
- RosPythonInterpreterClient();
-
- /**
- * @brief Destroy the RosPythonInterpreterClient object.
- */
- ~RosPythonInterpreterClient()
- {
+class RosPythonInterpreterClient {
+ public:
+ /**
+ * @brief Construct a new RosPythonInterpreterClient object.
+ */
+ RosPythonInterpreterClient();
+
+ /**
+ * @brief Destroy the RosPythonInterpreterClient object.
+ */
+ ~RosPythonInterpreterClient() {}
+
+ /**
+ * @brief Call the rosservice of the current running DynamicGraphManager.
+ *
+ * @param code_string To be executed in the embeded python interpreter.
+ * @return std::string
+ */
+ std::string run_python_command(const std::string& code_string);
+
+ /**
+ * @brief Call the rosservice of the current running DynamicGraphManager.
+ *
+ * @param filename
+ */
+ std::string run_python_script(const std::string& filename);
+
+ private:
+ /**
+ * @brief Connect to a designated service.
+ *
+ * @tparam RosServiceType
+ * @param service_name
+ * @param client
+ * @param timeout
+ */
+ template <typename RosServiceType>
+ typename rclcpp::Client<RosServiceType>::SharedPtr connect_to_rosservice(
+ const std::string& service_name, const DurationSec& timeout) {
+ typename rclcpp::Client<RosServiceType>::SharedPtr client;
+ try {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Waiting for service %s ...", service_name.c_str());
+ // let use wait for the existance of the services
+ client = ros_node_->create_client<RosServiceType>(service_name.c_str());
+ if (client->wait_for_service(timeout)) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Successfully connected to %s", service_name.c_str());
+ } else {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Failed to connect to %s", service_name.c_str());
+ }
+ } catch (...) {
+ throw std::runtime_error(service_name + " not available.");
}
-
- /**
- * @brief Call the rosservice of the current running DynamicGraphManager.
- *
- * @param code_string To be executed in the embeded python interpreter.
- * @return std::string
- */
- std::string run_python_command(const std::string& code_string);
-
- /**
- * @brief Call the rosservice of the current running DynamicGraphManager.
- *
- * @param filename
- */
- std::string run_python_script(const std::string& filename);
-
-private:
- /**
- * @brief Connect to a designated service.
- *
- * @tparam RosServiceType
- * @param service_name
- * @param client
- * @param timeout
- */
- template <typename RosServiceType>
- typename rclcpp::Client<RosServiceType>::SharedPtr connect_to_rosservice(
- const std::string& service_name, const DurationSec& timeout)
- {
- typename rclcpp::Client<RosServiceType>::SharedPtr client;
- try
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Waiting for service %s ...",
- service_name.c_str());
- // let use wait for the existance of the services
- client =
- ros_node_->create_client<RosServiceType>(service_name.c_str());
- if (client->wait_for_service(timeout))
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Successfully connected to %s",
- service_name.c_str());
- }
- else
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Failed to connect to %s",
- service_name.c_str());
- }
- }
- catch (...)
- {
- throw std::runtime_error(service_name + " not available.");
- }
- return client;
- }
-
- /**
- * @brief Connects to the RunCommand rosservice.
- *
- * @param timeout [in]
- */
- void connect_to_rosservice_run_python_command(
- const DurationSec& timeout = DurationSec(-1))
- {
- command_client_ = connect_to_rosservice<RunPythonCommandSrvType>(
- run_command_service_name_, timeout);
- }
-
- /**
- * @brief Connects to the RunPythonFile rosservice.
- *
- * @param timeout [in]
- */
- void connect_to_rosservice_run_python_script(
- const DurationSec& timeout = DurationSec(-1))
- {
- script_client_ = connect_to_rosservice<RunPythonFileSrvType>(
- run_script_service_name_, timeout);
- }
-
-private:
- /**
- * @brief Name of the local ros_node;
- */
- std::string ros_node_name_;
-
- /** @brief Handle for manipulating ROS objects. */
- RosNodePtr ros_node_;
-
- /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
- * rosservice for running a python command. */
- std::string run_command_service_name_;
-
- /** @brief Rosservice to run a python command.
- * @see run_command_service_name_ */
- RunPythonCommandClientPtr command_client_;
-
- /** @brief Input of the rosservice. */
- RunPythonCommandRequestPtr run_command_request_;
-
- /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
- * rosservice for running a python script. */
- std::string run_script_service_name_;
-
- /** @brief Rosservice to run a python script.
- * @see run_command_service_name_ */
- RunPythonFileClientPtr script_client_;
-
- /** @brief Input of the rosservice. */
- RunPythonFileRequestPtr run_file_request_;
-
- /** @brief timeout used during the connection to the rosservices in
- * seconds. */
- DurationSec timeout_connection_s_;
+ return client;
+ }
+
+ /**
+ * @brief Connects to the RunCommand rosservice.
+ *
+ * @param timeout [in]
+ */
+ void connect_to_rosservice_run_python_command(
+ const DurationSec& timeout = DurationSec(-1)) {
+ command_client_ = connect_to_rosservice<RunPythonCommandSrvType>(
+ run_command_service_name_, timeout);
+ }
+
+ /**
+ * @brief Connects to the RunPythonFile rosservice.
+ *
+ * @param timeout [in]
+ */
+ void connect_to_rosservice_run_python_script(
+ const DurationSec& timeout = DurationSec(-1)) {
+ script_client_ = connect_to_rosservice<RunPythonFileSrvType>(
+ run_script_service_name_, timeout);
+ }
+
+ private:
+ /**
+ * @brief Name of the local ros_node;
+ */
+ std::string ros_node_name_;
+
+ /** @brief Handle for manipulating ROS objects. */
+ RosNodePtr ros_node_;
+
+ /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
+ * rosservice for running a python command. */
+ std::string run_command_service_name_;
+
+ /** @brief Rosservice to run a python command.
+ * @see run_command_service_name_ */
+ RunPythonCommandClientPtr command_client_;
+
+ /** @brief Input of the rosservice. */
+ RunPythonCommandRequestPtr run_command_request_;
+
+ /** @brief Name of the DynamicGraphManager RosPythonInterpreterServer
+ * rosservice for running a python script. */
+ std::string run_script_service_name_;
+
+ /** @brief Rosservice to run a python script.
+ * @see run_command_service_name_ */
+ RunPythonFileClientPtr script_client_;
+
+ /** @brief Input of the rosservice. */
+ RunPythonFileRequestPtr run_file_request_;
+
+ /** @brief timeout used during the connection to the rosservices in
+ * seconds. */
+ DurationSec timeout_connection_s_;
};
} // namespace dynamic_graph_bridge
diff --git a/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp b/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
index c9ba3ddc16f12b491566d57d5953e79d95023e03..48f6f851ce020f7838f916e01335d0ed0a468423 100644
--- a/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
+++ b/include/dynamic_graph_bridge/ros_python_interpreter_server.hpp
@@ -16,137 +16,131 @@
#include "dynamic-graph/python/interpreter.hh"
#include "dynamic_graph_bridge/ros.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/// \brief This class wraps the implementation of the runCommand
/// service.
///
/// This takes as input a ROS node handle and do not handle the
/// callback so that the service behavior can be controlled from
/// the outside.
-class RosPythonInterpreterServer
-{
-public:
- /**
- * @brief run_python_command_callback_t define a std::function to be used
- * as callback to the rclcpp::service.
- *
- * The first argument of "runCommandCallback"
- * (const std::string & command) is bound to
- * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
- * And the second argument (std::string &result) is bound to
- * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
- */
- typedef std::function<void(RunPythonCommandRequestPtr,
- RunPythonCommandResponsePtr)>
- run_python_command_callback_t;
+class RosPythonInterpreterServer {
+ public:
+ /**
+ * @brief run_python_command_callback_t define a std::function to be used
+ * as callback to the rclcpp::service.
+ *
+ * The first argument of "runCommandCallback"
+ * (const std::string & command) is bound to
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
+ * And the second argument (std::string &result) is bound to
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
+ */
+ typedef std::function<void(RunPythonCommandRequestPtr,
+ RunPythonCommandResponsePtr)>
+ run_python_command_callback_t;
- /**
- * @brief run_python_file_callback_t define a std::function to be used as
- * callback to the rclcpp::service.
- *
- * The first argument of "runPythonFileCallback"
- * (std::string ifilename) is bound to
- * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
- * And a fake second argument is simulated in
- * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
- */
- typedef std::function<void(RunPythonFileRequestPtr,
- RunPythonFileResponsePtr)>
- run_python_file_callback_t;
+ /**
+ * @brief run_python_file_callback_t define a std::function to be used as
+ * callback to the rclcpp::service.
+ *
+ * The first argument of "runPythonFileCallback"
+ * (std::string ifilename) is bound to
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Request).
+ * And a fake second argument is simulated in
+ * (dynamic_graph_bridge::srv::RunPythonCommand::Response)
+ */
+ typedef std::function<void(RunPythonFileRequestPtr, RunPythonFileResponsePtr)>
+ run_python_file_callback_t;
- /**
- * @brief RosPythonInterpreterServer is the unique constructor of the class
- * @param node_handle is the RosNode used to advertize the
- * rclcpp::services
- */
- explicit RosPythonInterpreterServer();
+ /**
+ * @brief RosPythonInterpreterServer is the unique constructor of the class
+ * @param node_handle is the RosNode used to advertize the
+ * rclcpp::services
+ */
+ explicit RosPythonInterpreterServer();
- /**
- * @brief ~RosPythonInterpreterServer is the default destructor of the class
- */
- ~RosPythonInterpreterServer();
+ /**
+ * @brief ~RosPythonInterpreterServer is the default destructor of the class
+ */
+ ~RosPythonInterpreterServer();
- /**
- * @brief run_python_command used the python interpreter to execute the
- * input command
- * @param[in] command is the user input string command.
- * @param[out] result is the numerical result of the operation done.
- * @param[out] out is the string representation of the results.
- * @param[out] err is the output error string from python.
- */
- void run_python_command(const std::string& command,
- std::string& result,
- std::string& out,
- std::string& err);
+ /**
+ * @brief run_python_command used the python interpreter to execute the
+ * input command
+ * @param[in] command is the user input string command.
+ * @param[out] result is the numerical result of the operation done.
+ * @param[out] out is the string representation of the results.
+ * @param[out] err is the output error string from python.
+ */
+ void run_python_command(const std::string& command, std::string& result,
+ std::string& out, std::string& err);
- /**
- * @brief run_python_file executes the input scripts in the python
- * interpreter
- * @param ifilename is the path to the script to execute
- */
- void run_python_file(const std::string ifilename,
- std::string& result);
+ /**
+ * @brief run_python_file executes the input scripts in the python
+ * interpreter
+ * @param ifilename is the path to the script to execute
+ */
+ void run_python_file(const std::string ifilename, std::string& result);
- /**
- * @brief start_ros_service advertize the "run_python_command" and
- * "run_python_scripts" ros services
- */
- void start_ros_service();
+ /**
+ * @brief start_ros_service advertize the "run_python_command" and
+ * "run_python_scripts" ros services
+ */
+ void start_ros_service();
-protected:
- /**
- * @brief runCommandCallback is the "run_python_command" ros service
- * callback function.
- * @param req is the request. it is defined as a string in the
- * RunCommand.msg
- * @param res is the request. it is defined as a string in the
- * RunCommand.msg
- * @return true
- */
- void runCommandCallback(RunPythonCommandRequestPtr req,
- RunPythonCommandResponsePtr res);
+ protected:
+ /**
+ * @brief runCommandCallback is the "run_python_command" ros service
+ * callback function.
+ * @param req is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @param res is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @return true
+ */
+ void runCommandCallback(RunPythonCommandRequestPtr req,
+ RunPythonCommandResponsePtr res);
- /**
- * @brief runCommandCallback is the "run_python_script" ros service callback
- * function.
- * @param req is the request. it is defined as a string in the
- * RunCommand.msg
- * @param res is the request. it is defined as a string in the
- * RunCommand.msg
- * @return true
- */
- void runPythonFileCallback(RunPythonFileRequestPtr req,
- RunPythonFileResponsePtr res);
+ /**
+ * @brief runCommandCallback is the "run_python_script" ros service callback
+ * function.
+ * @param req is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @param res is the request. it is defined as a string in the
+ * RunCommand.msg
+ * @return true
+ */
+ void runPythonFileCallback(RunPythonFileRequestPtr req,
+ RunPythonFileResponsePtr res);
-private:
- /**
- * @brief interpreter_ is the python interpreter itself
- */
- dynamicgraph::python::Interpreter interpreter_;
- /**
- * @brief ros_node_ is reference to the RosNode used to
- * advertize the rclcpp::services
- */
- RosNodePtr ros_node_;
- /**
- * @brief run_python_command_srv_ is the "run_python_command"
- * rclcpp::service c++ object
- *
- * This kind of ros object require *NOT* to be destroyed. otherwize the
- * rclcpp::service is cancelled. This is the reason why this object is an
- * attribute of the class
- */
- RunPythonCommandServerPtr run_python_command_srv_;
- /**
- * @brief run_python_file_srv_ is the "run_python_script" rclcpp::service
- * c++ object
- *
- * This kind of ros object require *NOT* to be destroyed. otherwize the
- * rclcpp::service is cancelled. This is the reason why this object is an
- * attribute of the class
- */
- RunPythonFileServerPtr run_python_file_srv_;
+ private:
+ /**
+ * @brief interpreter_ is the python interpreter itself
+ */
+ dynamicgraph::python::Interpreter interpreter_;
+ /**
+ * @brief ros_node_ is reference to the RosNode used to
+ * advertize the rclcpp::services
+ */
+ RosNodePtr ros_node_;
+ /**
+ * @brief run_python_command_srv_ is the "run_python_command"
+ * rclcpp::service c++ object
+ *
+ * This kind of ros object require *NOT* to be destroyed. otherwize the
+ * rclcpp::service is cancelled. This is the reason why this object is an
+ * attribute of the class
+ */
+ RunPythonCommandServerPtr run_python_command_srv_;
+ /**
+ * @brief run_python_file_srv_ is the "run_python_script" rclcpp::service
+ * c++ object
+ *
+ * This kind of ros object require *NOT* to be destroyed. otherwize the
+ * rclcpp::service is cancelled. This is the reason why this object is an
+ * attribute of the class
+ */
+ RunPythonFileServerPtr run_python_file_srv_;
};
} // namespace dynamic_graph_bridge
diff --git a/include/dynamic_graph_bridge/sot_loader.hh b/include/dynamic_graph_bridge/sot_loader.hh
index c3054764a0a276443ebd999c7d2481f9aafef69b..d2f3c68ab3eede858085f504da6683d4b86ee697 100644
--- a/include/dynamic_graph_bridge/sot_loader.hh
+++ b/include/dynamic_graph_bridge/sot_loader.hh
@@ -22,20 +22,22 @@
#include <pinocchio/fwd.hpp>
// Boost includes
-#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/program_options.hpp>
#include <thread>
// ROS includes
//#include "ros/ros.h"
-#include "std_srvs/srv/empty.hpp"
-#include <sensor_msgs/msg/joint_state.hpp>
#include <tf2_ros/transform_broadcaster.h>
+#include <sensor_msgs/msg/joint_state.hpp>
+
+#include "std_srvs/srv/empty.hpp"
+
// Sot Framework includes
-#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
// Dynamic-graph-bridge includes.
#include <dynamic_graph_bridge/sot_loader_basic.hh>
@@ -100,7 +102,8 @@ class SotLoader : public SotLoaderBasic {
// \brief Prepare the SoT framework.
void setup();
- // \brief Method for the thread implementing the starting and stopping part of dynamic_graph
+ // \brief Method for the thread implementing the starting and stopping part of
+ // dynamic_graph
void workThreadLoader();
// \brief Join the thread.
diff --git a/include/dynamic_graph_bridge/sot_loader_basic.hh b/include/dynamic_graph_bridge/sot_loader_basic.hh
index a1f187170f93f33f646cc077e51c69c30ea08422..9f1fd2db14b96c29ff26d87deddec29974e0c0ec 100644
--- a/include/dynamic_graph_bridge/sot_loader_basic.hh
+++ b/include/dynamic_graph_bridge/sot_loader_basic.hh
@@ -22,18 +22,20 @@
#include <pinocchio/fwd.hpp>
// Boost includes
-#include <boost/program_options.hpp>
#include <boost/foreach.hpp>
#include <boost/format.hpp>
+#include <boost/program_options.hpp>
// ROS includes
#include <rclcpp/rclcpp.hpp>
-#include "std_srvs/srv/empty.hpp"
#include <sensor_msgs/msg/joint_state.hpp>
+#include "std_srvs/srv/empty.hpp"
+
// Sot Framework includes
-#include <sot/core/debug.hh>
#include <sot/core/abstract-sot-external-interface.hh>
+#include <sot/core/debug.hh>
+
#include "dynamic_graph_bridge/ros.hpp"
namespace po = boost::program_options;
@@ -54,7 +56,7 @@ class SotLoaderBasic {
void* sotRobotControllerLibrary_;
/// \brief Map between SoT state vector and some joint_state_links
- //XmlRpc::XmlRpcValue stateVectorMap_;
+ // XmlRpc::XmlRpcValue stateVectorMap_;
/// \brief List of parallel joints from the state vector.
typedef std::vector<int> parallel_joints_to_state_vector_t;
@@ -84,7 +86,6 @@ class SotLoaderBasic {
// Ordered list of joint names describing the robot state.
std::vector<std::string> stateVectorMap_;
-
public:
SotLoaderBasic();
virtual ~SotLoaderBasic();
diff --git a/src/dg_ros_mapping.cpp b/src/dg_ros_mapping.cpp
index 0f85f6c9362ed1094a6ad30a4e74b23ac1f3b290..5316817f5fb63884a8b6ac10c10ad951a0037853 100644
--- a/src/dg_ros_mapping.cpp
+++ b/src/dg_ros_mapping.cpp
@@ -9,25 +9,24 @@
#include "dg_ros_mapping.hpp"
-namespace dynamic_graph_bridge
-{
-#define DG_TO_ROS_IMPL(DgRosMappingType) \
- template <> \
- void DgRosMappingType::dg_to_ros(const DgRosMappingType::dg_t& dg_src, \
- DgRosMappingType::ros_t& ros_dst)
+namespace dynamic_graph_bridge {
+#define DG_TO_ROS_IMPL(DgRosMappingType) \
+ template <> \
+ void DgRosMappingType::dg_to_ros(const DgRosMappingType::dg_t& dg_src, \
+ DgRosMappingType::ros_t& ros_dst)
-#define ROS_TO_DG_IMPL(DgRosMappingType) \
- template <> \
- void DgRosMappingType::ros_to_dg(const DgRosMappingType::ros_t& ros_src, \
- DgRosMappingType::dg_t& dg_dst)
+#define ROS_TO_DG_IMPL(DgRosMappingType) \
+ template <> \
+ void DgRosMappingType::ros_to_dg(const DgRosMappingType::ros_t& ros_src, \
+ DgRosMappingType::dg_t& dg_dst)
#define DG_DEFAULT_VALUE(DgRosMappingType) \
- template <> \
- const DgRosMappingType::dg_t DgRosMappingType::default_value
+ template <> \
+ const DgRosMappingType::dg_t DgRosMappingType::default_value
#define DG_SIGNAL_TYPE_NAME(DgRosMappingType) \
- template <> \
- const std::string DgRosMappingType::signal_type_name
+ template <> \
+ const std::string DgRosMappingType::signal_type_name
/*
* Create the list of available types for error and documentation.
@@ -42,14 +41,8 @@ const std::vector<std::string> DgRosTypes::type_list =
typedef DgRosMapping<std_msgs::msg::Float64, double> DgRosMappingFloat;
DG_SIGNAL_TYPE_NAME(DgRosMappingFloat) = DgRosTypes::get_double();
DG_DEFAULT_VALUE(DgRosMappingFloat) = 0.0;
-DG_TO_ROS_IMPL(DgRosMappingFloat)
-{
- ros_dst.data = dg_src;
-}
-ROS_TO_DG_IMPL(DgRosMappingFloat)
-{
- dg_dst = ros_src.data;
-}
+DG_TO_ROS_IMPL(DgRosMappingFloat) { ros_dst.data = dg_src; }
+ROS_TO_DG_IMPL(DgRosMappingFloat) { dg_dst = ros_src.data; }
/*
* Unsigned int
@@ -59,14 +52,8 @@ typedef DgRosMapping<std_msgs::msg::UInt32, unsigned int>
DgRosMappingUnsignedInt;
DG_SIGNAL_TYPE_NAME(DgRosMappingUnsignedInt) = DgRosTypes::get_unsigned();
DG_DEFAULT_VALUE(DgRosMappingUnsignedInt) = 0.0;
-DG_TO_ROS_IMPL(DgRosMappingUnsignedInt)
-{
- ros_dst.data = dg_src;
-}
-ROS_TO_DG_IMPL(DgRosMappingUnsignedInt)
-{
- dg_dst = ros_src.data;
-}
+DG_TO_ROS_IMPL(DgRosMappingUnsignedInt) { ros_dst.data = dg_src; }
+ROS_TO_DG_IMPL(DgRosMappingUnsignedInt) { dg_dst = ros_src.data; }
/*
* Matrix, DG and Ros matrix storage orders are row major.
@@ -75,34 +62,28 @@ ROS_TO_DG_IMPL(DgRosMappingUnsignedInt)
typedef DgRosMapping<RosMatrix, DgMatrix> DgRosMappingMatrix;
DG_SIGNAL_TYPE_NAME(DgRosMappingMatrix) = DgRosTypes::get_matrix();
DG_DEFAULT_VALUE(DgRosMappingMatrix) = DgMatrix::Zero(0, 0);
-DG_TO_ROS_IMPL(DgRosMappingMatrix)
-{
- // For simplicity and easiness of maintenance we implement a slower version
- // of the copy here.
- ros_dst.width = static_cast<unsigned int>(dg_src.rows());
- ros_dst.data.resize(dg_src.size());
- for (int row = 0; row < dg_src.rows(); ++row)
- {
- for (int col = 0; col < dg_src.cols(); ++col)
- {
- ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
- }
+DG_TO_ROS_IMPL(DgRosMappingMatrix) {
+ // For simplicity and easiness of maintenance we implement a slower version
+ // of the copy here.
+ ros_dst.width = static_cast<unsigned int>(dg_src.rows());
+ ros_dst.data.resize(dg_src.size());
+ for (int row = 0; row < dg_src.rows(); ++row) {
+ for (int col = 0; col < dg_src.cols(); ++col) {
+ ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
}
-}
-ROS_TO_DG_IMPL(DgRosMappingMatrix)
-{
- // For simplicity and easiness of maintenance we implement a slower version
- // of the copy here.
- int rows = ros_src.width;
- int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
- dg_dst.resize(rows, cols);
- for (int row = 0; row < dg_dst.rows(); ++row)
- {
- for (int col = 0; col < dg_dst.cols(); ++col)
- {
- dg_dst(row, col) = ros_src.data[row * rows + col];
- }
+ }
+}
+ROS_TO_DG_IMPL(DgRosMappingMatrix) {
+ // For simplicity and easiness of maintenance we implement a slower version
+ // of the copy here.
+ int rows = ros_src.width;
+ int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
+ dg_dst.resize(rows, cols);
+ for (int row = 0; row < dg_dst.rows(); ++row) {
+ for (int col = 0; col < dg_dst.cols(); ++col) {
+ dg_dst(row, col) = ros_src.data[row * rows + col];
}
+ }
}
/*
@@ -111,21 +92,17 @@ ROS_TO_DG_IMPL(DgRosMappingMatrix)
typedef DgRosMapping<RosVector, DgVector> DgRosMappingVector;
DG_SIGNAL_TYPE_NAME(DgRosMappingVector) = DgRosTypes::get_vector();
DG_DEFAULT_VALUE(DgRosMappingVector) = DgVector::Zero(0);
-DG_TO_ROS_IMPL(DgRosMappingVector)
-{
- ros_dst.data.resize(static_cast<unsigned int>(dg_src.size()));
- for (unsigned int i = 0; i < dg_src.size(); ++i)
- {
- ros_dst.data[i] = dg_src[i];
- }
+DG_TO_ROS_IMPL(DgRosMappingVector) {
+ ros_dst.data.resize(static_cast<unsigned int>(dg_src.size()));
+ for (unsigned int i = 0; i < dg_src.size(); ++i) {
+ ros_dst.data[i] = dg_src[i];
+ }
}
-ROS_TO_DG_IMPL(DgRosMappingVector)
-{
- dg_dst.resize(static_cast<int>(ros_src.data.size()));
- for (unsigned int i = 0; i < dg_dst.size(); ++i)
- {
- dg_dst[i] = ros_src.data[i];
- }
+ROS_TO_DG_IMPL(DgRosMappingVector) {
+ dg_dst.resize(static_cast<int>(ros_src.data.size()));
+ for (unsigned int i = 0; i < dg_dst.size(); ++i) {
+ dg_dst[i] = ros_src.data[i];
+ }
}
/*
@@ -134,27 +111,22 @@ ROS_TO_DG_IMPL(DgRosMappingVector)
typedef DgRosMapping<geometry_msgs::msg::Vector3, DgVector> DgRosMappingVector3;
DG_SIGNAL_TYPE_NAME(DgRosMappingVector3) = DgRosTypes::get_vector3();
DG_DEFAULT_VALUE(DgRosMappingVector3) = DgVector::Zero(3);
-DG_TO_ROS_IMPL(DgRosMappingVector3)
-{
- if (dg_src.size() > 0)
- {
- ros_dst.x = dg_src(0);
- if (dg_src.size() > 1)
- {
- ros_dst.y = dg_src(1);
- if (dg_src.size() > 2)
- {
- ros_dst.z = dg_src(2);
- }
- }
+DG_TO_ROS_IMPL(DgRosMappingVector3) {
+ if (dg_src.size() > 0) {
+ ros_dst.x = dg_src(0);
+ if (dg_src.size() > 1) {
+ ros_dst.y = dg_src(1);
+ if (dg_src.size() > 2) {
+ ros_dst.z = dg_src(2);
+ }
}
+ }
}
-ROS_TO_DG_IMPL(DgRosMappingVector3)
-{
- dg_dst.resize(3);
- dg_dst[0] = ros_src.x;
- dg_dst[1] = ros_src.y;
- dg_dst[2] = ros_src.z;
+ROS_TO_DG_IMPL(DgRosMappingVector3) {
+ dg_dst.resize(3);
+ dg_dst[0] = ros_src.x;
+ dg_dst[1] = ros_src.y;
+ dg_dst[2] = ros_src.z;
}
/*
@@ -166,29 +138,25 @@ typedef DgRosMapping<geometry_msgs::msg::Transform, MatrixHomogeneous>
DG_SIGNAL_TYPE_NAME(DgRosMappingMatrixHomo) =
DgRosTypes::get_matrix_homogeneous();
DG_DEFAULT_VALUE(DgRosMappingMatrixHomo) = MatrixHomogeneous::Identity();
-DG_TO_ROS_IMPL(DgRosMappingMatrixHomo)
-{
- ros_dst.translation.x = dg_src.translation()(0);
- ros_dst.translation.y = dg_src.translation()(1);
- ros_dst.translation.z = dg_src.translation()(2);
+DG_TO_ROS_IMPL(DgRosMappingMatrixHomo) {
+ ros_dst.translation.x = dg_src.translation()(0);
+ ros_dst.translation.y = dg_src.translation()(1);
+ ros_dst.translation.z = dg_src.translation()(2);
- VectorQuaternion q(dg_src.linear());
- ros_dst.rotation.x = q.x();
- ros_dst.rotation.y = q.y();
- ros_dst.rotation.z = q.z();
- ros_dst.rotation.w = q.w();
+ VectorQuaternion q(dg_src.linear());
+ ros_dst.rotation.x = q.x();
+ ros_dst.rotation.y = q.y();
+ ros_dst.rotation.z = q.z();
+ ros_dst.rotation.w = q.w();
}
-ROS_TO_DG_IMPL(DgRosMappingMatrixHomo)
-{
- dg_dst.translation()(0) = ros_src.translation.x;
- dg_dst.translation()(1) = ros_src.translation.y;
- dg_dst.translation()(2) = ros_src.translation.z;
+ROS_TO_DG_IMPL(DgRosMappingMatrixHomo) {
+ dg_dst.translation()(0) = ros_src.translation.x;
+ dg_dst.translation()(1) = ros_src.translation.y;
+ dg_dst.translation()(2) = ros_src.translation.z;
- VectorQuaternion q(ros_src.rotation.w,
- ros_src.rotation.x,
- ros_src.rotation.y,
- ros_src.rotation.z);
- dg_dst.linear() = q.matrix();
+ VectorQuaternion q(ros_src.rotation.w, ros_src.rotation.x, ros_src.rotation.y,
+ ros_src.rotation.z);
+ dg_dst.linear() = q.matrix();
}
/*
@@ -198,28 +166,25 @@ ROS_TO_DG_IMPL(DgRosMappingMatrixHomo)
typedef DgRosMapping<geometry_msgs::msg::Twist, DgVector> DgRosMappingTwist;
DG_SIGNAL_TYPE_NAME(DgRosMappingTwist) = DgRosTypes::get_twist();
DG_DEFAULT_VALUE(DgRosMappingTwist) = DgVector::Zero(6);
-DG_TO_ROS_IMPL(DgRosMappingTwist)
-{
- if (dg_src.size() != 6)
- {
- throw std::runtime_error("failed to convert invalid twist");
- }
- ros_dst.linear.x = dg_src(0);
- ros_dst.linear.y = dg_src(1);
- ros_dst.linear.z = dg_src(2);
- ros_dst.angular.x = dg_src(3);
- ros_dst.angular.y = dg_src(4);
- ros_dst.angular.z = dg_src(5);
-}
-ROS_TO_DG_IMPL(DgRosMappingTwist)
-{
- dg_dst.resize(6);
- dg_dst(0) = ros_src.linear.x;
- dg_dst(1) = ros_src.linear.y;
- dg_dst(2) = ros_src.linear.z;
- dg_dst(3) = ros_src.angular.x;
- dg_dst(4) = ros_src.angular.y;
- dg_dst(5) = ros_src.angular.z;
+DG_TO_ROS_IMPL(DgRosMappingTwist) {
+ if (dg_src.size() != 6) {
+ throw std::runtime_error("failed to convert invalid twist");
+ }
+ ros_dst.linear.x = dg_src(0);
+ ros_dst.linear.y = dg_src(1);
+ ros_dst.linear.z = dg_src(2);
+ ros_dst.angular.x = dg_src(3);
+ ros_dst.angular.y = dg_src(4);
+ ros_dst.angular.z = dg_src(5);
+}
+ROS_TO_DG_IMPL(DgRosMappingTwist) {
+ dg_dst.resize(6);
+ dg_dst(0) = ros_src.linear.x;
+ dg_dst(1) = ros_src.linear.y;
+ dg_dst(2) = ros_src.linear.z;
+ dg_dst(3) = ros_src.angular.x;
+ dg_dst(4) = ros_src.angular.y;
+ dg_dst(5) = ros_src.angular.z;
}
/*
@@ -230,14 +195,12 @@ typedef DgRosMapping<geometry_msgs::msg::Vector3Stamped, DgVector>
DG_SIGNAL_TYPE_NAME(DgRosMappingVector3Stamped) =
DgRosTypes::get_vector3_stamped();
DG_DEFAULT_VALUE(DgRosMappingVector3Stamped) = DgVector::Zero(3);
-DG_TO_ROS_IMPL(DgRosMappingVector3Stamped)
-{
- make_header(ros_dst.header);
- DgRosMappingVector3::dg_to_ros(dg_src, ros_dst.vector);
+DG_TO_ROS_IMPL(DgRosMappingVector3Stamped) {
+ make_header(ros_dst.header);
+ DgRosMappingVector3::dg_to_ros(dg_src, ros_dst.vector);
}
-ROS_TO_DG_IMPL(DgRosMappingVector3Stamped)
-{
- DgRosMappingVector3::ros_to_dg(ros_src.vector, dg_dst);
+ROS_TO_DG_IMPL(DgRosMappingVector3Stamped) {
+ DgRosMappingVector3::ros_to_dg(ros_src.vector, dg_dst);
}
/*
@@ -249,14 +212,12 @@ typedef DgRosMapping<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>
DG_SIGNAL_TYPE_NAME(DgRosMappingMatrixHomoStamped) =
DgRosTypes::get_matrix_homogeneous_stamped();
DG_DEFAULT_VALUE(DgRosMappingMatrixHomoStamped) = MatrixHomogeneous::Identity();
-DG_TO_ROS_IMPL(DgRosMappingMatrixHomoStamped)
-{
- make_header(ros_dst.header);
- DgRosMappingMatrixHomo::dg_to_ros(dg_src, ros_dst.transform);
+DG_TO_ROS_IMPL(DgRosMappingMatrixHomoStamped) {
+ make_header(ros_dst.header);
+ DgRosMappingMatrixHomo::dg_to_ros(dg_src, ros_dst.transform);
}
-ROS_TO_DG_IMPL(DgRosMappingMatrixHomoStamped)
-{
- DgRosMappingMatrixHomo::ros_to_dg(ros_src.transform, dg_dst);
+ROS_TO_DG_IMPL(DgRosMappingMatrixHomoStamped) {
+ DgRosMappingMatrixHomo::ros_to_dg(ros_src.transform, dg_dst);
}
/*
@@ -267,14 +228,12 @@ typedef DgRosMapping<geometry_msgs::msg::TwistStamped, DgVector>
DgRosMappingTwistStamped;
DG_SIGNAL_TYPE_NAME(DgRosMappingTwistStamped) = DgRosTypes::get_twist_stamped();
DG_DEFAULT_VALUE(DgRosMappingTwistStamped) = DgVector::Zero(6);
-DG_TO_ROS_IMPL(DgRosMappingTwistStamped)
-{
- make_header(ros_dst.header);
- DgRosMappingTwist::dg_to_ros(dg_src, ros_dst.twist);
+DG_TO_ROS_IMPL(DgRosMappingTwistStamped) {
+ make_header(ros_dst.header);
+ DgRosMappingTwist::dg_to_ros(dg_src, ros_dst.twist);
}
-ROS_TO_DG_IMPL(DgRosMappingTwistStamped)
-{
- DgRosMappingTwist::ros_to_dg(ros_src.twist, dg_dst);
+ROS_TO_DG_IMPL(DgRosMappingTwistStamped) {
+ DgRosMappingTwist::ros_to_dg(ros_src.twist, dg_dst);
}
} // namespace dynamic_graph_bridge
diff --git a/src/dg_ros_mapping.hpp b/src/dg_ros_mapping.hpp
index 5d5d443be6353fa234ddc858961410e82251df1a..98e639b4e2d8062ccb8e7d4967f63f34d06c0435 100644
--- a/src/dg_ros_mapping.hpp
+++ b/src/dg_ros_mapping.hpp
@@ -36,8 +36,7 @@
#include "dynamic_graph_bridge_msgs/msg/matrix.hpp"
#include "dynamic_graph_bridge_msgs/msg/vector.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/** @brief Conventient renaming for ease of implementation. */
typedef dynamicgraph::Vector DgVector;
/** @brief Conventient renaming for ease of implementation. */
@@ -47,24 +46,22 @@ typedef dynamic_graph_bridge_msgs::msg::Vector RosVector;
/** @brief Conventient renaming for ease of implementation. */
typedef dynamic_graph_bridge_msgs::msg::Matrix RosMatrix;
-struct DgRosTypes
-{
- static std::vector<std::string> create_type_list()
- {
- std::vector<std::string> tmp_type_list;
- tmp_type_list.push_back("double");
- tmp_type_list.push_back("unsigned");
- tmp_type_list.push_back("matrix");
- tmp_type_list.push_back("vector");
- tmp_type_list.push_back("vector3");
- tmp_type_list.push_back("vector3_stamped");
- tmp_type_list.push_back("matrix_homogeneous");
- tmp_type_list.push_back("matrix_homogeneous_stamped");
- tmp_type_list.push_back("twist");
- tmp_type_list.push_back("twist_stamped");
- return tmp_type_list;
- }
- // clang-format off
+struct DgRosTypes {
+ static std::vector<std::string> create_type_list() {
+ std::vector<std::string> tmp_type_list;
+ tmp_type_list.push_back("double");
+ tmp_type_list.push_back("unsigned");
+ tmp_type_list.push_back("matrix");
+ tmp_type_list.push_back("vector");
+ tmp_type_list.push_back("vector3");
+ tmp_type_list.push_back("vector3_stamped");
+ tmp_type_list.push_back("matrix_homogeneous");
+ tmp_type_list.push_back("matrix_homogeneous_stamped");
+ tmp_type_list.push_back("twist");
+ tmp_type_list.push_back("twist_stamped");
+ return tmp_type_list;
+ }
+ // clang-format off
static const std::string& get_double(){return type_list[0];};
static const std::string& get_unsigned(){return type_list[1];};
static const std::string& get_matrix(){return type_list[2];};
@@ -76,8 +73,8 @@ struct DgRosTypes
{return type_list[7];};
static const std::string& get_twist(){return type_list[8];};
static const std::string& get_twist_stamped(){return type_list[9];};
- // clang-format on
- static const std::vector<std::string> type_list;
+ // clang-format on
+ static const std::vector<std::string> type_list;
};
/**
@@ -93,111 +90,104 @@ struct DgRosTypes
* @tparam DgType is the type of the Dynamic Graph data.
*/
template <class RosType, class DgType>
-class DgRosMapping
-{
-public:
- /*
- * Some renaming first
- */
-
- /** @brief Dynamic Graph type. */
- typedef DgType dg_t;
- /** @brief Ros type. */
- typedef RosType ros_t;
- /** @brief Ros type as a shared pointer. */
- typedef std::shared_ptr<RosType> ros_shared_ptr_t;
- /** @brief Output signal type. */
- typedef dynamicgraph::SignalTimeDependent<dg_t, int> signal_out_t;
- /** @brief Output signal type. */
- typedef dynamicgraph::SignalTimeDependent<timestamp_t, int>
- signal_timestamp_out_t;
- /** @brief Input signal type. */
- typedef dynamicgraph::SignalPtr<dg_t, int> signal_in_t;
- /** @brief Signal callback function types. */
- typedef std::function<dg_t&(dg_t&, int)> signal_callback_t;
-
- /** @brief Name of the signal type. Used during the creation of signals. */
- static const std::string signal_type_name;
- static const dg_t default_value;
-
- /**
- * @brief Set the default value to the inuput DG signal.
- *
- * @tparam SignalType (in/out)
- * @param s
- */
- template <typename SignalTypePtr>
- static void set_default(SignalTypePtr s)
- {
- s->setConstant(DgRosMapping<RosType, DgType>::default_value);
- }
-
- /**
- * @brief Set the default value to the input DG object.
- *
- * @param s
- */
- static void set_default(dg_t& d)
- {
- d = DgRosMapping<RosType, DgType>::default_value;
- }
-
- /**
- * @brief Convert ROS time to std::chrono.
- *
- * @param ros_time
- * @return timestamp_t
- */
- static timestamp_t from_ros_time(rclcpp::Time ros_time)
- {
- return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
- }
-
- /**
- * @brief Get epoch time as ROS time start from there.
- *
- * @return timestamp_t
- */
- static timestamp_t epoch_time()
- {
- return std::chrono::time_point<std::chrono::high_resolution_clock>{};
- }
-
- /**
- * @brief Convert a ROS object into DG one.
- *
- * @param src
- * @param dst
- */
- static void ros_to_dg(const ros_t& ros_src, dg_t& dg_dst);
-
- /**
- * @brief Convert a DG object into a ROS one.
- *
- * @param src
- * @param dst
- */
- static void dg_to_ros(const dg_t& dg_src, ros_t& ros_dst);
+class DgRosMapping {
+ public:
+ /*
+ * Some renaming first
+ */
+
+ /** @brief Dynamic Graph type. */
+ typedef DgType dg_t;
+ /** @brief Ros type. */
+ typedef RosType ros_t;
+ /** @brief Ros type as a shared pointer. */
+ typedef std::shared_ptr<RosType> ros_shared_ptr_t;
+ /** @brief Output signal type. */
+ typedef dynamicgraph::SignalTimeDependent<dg_t, int> signal_out_t;
+ /** @brief Output signal type. */
+ typedef dynamicgraph::SignalTimeDependent<timestamp_t, int>
+ signal_timestamp_out_t;
+ /** @brief Input signal type. */
+ typedef dynamicgraph::SignalPtr<dg_t, int> signal_in_t;
+ /** @brief Signal callback function types. */
+ typedef std::function<dg_t&(dg_t&, int)> signal_callback_t;
+
+ /** @brief Name of the signal type. Used during the creation of signals. */
+ static const std::string signal_type_name;
+ static const dg_t default_value;
+
+ /**
+ * @brief Set the default value to the inuput DG signal.
+ *
+ * @tparam SignalType (in/out)
+ * @param s
+ */
+ template <typename SignalTypePtr>
+ static void set_default(SignalTypePtr s) {
+ s->setConstant(DgRosMapping<RosType, DgType>::default_value);
+ }
+
+ /**
+ * @brief Set the default value to the input DG object.
+ *
+ * @param s
+ */
+ static void set_default(dg_t& d) {
+ d = DgRosMapping<RosType, DgType>::default_value;
+ }
+
+ /**
+ * @brief Convert ROS time to std::chrono.
+ *
+ * @param ros_time
+ * @return timestamp_t
+ */
+ static timestamp_t from_ros_time(rclcpp::Time ros_time) {
+ return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
+ }
+
+ /**
+ * @brief Get epoch time as ROS time start from there.
+ *
+ * @return timestamp_t
+ */
+ static timestamp_t epoch_time() {
+ return std::chrono::time_point<std::chrono::high_resolution_clock>{};
+ }
+
+ /**
+ * @brief Convert a ROS object into DG one.
+ *
+ * @param src
+ * @param dst
+ */
+ static void ros_to_dg(const ros_t& ros_src, dg_t& dg_dst);
+
+ /**
+ * @brief Convert a DG object into a ROS one.
+ *
+ * @param src
+ * @param dst
+ */
+ static void dg_to_ros(const dg_t& dg_src, ros_t& ros_dst);
};
template <class RosEntity>
inline std::string make_signal_string(const RosEntity& entity,
bool isInputSignal,
const std::string& signal_type,
- const std::string& signal_name)
-{
- std::ostringstream oss;
- oss << entity.getClassName() << "(" << entity.getName() << ")"
- << "::" << (isInputSignal ? "input" : "output") << "(" << signal_type
- << ")"
- << "::" << signal_name;
- return oss.str();
+ const std::string& signal_name) {
+ std::ostringstream oss;
+ oss << entity.getClassName() << "(" << entity.getName() << ")"
+ << "::" << (isInputSignal ? "input" : "output") << "(" << signal_type
+ << ")"
+ << "::" << signal_name;
+ return oss.str();
}
-inline void make_header(std_msgs::msg::Header& header)
-{
- header.stamp = get_ros_node(DG_ROS_NODE_NAME)->get_clock()->now();
- header.frame_id = "/dynamic_graph/world";
+inline void make_header(std_msgs::msg::Header& header) {
+ header.stamp = get_ros_node(DG_ROS_NODE_NAME)->get_clock()->now();
+ header.frame_id = "/dynamic_graph/world";
}
} // namespace dynamic_graph_bridge
diff --git a/src/fwd.hpp b/src/fwd.hpp
index 878b347e35c7c4d16fab44f923ff1c0da8a73516..cf862b155f9e8797310f6201f077ce10cb14b4b4 100644
--- a/src/fwd.hpp
+++ b/src/fwd.hpp
@@ -10,18 +10,16 @@
#pragma once
-#include <ostream>
#include <chrono>
+#include <ostream>
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/** @brief Time stamp type. */
typedef std::chrono::time_point<std::chrono::high_resolution_clock> timestamp_t;
} // namespace dynamic_graph_bridge
-namespace dynamicgraph
-{
+namespace dynamicgraph {
/**
* @brief out stream the time stamp data.
*
@@ -29,17 +27,16 @@ namespace dynamicgraph
* @param time_stamp
* @return std::ostream&
*
- * For clang this function needs to be forward declared before the template using it.
- * This is more in accordance to the standard.
+ * For clang this function needs to be forward declared before the template
+ * using it. This is more in accordance to the standard.
*/
inline std::ostream &operator<<(
- std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp)
-{
- std::chrono::time_point<std::chrono::high_resolution_clock,
- std::chrono::milliseconds>
- time_stamp_nanosec =
- std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
- os << time_stamp_nanosec.time_since_epoch().count();
- return os;
-}
+ std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp) {
+ std::chrono::time_point<std::chrono::high_resolution_clock,
+ std::chrono::milliseconds>
+ time_stamp_nanosec =
+ std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
+ os << time_stamp_nanosec.time_since_epoch().count();
+ return os;
}
+} // namespace dynamicgraph
diff --git a/src/matrix_geometry.hpp b/src/matrix_geometry.hpp
index 98ba8029c39da5c42cabb5dd1dd5c2584aefa0ea..6c66826f20ef2f928007ea65f52bd91341f09109 100644
--- a/src/matrix_geometry.hpp
+++ b/src/matrix_geometry.hpp
@@ -12,13 +12,13 @@
#include <dynamic-graph/eigen-io.h>
#include <dynamic-graph/linear-algebra.h>
+
#include <Eigen/Core>
#include <Eigen/Geometry>
#define MRAWDATA(x) x.data()
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
typedef Eigen::Transform<double, 3, Eigen::Affine> MatrixHomogeneous;
typedef Eigen::Matrix<double, 3, 3> MatrixRotation;
typedef Eigen::AngleAxis<double> VectorUTheta;
@@ -28,19 +28,18 @@ typedef Eigen::Vector3d VectorRollPitchYaw;
typedef Eigen::Matrix<double, 6, 6> MatrixForce;
typedef Eigen::Matrix<double, 6, 6> MatrixTwist;
-inline void buildFrom(const MatrixHomogeneous& MH, MatrixTwist& MT)
-{
- Eigen::Vector3d _t = MH.translation();
- MatrixRotation R(MH.linear());
- Eigen::Matrix3d Tx;
- Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
- Eigen::Matrix3d sk;
- sk = Tx * R;
-
- MT.block<3, 3>(0, 0) = R;
- MT.block<3, 3>(0, 3) = sk;
- MT.block<3, 3>(3, 0).setZero();
- MT.block<3, 3>(3, 3) = R;
+inline void buildFrom(const MatrixHomogeneous& MH, MatrixTwist& MT) {
+ Eigen::Vector3d _t = MH.translation();
+ MatrixRotation R(MH.linear());
+ Eigen::Matrix3d Tx;
+ Tx << 0, -_t(2), _t(1), _t(2), 0, -_t(0), -_t(1), _t(0), 0;
+ Eigen::Matrix3d sk;
+ sk = Tx * R;
+
+ MT.block<3, 3>(0, 0) = R;
+ MT.block<3, 3>(0, 3) = sk;
+ MT.block<3, 3>(3, 0).setZero();
+ MT.block<3, 3>(3, 3) = R;
}
} // namespace dynamic_graph_bridge
diff --git a/src/programs/geometric_simu.cpp b/src/programs/geometric_simu.cpp
index 0781323c251e203f0f484df22f544ad52195eb95..0fd97b7b22520f55049be0f20a9f36c2d43e3745 100644
--- a/src/programs/geometric_simu.cpp
+++ b/src/programs/geometric_simu.cpp
@@ -7,16 +7,16 @@
*/
#include <iostream>
-
#define ENABLE_RT_LOG
#include <dynamic-graph/real-time-logger.h>
-#include <dynamic_graph_bridge/sot_loader.hh>
#include <dynamic_graph_bridge/ros.hpp>
+#include <dynamic_graph_bridge/sot_loader.hh>
int main(int argc, char *argv[]) {
- ::dynamicgraph::RealTimeLogger::instance()
- .addOutputStream(::dynamicgraph::LoggerStreamPtr_t(new dynamicgraph::LoggerIOStream(std::cout)));
+ ::dynamicgraph::RealTimeLogger::instance().addOutputStream(
+ ::dynamicgraph::LoggerStreamPtr_t(
+ new dynamicgraph::LoggerIOStream(std::cout)));
rclcpp::init(argc, argv);
diff --git a/src/ros.cpp b/src/ros.cpp
index ae62cedfd9e1ab9917b911bf0ef06886936523af..c306efc9f3ecd6b40b2decb3401d364a97f8403f 100644
--- a/src/ros.cpp
+++ b/src/ros.cpp
@@ -8,10 +8,10 @@
*/
/// Standard includes
-#include <deque>
#include <atomic>
-#include <fstream>
#include <chrono>
+#include <deque>
+#include <fstream>
#include <thread>
/// ROS2 includes
@@ -25,8 +25,7 @@
#include <sys/param.h>
#endif
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/*
* Private methods
*/
@@ -56,134 +55,112 @@ static GlobalListOfRosNodeType GLOBAL_LIST_OF_ROS_NODE;
/**
* @brief Small class allowing to start a ROS spin in a thread.
*/
-class Executor
-{
-public:
- Executor() : ros_executor_(rclcpp::ExecutorOptions(), 4)
- {
- is_thread_running_ = false;
- is_spinning_ = false;
- list_node_added_.clear();
+class Executor {
+ public:
+ Executor() : ros_executor_(rclcpp::ExecutorOptions(), 4) {
+ is_thread_running_ = false;
+ is_spinning_ = false;
+ list_node_added_.clear();
+ }
+
+ /**
+ * @brief Upon destruction close the thread and stop spinning.
+ *
+ * @return
+ */
+ ~Executor() { stop_spinning(); }
+
+ /**
+ * @brief Start ros_spin in a different thread to not block the current one.
+ */
+ void spin_non_blocking() {
+ if (!is_thread_running_ && !is_spinning_) {
+ std::cout << "Start ros spin in thread." << std::endl;
+ // Marking thread as started to avoid a second thread from getting
+ // started.
+ is_thread_running_ = true;
+ thread_ = std::thread(&Executor::thread_callback, this);
}
-
- /**
- * @brief Upon destruction close the thread and stop spinning.
- *
- * @return
- */
- ~Executor()
- {
- stop_spinning();
+ }
+
+ /**
+ * @brief Block the current thread and make ROS spinning.
+ */
+ void spin() {
+ if (is_thread_running_) {
+ while (ros_ok() && is_thread_running_) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
+ }
+ } else {
+ is_spinning_ = true;
+ ros_executor_.spin();
+ is_spinning_ = false;
}
+ }
- /**
- * @brief Start ros_spin in a different thread to not block the current one.
- */
- void spin_non_blocking()
- {
- if (!is_thread_running_ && !is_spinning_)
- {
- std::cout << "Start ros spin in thread." << std::endl;
- // Marking thread as started to avoid a second thread from getting
- // started.
- is_thread_running_ = true;
- thread_ = std::thread(&Executor::thread_callback, this);
- }
+ void add_node(const std::string& ros_node_name) {
+ if (std::find(list_node_added_.begin(), list_node_added_.end(),
+ ros_node_name) == list_node_added_.end()) {
+ list_node_added_.push_back(ros_node_name);
+ ros_executor_.add_node(get_ros_node(ros_node_name));
}
-
- /**
- * @brief Block the current thread and make ROS spinning.
- */
- void spin()
- {
- if (is_thread_running_)
- {
- while (ros_ok() && is_thread_running_)
- {
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
- }
- else
- {
- is_spinning_ = true;
- ros_executor_.spin();
- is_spinning_ = false;
- }
+ }
+
+ void remove_node(const std::string& ros_node_name) {
+ std::deque<std::string>::iterator el = std::find(
+ list_node_added_.begin(), list_node_added_.end(), ros_node_name);
+ if (el != list_node_added_.end()) {
+ list_node_added_.erase(el);
+ assert(std::find(list_node_added_.begin(), list_node_added_.end(),
+ ros_node_name) == list_node_added_.end() &&
+ "The node has not been removed properly.");
+ ros_executor_.remove_node(get_ros_node(ros_node_name));
}
-
- void add_node(const std::string& ros_node_name)
- {
- if (std::find(list_node_added_.begin(),
- list_node_added_.end(),
- ros_node_name) == list_node_added_.end())
- {
- list_node_added_.push_back(ros_node_name);
- ros_executor_.add_node(get_ros_node(ros_node_name));
- }
+ }
+
+ /**
+ * @brief Stop the spinning al together. Callable in a different thread.
+ */
+ void stop_spinning() {
+ while (is_thread_running_ || is_spinning_) {
+ ros_executor_.cancel();
+ std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
-
- void remove_node(const std::string& ros_node_name)
- {
- std::deque<std::string>::iterator el = std::find(
- list_node_added_.begin(), list_node_added_.end(), ros_node_name);
- if (el != list_node_added_.end())
- {
- list_node_added_.erase(el);
- assert(std::find(list_node_added_.begin(),
- list_node_added_.end(),
- ros_node_name) == list_node_added_.end() &&
- "The node has not been removed properly.");
- ros_executor_.remove_node(get_ros_node(ros_node_name));
- }
+ if (thread_.joinable()) {
+ thread_.join();
}
-
- /**
- * @brief Stop the spinning al together. Callable in a different thread.
- */
- void stop_spinning()
- {
- while (is_thread_running_ || is_spinning_)
- {
- ros_executor_.cancel();
- std::this_thread::sleep_for(std::chrono::milliseconds(100));
- }
- if (thread_.joinable())
- {
- thread_.join();
- }
- }
-
-private:
- /**
- * @brief Thread callback function
- */
- void thread_callback()
- {
- ros_executor_.spin();
- is_thread_running_ = false;
- }
-
- /**
- * @brief Check if the thread is running.
- */
- std::atomic<bool> is_thread_running_;
-
- /**
- * @brief Check if the the executor is spinning.
- */
- std::atomic<bool> is_spinning_;
-
- /**
- * @brief Thread in which the EXECUTOR spins.
- */
- std::thread thread_;
-
- /**
- * @brief Object that execute the ROS callbacks in a different thread.
- */
- RosExecutor ros_executor_;
-
- std::deque<std::string> list_node_added_;
+ }
+
+ private:
+ /**
+ * @brief Thread callback function
+ */
+ void thread_callback() {
+ ros_executor_.spin();
+ is_thread_running_ = false;
+ }
+
+ /**
+ * @brief Check if the thread is running.
+ */
+ std::atomic<bool> is_thread_running_;
+
+ /**
+ * @brief Check if the the executor is spinning.
+ */
+ std::atomic<bool> is_spinning_;
+
+ /**
+ * @brief Thread in which the EXECUTOR spins.
+ */
+ std::thread thread_;
+
+ /**
+ * @brief Object that execute the ROS callbacks in a different thread.
+ */
+ RosExecutor ros_executor_;
+
+ std::deque<std::string> list_node_added_;
}; // class Executor
@@ -201,29 +178,28 @@ ExecutorPtr EXECUTOR = nullptr;
*
* @Return std::string the current executable name.
*/
-std::string executable_name()
-{
+std::string executable_name() {
#if defined(PLATFORM_POSIX) || \
defined(__linux__) // check defines for your setup
- std::string sp;
- std::ifstream("/proc/self/comm") >> sp;
- return sp;
+ std::string sp;
+ std::ifstream("/proc/self/comm") >> sp;
+ return sp;
#elif defined(_WIN32)
- char buf[MAX_PATH];
- GetModuleFileNameA(nullptr, buf, MAX_PATH);
- return buf;
+ char buf[MAX_PATH];
+ GetModuleFileNameA(nullptr, buf, MAX_PATH);
+ return buf;
#elif defined(__APPLE__)
- uint32_t buf_length=MAXPATHLEN;
- char buf[MAXPATHLEN];
- _NSGetExecutablePath(buf,&buf_length);
- return buf;
+ uint32_t buf_length = MAXPATHLEN;
+ char buf[MAXPATHLEN];
+ _NSGetExecutablePath(buf, &buf_length);
+ return buf;
#else
- static_assert(false, "unrecognized platform");
+ static_assert(false, "unrecognized platform");
#endif
}
@@ -231,119 +207,89 @@ std::string executable_name()
/**
* @brief Private function that allow us to initialize ROS only once.
*/
-void ros_init()
-{
- if (!rclcpp::ok())
- {
- /** call rclcpp::init */
- int argc = 1;
- char* arg0 = strdup(executable_name().c_str());
- char* argv[] = {arg0, nullptr};
- rclcpp::init(argc, argv);
- free(arg0);
- }
+void ros_init() {
+ if (!rclcpp::ok()) {
+ /** call rclcpp::init */
+ int argc = 1;
+ char* arg0 = strdup(executable_name().c_str());
+ char* argv[] = {arg0, nullptr};
+ rclcpp::init(argc, argv);
+ free(arg0);
+ }
}
/*
* Public Functions => user API, see ros.hpp for the docstrings.
*/
-bool ros_node_exists(std::string node_name)
-{
- if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
- GLOBAL_LIST_OF_ROS_NODE.end())
- {
- return false;
- }
- if (GLOBAL_LIST_OF_ROS_NODE.at(node_name) == nullptr)
- {
- return false;
- }
- return true;
+bool ros_node_exists(std::string node_name) {
+ if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
+ GLOBAL_LIST_OF_ROS_NODE.end()) {
+ return false;
+ }
+ if (GLOBAL_LIST_OF_ROS_NODE.at(node_name) == nullptr) {
+ return false;
+ }
+ return true;
}
-ExecutorPtr get_ros_executor()
-{
- ros_init();
- if (!EXECUTOR)
- {
- EXECUTOR = std::make_shared<Executor>();
- }
- return EXECUTOR;
+ExecutorPtr get_ros_executor() {
+ ros_init();
+ if (!EXECUTOR) {
+ EXECUTOR = std::make_shared<Executor>();
+ }
+ return EXECUTOR;
}
-RosNodePtr get_ros_node(std::string node_name)
-{
- ros_init();
- if (!ros_node_exists(node_name))
- {
- GLOBAL_LIST_OF_ROS_NODE[node_name] = RosNodePtr(nullptr);
- }
- if (!GLOBAL_LIST_OF_ROS_NODE[node_name] ||
- GLOBAL_LIST_OF_ROS_NODE[node_name].get() == nullptr)
- {
- /** RosNode instanciation */
- GLOBAL_LIST_OF_ROS_NODE[node_name] =
- std::make_shared<RosNode>(
- node_name, "dynamic_graph_bridge");
-
- }
- /** Return a reference to the node handle so any function can use it */
- return GLOBAL_LIST_OF_ROS_NODE[node_name];
+RosNodePtr get_ros_node(std::string node_name) {
+ ros_init();
+ if (!ros_node_exists(node_name)) {
+ GLOBAL_LIST_OF_ROS_NODE[node_name] = RosNodePtr(nullptr);
+ }
+ if (!GLOBAL_LIST_OF_ROS_NODE[node_name] ||
+ GLOBAL_LIST_OF_ROS_NODE[node_name].get() == nullptr) {
+ /** RosNode instanciation */
+ GLOBAL_LIST_OF_ROS_NODE[node_name] =
+ std::make_shared<RosNode>(node_name, "dynamic_graph_bridge");
+ }
+ /** Return a reference to the node handle so any function can use it */
+ return GLOBAL_LIST_OF_ROS_NODE[node_name];
}
-void ros_add_node_to_executor(const std::string& node_name)
-{
- get_ros_executor()->add_node(node_name);
+void ros_add_node_to_executor(const std::string& node_name) {
+ get_ros_executor()->add_node(node_name);
}
-void ros_shutdown(std::string node_name)
-{
- if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
- GLOBAL_LIST_OF_ROS_NODE.end())
- {
- return;
- }
- get_ros_executor()->remove_node(node_name);
- GLOBAL_LIST_OF_ROS_NODE.erase(node_name);
+void ros_shutdown(std::string node_name) {
+ if (GLOBAL_LIST_OF_ROS_NODE.find(node_name) ==
+ GLOBAL_LIST_OF_ROS_NODE.end()) {
+ return;
+ }
+ get_ros_executor()->remove_node(node_name);
+ GLOBAL_LIST_OF_ROS_NODE.erase(node_name);
}
-void ros_shutdown()
-{
- // rclcpp::shutdown();
+void ros_shutdown() {
+ // rclcpp::shutdown();
}
-void ros_clean()
-{
- ros_stop_spinning();
- GlobalListOfRosNodeType::iterator ros_node_it =
- GLOBAL_LIST_OF_ROS_NODE.begin();
- while (!GLOBAL_LIST_OF_ROS_NODE.empty())
- {
- ros_shutdown(ros_node_it->first);
- ros_node_it = GLOBAL_LIST_OF_ROS_NODE.begin();
- }
- GLOBAL_LIST_OF_ROS_NODE.clear();
+void ros_clean() {
+ ros_stop_spinning();
+ GlobalListOfRosNodeType::iterator ros_node_it =
+ GLOBAL_LIST_OF_ROS_NODE.begin();
+ while (!GLOBAL_LIST_OF_ROS_NODE.empty()) {
+ ros_shutdown(ros_node_it->first);
+ ros_node_it = GLOBAL_LIST_OF_ROS_NODE.begin();
+ }
+ GLOBAL_LIST_OF_ROS_NODE.clear();
}
-bool ros_ok()
-{
- return rclcpp::ok();
-}
+bool ros_ok() { return rclcpp::ok(); }
-void ros_spin()
-{
- get_ros_executor()->spin();
-}
+void ros_spin() { get_ros_executor()->spin(); }
-void ros_spin_non_blocking()
-{
- get_ros_executor()->spin_non_blocking();
-}
+void ros_spin_non_blocking() { get_ros_executor()->spin_non_blocking(); }
-void ros_stop_spinning()
-{
- get_ros_executor()->stop_spinning();
-}
+void ros_stop_spinning() { get_ros_executor()->stop_spinning(); }
} // end of namespace dynamic_graph_bridge.
diff --git a/src/ros_parameter.cpp b/src/ros_parameter.cpp
index 485fd1063b24a80e7d2adf2de84dd0feef25de8b..3cf1e7c2a0eab30ffdf053c03061ff023f019556 100644
--- a/src/ros_parameter.cpp
+++ b/src/ros_parameter.cpp
@@ -1,60 +1,53 @@
-#include <sot/core/robot-utils.hh>
+#include "dynamic_graph_bridge/ros_parameter.hpp"
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/parsers/urdf.hpp"
+#include <urdf_parser/urdf_parser.h>
-#include <stdexcept>
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
+#include <sot/core/robot-utils.hh>
+#include <stdexcept>
-#include <urdf_parser/urdf_parser.h>
-
-#include "dynamic_graph_bridge/ros_parameter.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/parsers/urdf.hpp"
namespace dynamicgraph {
- bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh)
- {
-
- if (!nh->has_parameter("robot_description"))
- {
- nh->declare_parameter("robot_description",std::string(""));
- }
-
- std::string robot_description;
- std::string parameter_name("robot_description");
- nh->get_parameter(parameter_name,robot_description);
- if (robot_description.empty())
- {
- RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
- "Parameter robot_description is empty");
- return false;
- }
-
- std::string model_name("robot");
-
- // Search for the robot util related to robot_name.
- sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
- // If does not exist then it is created.
- if (aRobotUtil == sot::RefVoidRobotUtil())
- aRobotUtil = sot::createRobotUtil(model_name);
-
- // If the creation is fine
- if (aRobotUtil != sot::RefVoidRobotUtil())
- {
- // Then set the robot model.
- aRobotUtil->set_parameter(parameter_name,robot_description);
- RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "Set parameter_name : %s.",parameter_name.c_str());
- // Everything went fine.
- return true;
- }
- RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
- "Wrong initialization of parameter_name %s",
- parameter_name.c_str());
+bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh) {
+ if (!nh->has_parameter("robot_description")) {
+ nh->declare_parameter("robot_description", std::string(""));
+ }
- // Otherwise something went wrong.
+ std::string robot_description;
+ std::string parameter_name("robot_description");
+ nh->get_parameter(parameter_name, robot_description);
+ if (robot_description.empty()) {
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Parameter robot_description is empty");
return false;
+ }
+ std::string model_name("robot");
+
+ // Search for the robot util related to robot_name.
+ sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
+ // If does not exist then it is created.
+ if (aRobotUtil == sot::RefVoidRobotUtil())
+ aRobotUtil = sot::createRobotUtil(model_name);
+
+ // If the creation is fine
+ if (aRobotUtil != sot::RefVoidRobotUtil()) {
+ // Then set the robot model.
+ aRobotUtil->set_parameter(parameter_name, robot_description);
+ RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Set parameter_name : %s.", parameter_name.c_str());
+ // Everything went fine.
+ return true;
}
+ RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
+ "Wrong initialization of parameter_name %s",
+ parameter_name.c_str());
+ // Otherwise something went wrong.
+ return false;
}
+
+} // namespace dynamicgraph
diff --git a/src/ros_publish-python-module-py.cc b/src/ros_publish-python-module-py.cc
index 831462b04c76e39333494ba977c32e74aec9ff1d..f20e669fd70f477b5dfea49028627ceb6d295cc0 100644
--- a/src/ros_publish-python-module-py.cc
+++ b/src/ros_publish-python-module-py.cc
@@ -1,12 +1,11 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_publish.hpp"
namespace dgb = dynamic_graph_bridge;
namespace dg = dynamicgraph;
-
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dgb::RosPublish, bp::bases<dg::Entity>,
diff --git a/src/ros_publish.cpp b/src/ros_publish.cpp
index 9ed9b7fb331f91dbf4e284f705d418150cb1983d..a15a12db80fddbee7fd2b54a232037fd76ad0600 100644
--- a/src/ros_publish.cpp
+++ b/src/ros_publish.cpp
@@ -7,11 +7,11 @@
* @brief Implements the RosPublish class.
*/
-#include "dynamic_graph_bridge/ros.hpp"
#include "ros_publish.hpp"
-namespace dynamic_graph_bridge
-{
+#include "dynamic_graph_bridge/ros.hpp"
+
+namespace dynamic_graph_bridge {
/*
* RosPublish class
*/
@@ -32,276 +32,213 @@ RosPublish::RosPublish(const std::string& n)
make_signal_string(*this, true, "int", trigger_signal_name_)),
rate_nanosec_(std::chrono::nanoseconds(50000000)) // 50ms
{
- signalRegistration(trigger_);
-
- ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
- ros_add_node_to_executor(DG_ROS_NODE_NAME);
- ros_spin_non_blocking();
- binded_signals_.clear();
- /** @todo Check this needUpdateFromAllChildren */
- trigger_.setNeedUpdateFromAllChildren(true);
- try
- {
- last_publicated_ = ros_node_->get_clock()->now();
- }
- catch (const std::exception& exc)
- {
- throw std::runtime_error(
- "Failed to call ros_node_->get_clock()->now():" +
- std::string(exc.what()));
- }
- mutex_.unlock();
-
- std::string doc_string;
- doc_string =
- "\n"
- " Add a signal writing data to a ROS topic.\n"
- "\n"
- " Input:\n"
- " - type: string among:\n";
- for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
- {
- doc_string += " - " + DgRosTypes::type_list[i] + "\n";
- }
- doc_string +=
- " - signal: the signal name in dynamic-graph,\n"
- " - topic: the topic name in ROS.\n"
- "\n";
- addCommand("add", new command::ros_publish::Add(*this, doc_string));
- doc_string =
- "\n"
- " Remove a signal writing data to a ROS topic.\n"
- "\n"
- " Input:\n"
- " - name of the signal to remove (see method list for the list of "
- "signals).\n"
- "\n";
- addCommand("rm", new command::ros_publish::Rm(*this, doc_string));
- doc_string =
- "\n"
- " Remove all signals writing data to a ROS topic.\n"
- "\n"
- " No input.\n"
- "\n";
- addCommand("clear", new command::ros_publish::Clear(*this, doc_string));
- doc_string =
- "\n"
- " List signals writing data to a ROS topic.\n"
- "\n"
- " No input.\n"
- "\n";
- addCommand("list", new command::ros_publish::List(*this, doc_string));
+ signalRegistration(trigger_);
+
+ ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
+ ros_add_node_to_executor(DG_ROS_NODE_NAME);
+ ros_spin_non_blocking();
+ binded_signals_.clear();
+ /** @todo Check this needUpdateFromAllChildren */
+ trigger_.setNeedUpdateFromAllChildren(true);
+ try {
+ last_publicated_ = ros_node_->get_clock()->now();
+ } catch (const std::exception& exc) {
+ throw std::runtime_error("Failed to call ros_node_->get_clock()->now():" +
+ std::string(exc.what()));
+ }
+ mutex_.unlock();
+
+ std::string doc_string;
+ doc_string =
+ "\n"
+ " Add a signal writing data to a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - type: string among:\n";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i) {
+ doc_string += " - " + DgRosTypes::type_list[i] + "\n";
+ }
+ doc_string +=
+ " - signal: the signal name in dynamic-graph,\n"
+ " - topic: the topic name in ROS.\n"
+ "\n";
+ addCommand("add", new command::ros_publish::Add(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove a signal writing data to a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - name of the signal to remove (see method list for the list of "
+ "signals).\n"
+ "\n";
+ addCommand("rm", new command::ros_publish::Rm(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove all signals writing data to a ROS topic.\n"
+ "\n"
+ " No input.\n"
+ "\n";
+ addCommand("clear", new command::ros_publish::Clear(*this, doc_string));
+ doc_string =
+ "\n"
+ " List signals writing data to a ROS topic.\n"
+ "\n"
+ " No input.\n"
+ "\n";
+ addCommand("list", new command::ros_publish::List(*this, doc_string));
} // namespace dynamic_graph_bridge
-RosPublish::~RosPublish()
-{
-}
+RosPublish::~RosPublish() {}
-void RosPublish::display(std::ostream& os) const
-{
- os << this->CLASS_NAME << "(" << this->name << ")." << std::endl;
+void RosPublish::display(std::ostream& os) const {
+ os << this->CLASS_NAME << "(" << this->name << ")." << std::endl;
}
-void RosPublish::rm(const std::string& signal)
-{
- if (binded_signals_.find(signal) == binded_signals_.end())
- {
- return;
- }
-
- if (signal == trigger_signal_name_)
- {
- std::cerr << "The trigger signal should not be removed. Aborting."
- << std::endl;
- return;
- }
-
- // lock the mutex to avoid deleting the signal during a call to trigger
- while (!mutex_.try_lock())
- {
- }
- signalDeregistration(signal);
- binded_signals_.erase(signal);
- mutex_.unlock();
+void RosPublish::rm(const std::string& signal) {
+ if (binded_signals_.find(signal) == binded_signals_.end()) {
+ return;
+ }
+
+ if (signal == trigger_signal_name_) {
+ std::cerr << "The trigger signal should not be removed. Aborting."
+ << std::endl;
+ return;
+ }
+
+ // lock the mutex to avoid deleting the signal during a call to trigger
+ while (!mutex_.try_lock()) {
+ }
+ signalDeregistration(signal);
+ binded_signals_.erase(signal);
+ mutex_.unlock();
}
-std::string RosPublish::list() const
-{
- std::string result("[");
- for (std::map<std::string, BindedSignal>::const_iterator it =
- binded_signals_.begin();
- it != binded_signals_.end();
- it++)
- {
- result += "'" + it->first + "',";
- }
- result += "]";
- return result;
+std::string RosPublish::list() const {
+ std::string result("[");
+ for (std::map<std::string, BindedSignal>::const_iterator it =
+ binded_signals_.begin();
+ it != binded_signals_.end(); it++) {
+ result += "'" + it->first + "',";
+ }
+ result += "]";
+ return result;
}
-void RosPublish::clear()
-{
- std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
- for (; it != binded_signals_.end();)
- {
- if (it->first != trigger_signal_name_)
- {
- rm(it->first);
- it = binded_signals_.begin();
- }
- else
- {
- ++it;
- }
+void RosPublish::clear() {
+ std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
+ for (; it != binded_signals_.end();) {
+ if (it->first != trigger_signal_name_) {
+ rm(it->first);
+ it = binded_signals_.begin();
+ } else {
+ ++it;
}
+ }
}
-int& RosPublish::trigger(int& dummy, int)
-{
- typedef std::map<std::string, BindedSignal>::iterator iterator_t;
-
- // If we did not wait long enough (> rate_nanosec_), we do nothing.
- rclcpp::Duration dt = ros_node_->get_clock()->now() - last_publicated_;
- if (dt < rate_nanosec_)
- {
- return dummy;
- }
-
- // We reset the previous publishing time as "now".
- last_publicated_ = ros_node_->get_clock()->now();
+int& RosPublish::trigger(int& dummy, int) {
+ typedef std::map<std::string, BindedSignal>::iterator iterator_t;
- // We lock the mutex for the full duration of the publication.
- mutex_.lock();
- for (iterator_t it = binded_signals_.begin(); it != binded_signals_.end();
- ++it)
- {
- // calling the callback corresponding to the input signal in the tuple.
- std::get<1>(it->second)();
- }
- mutex_.unlock();
+ // If we did not wait long enough (> rate_nanosec_), we do nothing.
+ rclcpp::Duration dt = ros_node_->get_clock()->now() - last_publicated_;
+ if (dt < rate_nanosec_) {
return dummy;
+ }
+
+ // We reset the previous publishing time as "now".
+ last_publicated_ = ros_node_->get_clock()->now();
+
+ // We lock the mutex for the full duration of the publication.
+ mutex_.lock();
+ for (iterator_t it = binded_signals_.begin(); it != binded_signals_.end();
+ ++it) {
+ // calling the callback corresponding to the input signal in the tuple.
+ std::get<1>(it->second)();
+ }
+ mutex_.unlock();
+ return dummy;
}
-std::string RosPublish::getDocString() const
-{
- return doc_string_;
-}
+std::string RosPublish::getDocString() const { return doc_string_; }
/*
* RosPublish command
*/
-namespace command
-{
-namespace ros_publish
-{
+namespace command {
+namespace ros_publish {
Clear::Clear(RosPublish& entity, const std::string& doc_string)
- : Command(entity, std::vector<Value::Type>(), doc_string)
-{
-}
+ : Command(entity, std::vector<Value::Type>(), doc_string) {}
-Value Clear::doExecute()
-{
- RosPublish& entity = static_cast<RosPublish&>(owner());
+Value Clear::doExecute() {
+ RosPublish& entity = static_cast<RosPublish&>(owner());
- entity.clear();
- return Value();
+ entity.clear();
+ return Value();
}
List::List(RosPublish& entity, const std::string& doc_string)
- : Command(entity, std::vector<Value::Type>(), doc_string)
-{
-}
+ : Command(entity, std::vector<Value::Type>(), doc_string) {}
-Value List::doExecute()
-{
- RosPublish& entity = static_cast<RosPublish&>(owner());
- return Value(entity.list());
+Value List::doExecute() {
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ return Value(entity.list());
}
Add::Add(RosPublish& entity, const std::string& doc_string)
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
- doc_string)
-{
-}
-
-Value Add::doExecute()
-{
- RosPublish& entity = static_cast<RosPublish&>(owner());
- std::vector<Value> values = getParameterValues();
-
- const std::string& type = values[0].value();
- const std::string& signal = values[1].value();
- const std::string& topic = values[2].value();
-
- if (type == DgRosTypes::get_double())
- {
- entity.add<std_msgs::msg::Float64, double>(signal, topic);
- }
- else if (type == DgRosTypes::get_unsigned())
- {
- entity.add<std_msgs::msg::UInt32, unsigned int>(signal, topic);
- }
- else if (type == DgRosTypes::get_matrix())
- {
- entity.add<RosMatrix, DgMatrix>(signal, topic);
- }
- else if (type == DgRosTypes::get_vector())
- {
- entity.add<RosVector, DgVector>(signal, topic);
- }
- else if (type == DgRosTypes::get_vector3())
- {
- entity.add<geometry_msgs::msg::Vector3, DgVector>(signal, topic);
- }
- else if (type == DgRosTypes::get_vector3_stamped())
- {
- entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal, topic);
- }
- else if (type == DgRosTypes::get_matrix_homogeneous())
- {
- entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(signal,
- topic);
- }
- else if (type == DgRosTypes::get_matrix_homogeneous_stamped())
- {
- entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(
- signal, topic);
- }
- else if (type == DgRosTypes::get_twist())
- {
- entity.add<geometry_msgs::msg::Twist, DgVector>(signal, topic);
- }
- else if (type == DgRosTypes::get_twist_stamped())
- {
- entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal, topic);
- }
- else
- {
- std::cerr << "RosPublish(" << entity.getName()
- << ")::add(): bad type given (" << type << ")."
- << " Possible choice is among:";
- for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
- {
- std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
- }
- }
- return Value();
+ doc_string) {}
+
+Value Add::doExecute() {
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal = values[1].value();
+ const std::string& topic = values[2].value();
+
+ if (type == DgRosTypes::get_double()) {
+ entity.add<std_msgs::msg::Float64, double>(signal, topic);
+ } else if (type == DgRosTypes::get_unsigned()) {
+ entity.add<std_msgs::msg::UInt32, unsigned int>(signal, topic);
+ } else if (type == DgRosTypes::get_matrix()) {
+ entity.add<RosMatrix, DgMatrix>(signal, topic);
+ } else if (type == DgRosTypes::get_vector()) {
+ entity.add<RosVector, DgVector>(signal, topic);
+ } else if (type == DgRosTypes::get_vector3()) {
+ entity.add<geometry_msgs::msg::Vector3, DgVector>(signal, topic);
+ } else if (type == DgRosTypes::get_vector3_stamped()) {
+ entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal, topic);
+ } else if (type == DgRosTypes::get_matrix_homogeneous()) {
+ entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(signal, topic);
+ } else if (type == DgRosTypes::get_matrix_homogeneous_stamped()) {
+ entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(signal,
+ topic);
+ } else if (type == DgRosTypes::get_twist()) {
+ entity.add<geometry_msgs::msg::Twist, DgVector>(signal, topic);
+ } else if (type == DgRosTypes::get_twist_stamped()) {
+ entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal, topic);
+ } else {
+ std::cerr << "RosPublish(" << entity.getName()
+ << ")::add(): bad type given (" << type << ")."
+ << " Possible choice is among:";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i) {
+ std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
+ }
+ }
+ return Value();
}
Rm::Rm(RosPublish& entity, const std::string& doc_string)
- : Command(entity, boost::assign::list_of(Value::STRING), doc_string)
-{
-}
-
-Value Rm::doExecute()
-{
- RosPublish& entity = static_cast<RosPublish&>(owner());
- std::vector<Value> values = getParameterValues();
- const std::string& signal = values[0].value();
- entity.rm(signal);
- return Value();
+ : Command(entity, boost::assign::list_of(Value::STRING), doc_string) {}
+
+Value Rm::doExecute() {
+ RosPublish& entity = static_cast<RosPublish&>(owner());
+ std::vector<Value> values = getParameterValues();
+ const std::string& signal = values[0].value();
+ entity.rm(signal);
+ return Value();
}
} // namespace ros_publish
} // end of namespace command.
diff --git a/src/ros_publish.hpp b/src/ros_publish.hpp
index 68b89ef29d89c61afd5c0c9b71eea82fa74f2946..32131cb719e43dcc9b2511f44a33babffa993e01 100644
--- a/src/ros_publish.hpp
+++ b/src/ros_publish.hpp
@@ -20,152 +20,148 @@
#include "dg_ros_mapping.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/** @brief Publish dynamic-graph information into ROS. */
-class RosPublish : public dynamicgraph::Entity
-{
- DYNAMIC_GRAPH_ENTITY_DECL();
-
-public:
- /**
- * @brief Callback function publishing in the ROS topic.
- * This callback is called during the trigger signal callback function.
- */
- typedef std::function<void(void)> PublisherCallback;
-
- /**
- * @brief Tuple composed by the generated input signal and its callback
- * function. The callback function publishes the input signal content.
- */
- typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
- PublisherCallback>
- BindedSignal;
-
- /**
- * @brief Construct a new RosPublish object.
- *
- * @param n
- */
- RosPublish(const std::string& n);
-
- /**
- * @brief Destroy the RosPublish object.
- *
- */
- virtual ~RosPublish();
-
- /**
- * @brief Get the documentation string.
- *
- * @return std::string
- */
- virtual std::string getDocString() const;
-
- /**
- * @brief Display the class information.
- *
- * @param os
- */
- void display(std::ostream& os) const;
-
- /**
- * @brief Add a signal to publish to ROS.
- *
- * @tparam RosType type of the ROS message.
- * @tparam DgType type of the Dynamic Graph signal data.
- * @param signal name.
- * @param topic name.
- */
- template <typename RosType, typename DgType>
- void add(const std::string& signal, const std::string& topic);
-
- /**
- * @brief Remove a signal to publish to ROS.
- *
- * @param signal name.
- */
- void rm(const std::string& signal);
-
- /**
- * @brief List of all signal and topics currently published.
- *
- * @return std::string
- */
- std::string list() const;
-
- /**
- * @brief Remove all signal published to ROS.
- */
- void clear();
-
-private:
- /**
- * @brief Trigger signal callback method.
- *
- * @return int&
- */
- int& trigger(int&, int);
-
- /**
- * @brief Send the data from the input signal to the ROS topic.
- *
- * @tparam RosType ROS message type.
- * @tparam DgType Dynamic graph signal data type.
- * @param publisher pointer to the ros publisher.
- * @param signal signal name.
- */
- template <typename RosType, typename DgType>
- void send_data(
- std::shared_ptr<rclcpp::Publisher<
- typename DgRosMapping<RosType, DgType>::ros_t> > publisher,
- std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t>
- signal);
-
-private:
- /** @brief Doc string associated to the entity. */
- static const std::string doc_string_;
-
- /** @brief Name of the trigger signal. */
- static const std::string trigger_signal_name_;
-
- /** @brief Trigger signal publishing the signal values every other
- * iterations. */
- dynamicgraph::SignalTimeDependent<int, int> trigger_;
-
- /** @brief Publishing rate. Default is 50ms. */
- rclcpp::Duration rate_nanosec_;
-
- /** @brief ROS node pointer. */
- RosNodePtr ros_node_;
-
- /** @brief Named list of signals associated with it's callback functions. */
- std::map<std::string, BindedSignal> binded_signals_;
-
- /** @brief Last published time. */
- rclcpp::Time last_publicated_;
-
- /** @brief Protects the command and trigger callbacks. */
- std::mutex mutex_;
+class RosPublish : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ /**
+ * @brief Callback function publishing in the ROS topic.
+ * This callback is called during the trigger signal callback function.
+ */
+ typedef std::function<void(void)> PublisherCallback;
+
+ /**
+ * @brief Tuple composed by the generated input signal and its callback
+ * function. The callback function publishes the input signal content.
+ */
+ typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ PublisherCallback>
+ BindedSignal;
+
+ /**
+ * @brief Construct a new RosPublish object.
+ *
+ * @param n
+ */
+ RosPublish(const std::string& n);
+
+ /**
+ * @brief Destroy the RosPublish object.
+ *
+ */
+ virtual ~RosPublish();
+
+ /**
+ * @brief Get the documentation string.
+ *
+ * @return std::string
+ */
+ virtual std::string getDocString() const;
+
+ /**
+ * @brief Display the class information.
+ *
+ * @param os
+ */
+ void display(std::ostream& os) const;
+
+ /**
+ * @brief Add a signal to publish to ROS.
+ *
+ * @tparam RosType type of the ROS message.
+ * @tparam DgType type of the Dynamic Graph signal data.
+ * @param signal name.
+ * @param topic name.
+ */
+ template <typename RosType, typename DgType>
+ void add(const std::string& signal, const std::string& topic);
+
+ /**
+ * @brief Remove a signal to publish to ROS.
+ *
+ * @param signal name.
+ */
+ void rm(const std::string& signal);
+
+ /**
+ * @brief List of all signal and topics currently published.
+ *
+ * @return std::string
+ */
+ std::string list() const;
+
+ /**
+ * @brief Remove all signal published to ROS.
+ */
+ void clear();
+
+ private:
+ /**
+ * @brief Trigger signal callback method.
+ *
+ * @return int&
+ */
+ int& trigger(int&, int);
+
+ /**
+ * @brief Send the data from the input signal to the ROS topic.
+ *
+ * @tparam RosType ROS message type.
+ * @tparam DgType Dynamic graph signal data type.
+ * @param publisher pointer to the ros publisher.
+ * @param signal signal name.
+ */
+ template <typename RosType, typename DgType>
+ void send_data(
+ std::shared_ptr<
+ rclcpp::Publisher<typename DgRosMapping<RosType, DgType>::ros_t> >
+ publisher,
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t>
+ signal);
+
+ private:
+ /** @brief Doc string associated to the entity. */
+ static const std::string doc_string_;
+
+ /** @brief Name of the trigger signal. */
+ static const std::string trigger_signal_name_;
+
+ /** @brief Trigger signal publishing the signal values every other
+ * iterations. */
+ dynamicgraph::SignalTimeDependent<int, int> trigger_;
+
+ /** @brief Publishing rate. Default is 50ms. */
+ rclcpp::Duration rate_nanosec_;
+
+ /** @brief ROS node pointer. */
+ RosNodePtr ros_node_;
+
+ /** @brief Named list of signals associated with it's callback functions. */
+ std::map<std::string, BindedSignal> binded_signals_;
+
+ /** @brief Last published time. */
+ rclcpp::Time last_publicated_;
+
+ /** @brief Protects the command and trigger callbacks. */
+ std::mutex mutex_;
};
/*
* Commands
*/
-namespace command
-{
-namespace ros_publish
-{
+namespace command {
+namespace ros_publish {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
-#define ROS_PUBLISH_MAKE_COMMAND(CMD) \
- class CMD : public Command \
- { \
- public: \
- CMD(RosPublish& entity, const std::string& docstring); \
- virtual Value doExecute(); \
- }
+#define ROS_PUBLISH_MAKE_COMMAND(CMD) \
+ class CMD : public Command { \
+ public: \
+ CMD(RosPublish& entity, const std::string& docstring); \
+ virtual Value doExecute(); \
+ }
ROS_PUBLISH_MAKE_COMMAND(Add);
ROS_PUBLISH_MAKE_COMMAND(Clear);
diff --git a/src/ros_publish.hxx b/src/ros_publish.hxx
index 9882cbeaa763203cdb2199c57d94eb93b894cc9d..2cd762c7b93b20d657beccdfa646904eb3d5aae5 100644
--- a/src/ros_publish.hxx
+++ b/src/ros_publish.hxx
@@ -11,49 +11,47 @@
#include "dg_ros_mapping.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
template <typename RosType, typename DgType>
void RosPublish::send_data(
- std::shared_ptr<rclcpp::Publisher<
- typename DgRosMapping<RosType, DgType>::ros_t> > publisher,
- std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t> signal)
-{
- typename DgRosMapping<RosType, DgType>::ros_t msg;
- DgRosMapping<RosType, DgType>::dg_to_ros(signal->accessCopy(), msg);
- publisher->publish(msg);
+ std::shared_ptr<
+ rclcpp::Publisher<typename DgRosMapping<RosType, DgType>::ros_t> >
+ publisher,
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_in_t>
+ signal) {
+ typename DgRosMapping<RosType, DgType>::ros_t msg;
+ DgRosMapping<RosType, DgType>::dg_to_ros(signal->accessCopy(), msg);
+ publisher->publish(msg);
}
template <typename RosType, typename DgType>
-void RosPublish::add(const std::string& signal, const std::string& topic)
-{
- using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
- using signal_in_t = typename DgRosMapping<RosType, DgType>::signal_in_t;
-
- // Initialize the bindedSignal object.
- BindedSignal binded_signal;
-
- // Initialize the publisher.
- std::shared_ptr<rclcpp::Publisher<ros_t> > pub_ptr =
- ros_node_->create_publisher<ros_t>(topic, 10);
-
- // Initialize the signal.
- std::shared_ptr<signal_in_t> signal_ptr = std::make_shared<signal_in_t>(
- nullptr, // no explicit dependence
- make_signal_string(*this,
- true,
- DgRosMapping<RosType, DgType>::signal_type_name,
- signal));
- std::get<0>(binded_signal) = signal_ptr;
- DgRosMapping<RosType, DgType>::set_default(signal_ptr);
- signalRegistration(*std::get<0>(binded_signal));
-
- // Initialize the callback.
- PublisherCallback callback = std::bind(
- &RosPublish::send_data<RosType, DgType>, this, pub_ptr, signal_ptr);
- std::get<1>(binded_signal) = callback;
-
- binded_signals_[signal] = binded_signal;
+void RosPublish::add(const std::string& signal, const std::string& topic) {
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using signal_in_t = typename DgRosMapping<RosType, DgType>::signal_in_t;
+
+ // Initialize the bindedSignal object.
+ BindedSignal binded_signal;
+
+ // Initialize the publisher.
+ std::shared_ptr<rclcpp::Publisher<ros_t> > pub_ptr =
+ ros_node_->create_publisher<ros_t>(topic, 10);
+
+ // Initialize the signal.
+ std::shared_ptr<signal_in_t> signal_ptr = std::make_shared<signal_in_t>(
+ nullptr, // no explicit dependence
+ make_signal_string(*this, true,
+ DgRosMapping<RosType, DgType>::signal_type_name,
+ signal));
+ std::get<0>(binded_signal) = signal_ptr;
+ DgRosMapping<RosType, DgType>::set_default(signal_ptr);
+ signalRegistration(*std::get<0>(binded_signal));
+
+ // Initialize the callback.
+ PublisherCallback callback = std::bind(
+ &RosPublish::send_data<RosType, DgType>, this, pub_ptr, signal_ptr);
+ std::get<1>(binded_signal) = callback;
+
+ binded_signals_[signal] = binded_signal;
}
} // namespace dynamic_graph_bridge
diff --git a/src/ros_python_interpreter_client.cpp b/src/ros_python_interpreter_client.cpp
index d500435d864e1d835b41d7f7567450dcecd6091f..9150ff04d8220a9803e8f32db91b5a7ccea4b60a 100644
--- a/src/ros_python_interpreter_client.cpp
+++ b/src/ros_python_interpreter_client.cpp
@@ -13,145 +13,119 @@
#include <boost/algorithm/string.hpp>
#include <fstream>
-namespace dynamic_graph_bridge
-{
-RosPythonInterpreterClient::RosPythonInterpreterClient()
-{
- ros_node_name_ = "ros_python_interpreter_client";
- ros_node_ = get_ros_node(ros_node_name_);
-
- // Create a client for the single python command service of the
- // DynamicGraphManager.
- run_command_service_name_ = "/dynamic_graph_bridge/run_python_command";
- run_command_request_ = std::make_shared<RunPythonCommandSrvType::Request>();
- connect_to_rosservice_run_python_command();
-
- // Create a client for the python file reading service of the
- // DynamicGraphManager.
- run_script_service_name_ = "/dynamic_graph_bridge/run_python_file";
- run_file_request_ = std::make_shared<RunPythonFileSrvType::Request>();
- connect_to_rosservice_run_python_script();
- timeout_connection_s_ = DurationSec(1);
+namespace dynamic_graph_bridge {
+RosPythonInterpreterClient::RosPythonInterpreterClient() {
+ ros_node_name_ = "ros_python_interpreter_client";
+ ros_node_ = get_ros_node(ros_node_name_);
+
+ // Create a client for the single python command service of the
+ // DynamicGraphManager.
+ run_command_service_name_ = "/dynamic_graph_bridge/run_python_command";
+ run_command_request_ = std::make_shared<RunPythonCommandSrvType::Request>();
+ connect_to_rosservice_run_python_command();
+
+ // Create a client for the python file reading service of the
+ // DynamicGraphManager.
+ run_script_service_name_ = "/dynamic_graph_bridge/run_python_file";
+ run_file_request_ = std::make_shared<RunPythonFileSrvType::Request>();
+ connect_to_rosservice_run_python_script();
+ timeout_connection_s_ = DurationSec(1);
}
std::string RosPythonInterpreterClient::run_python_command(
- const std::string& code_string)
-{
- std::string return_string = "";
-
- // If the code is empty or just a line break, return an empty string.
- if (code_string == "" || code_string == "\n")
- {
- return return_string;
- }
+ const std::string& code_string) {
+ std::string return_string = "";
- try
- {
- if (!command_client_->wait_for_service(timeout_connection_s_))
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Connection to remote server lost. Reconnecting...");
- connect_to_rosservice_run_python_command(timeout_connection_s_);
- return return_string;
- }
-
- run_command_request_->input = code_string;
-
- auto response =
- command_client_->async_send_request(run_command_request_);
- // Wait for the result.
- while (rclcpp::ok() &&
- rclcpp::spin_until_future_complete(
- ros_node_, response, std::chrono::seconds(1)) !=
- rclcpp::FutureReturnCode::SUCCESS)
- {
- RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
- "Error while parsing command, retrying...");
- }
-
- // Get the standard output (print).
- return_string += response.get()->standardoutput;
-
- // Get the error.
- return_string += response.get()->standarderror;
-
- // Get the Result and print it is any.
- if (response.get()->result != "None")
- {
- return_string += response.get()->result;
- }
+ // If the code is empty or just a line break, return an empty string.
+ if (code_string == "" || code_string == "\n") {
+ return return_string;
+ }
+
+ try {
+ if (!command_client_->wait_for_service(timeout_connection_s_)) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ return return_string;
}
- catch (const std::exception& ex)
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- ex.what());
- connect_to_rosservice_run_python_command(timeout_connection_s_);
+
+ run_command_request_->input = code_string;
+
+ auto response = command_client_->async_send_request(run_command_request_);
+ // Wait for the result.
+ while (rclcpp::ok() && rclcpp::spin_until_future_complete(
+ ros_node_, response, std::chrono::seconds(1)) !=
+ rclcpp::FutureReturnCode::SUCCESS) {
+ RCLCPP_ERROR(rclcpp::get_logger("rclcpp"),
+ "Error while parsing command, retrying...");
}
- catch (...)
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Error caught, maybe the connection to remote server is "
- "lost. Reconnecting...");
- connect_to_rosservice_run_python_command(timeout_connection_s_);
+
+ // Get the standard output (print).
+ return_string += response.get()->standardoutput;
+
+ // Get the error.
+ return_string += response.get()->standarderror;
+
+ // Get the Result and print it is any.
+ if (response.get()->result != "None") {
+ return_string += response.get()->result;
}
- return return_string;
+ } catch (const std::exception& ex) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), ex.what());
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ } catch (...) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Error caught, maybe the connection to remote server is "
+ "lost. Reconnecting...");
+ connect_to_rosservice_run_python_command(timeout_connection_s_);
+ }
+ return return_string;
} // namespace dynamic_graph_bridge
std::string RosPythonInterpreterClient::run_python_script(
- const std::string& filename)
-{
- std::string return_string = "";
-
- std::ifstream file_if(filename.c_str());
- if (!file_if.good())
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Provided file does not exist: %s",
- filename.c_str());
- return return_string;
- }
+ const std::string& filename) {
+ std::string return_string = "";
- try
- {
- if (!script_client_->wait_for_service(timeout_connection_s_))
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Connection to remote server lost. Reconnecting...");
- connect_to_rosservice_run_python_script(timeout_connection_s_);
- return return_string;
- }
-
- run_file_request_->input = filename;
-
- auto response = script_client_->async_send_request(run_file_request_);
-
- if (rclcpp::spin_until_future_complete(ros_node_, response) ==
- rclcpp::FutureReturnCode::SUCCESS)
- {
- // Get the error.
- return_string += response.get()->result;
-
- // Get the Result and print it is any.
- return_string += response.get()->result;
- if (!boost::algorithm::ends_with(return_string, "\n"))
- {
- return_string += "\n";
- }
- }
- else
- {
- // We had an issue calling the service.
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Error while parsing scripts.");
- }
+ std::ifstream file_if(filename.c_str());
+ if (!file_if.good()) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Provided file does not exist: %s", filename.c_str());
+ return return_string;
+ }
+
+ try {
+ if (!script_client_->wait_for_service(timeout_connection_s_)) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_script(timeout_connection_s_);
+ return return_string;
}
- catch (...)
- {
- RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
- "Connection to remote server lost. Reconnecting...");
- connect_to_rosservice_run_python_script(timeout_connection_s_);
+
+ run_file_request_->input = filename;
+
+ auto response = script_client_->async_send_request(run_file_request_);
+
+ if (rclcpp::spin_until_future_complete(ros_node_, response) ==
+ rclcpp::FutureReturnCode::SUCCESS) {
+ // Get the error.
+ return_string += response.get()->result;
+
+ // Get the Result and print it is any.
+ return_string += response.get()->result;
+ if (!boost::algorithm::ends_with(return_string, "\n")) {
+ return_string += "\n";
+ }
+ } else {
+ // We had an issue calling the service.
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Error while parsing scripts.");
}
- return return_string;
+ } catch (...) {
+ RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),
+ "Connection to remote server lost. Reconnecting...");
+ connect_to_rosservice_run_python_script(timeout_connection_s_);
+ }
+ return return_string;
}
} // namespace dynamic_graph_bridge
diff --git a/src/ros_python_interpreter_server.cpp b/src/ros_python_interpreter_server.cpp
index aa7ddb4b9963f30486eceeec8e95276c85363af3..22c2d2361911d04c75bee72f06a7671d496cd882 100644
--- a/src/ros_python_interpreter_server.cpp
+++ b/src/ros_python_interpreter_server.cpp
@@ -13,84 +13,68 @@
#include <functional>
#include <memory>
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
static const int queueSize = 5;
RosPythonInterpreterServer::RosPythonInterpreterServer()
: interpreter_(),
ros_node_(get_ros_node("python_interpreter")),
run_python_command_srv_(nullptr),
- run_python_file_srv_(nullptr)
-{
- ros_add_node_to_executor("python_interpreter");
+ run_python_file_srv_(nullptr) {
+ ros_add_node_to_executor("python_interpreter");
}
-RosPythonInterpreterServer::~RosPythonInterpreterServer()
-{
-}
+RosPythonInterpreterServer::~RosPythonInterpreterServer() {}
-void RosPythonInterpreterServer::start_ros_service()
-{
- run_python_command_callback_t runCommandCb =
- std::bind(&RosPythonInterpreterServer::runCommandCallback,
- this,
- std::placeholders::_1,
- std::placeholders::_2);
- run_python_command_srv_ =
- ros_node_->create_service<RunPythonCommandSrvType>("run_python_command",
- runCommandCb);
+void RosPythonInterpreterServer::start_ros_service() {
+ run_python_command_callback_t runCommandCb =
+ std::bind(&RosPythonInterpreterServer::runCommandCallback, this,
+ std::placeholders::_1, std::placeholders::_2);
+ run_python_command_srv_ = ros_node_->create_service<RunPythonCommandSrvType>(
+ "run_python_command", runCommandCb);
- run_python_file_callback_t runPythonFileCb =
- std::bind(&RosPythonInterpreterServer::runPythonFileCallback,
- this,
- std::placeholders::_1,
- std::placeholders::_2);
- run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
- "run_python_file", runPythonFileCb);
+ run_python_file_callback_t runPythonFileCb =
+ std::bind(&RosPythonInterpreterServer::runPythonFileCallback, this,
+ std::placeholders::_1, std::placeholders::_2);
+ run_python_file_srv_ = ros_node_->create_service<RunPythonFileSrvType>(
+ "run_python_file", runPythonFileCb);
}
void RosPythonInterpreterServer::runCommandCallback(
- RunPythonCommandRequestPtr req, RunPythonCommandResponsePtr res)
-{
- std::stringstream buffer;
- std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
+ RunPythonCommandRequestPtr req, RunPythonCommandResponsePtr res) {
+ std::stringstream buffer;
+ std::streambuf* old = std::cout.rdbuf(buffer.rdbuf());
- run_python_command(
- req->input, res->result, res->standardoutput, res->standarderror);
+ run_python_command(req->input, res->result, res->standardoutput,
+ res->standarderror);
- if (!buffer.str().empty())
- {
- if (!boost::algorithm::ends_with(res->standardoutput, "\n") &&
- !res->standardoutput.empty())
- {
- res->standardoutput += "\n";
- }
- res->standardoutput += buffer.str();
+ if (!buffer.str().empty()) {
+ if (!boost::algorithm::ends_with(res->standardoutput, "\n") &&
+ !res->standardoutput.empty()) {
+ res->standardoutput += "\n";
}
- std::cout.rdbuf(old);
+ res->standardoutput += buffer.str();
+ }
+ std::cout.rdbuf(old);
- std::cout << res->standardoutput << std::endl;
+ std::cout << res->standardoutput << std::endl;
}
void RosPythonInterpreterServer::runPythonFileCallback(
- RunPythonFileRequestPtr req, RunPythonFileResponsePtr res)
-{
- run_python_file(req->input, res->result);
+ RunPythonFileRequestPtr req, RunPythonFileResponsePtr res) {
+ run_python_file(req->input, res->result);
}
void RosPythonInterpreterServer::run_python_command(const std::string& command,
std::string& result,
std::string& out,
- std::string& err)
-{
- interpreter_.python(command, result, out, err);
+ std::string& err) {
+ interpreter_.python(command, result, out, err);
}
void RosPythonInterpreterServer::run_python_file(const std::string ifilename,
- std::string& result)
-{
- interpreter_.runPythonFile(ifilename, result);
+ std::string& result) {
+ interpreter_.runPythonFile(ifilename, result);
}
} // namespace dynamic_graph_bridge
diff --git a/src/ros_subscribe-python-module-py.cc b/src/ros_subscribe-python-module-py.cc
index 338cf5aa38cba3914a2e719ad47019c1762eb2d8..77cf38bd39e2b7383ebc6dfe3dd548eaf8a4c41a 100644
--- a/src/ros_subscribe-python-module-py.cc
+++ b/src/ros_subscribe-python-module-py.cc
@@ -1,11 +1,11 @@
#include <dynamic-graph/python/module.hh>
+
#include "ros_subscribe.hpp"
namespace dgb = dynamic_graph_bridge;
namespace dg = dynamicgraph;
-BOOST_PYTHON_MODULE(wrap)
-{
+BOOST_PYTHON_MODULE(wrap) {
bp::import("dynamic_graph");
dg::python::exposeEntity<dgb::RosSubscribe, bp::bases<dg::Entity>,
diff --git a/src/ros_subscribe.cpp b/src/ros_subscribe.cpp
index 0f787ad993eb287fb0e6074b18ec93510044174a..093fb3e612308ce22b1cca571d5b3fce22fba8ae 100644
--- a/src/ros_subscribe.cpp
+++ b/src/ros_subscribe.cpp
@@ -7,12 +7,13 @@
* @date 2019-05-22
*/
-#include "fwd.hpp"
-#include <dynamic-graph/factory.h>
#include "ros_subscribe.hpp"
-namespace dynamic_graph_bridge
-{
+#include <dynamic-graph/factory.h>
+
+#include "fwd.hpp"
+
+namespace dynamic_graph_bridge {
/*
* RosSubscribe
*/
@@ -23,227 +24,172 @@ const std::string RosSubscribe::doc_string_(
"\n"
" Use command \"add\" to subscribe to a new signal.\n");
-RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n)
-{
- ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
- ros_add_node_to_executor(DG_ROS_NODE_NAME);
- ros_spin_non_blocking();
- binded_signals_.clear();
- std::string doc_string =
- "\n"
- " Add a signal reading data from a ROS topic.\n"
- "\n"
- " Input:\n"
- " - type: string among:\n";
- for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
- {
- doc_string += " - " + DgRosTypes::type_list[i] + "\n";
- }
- doc_string +=
- " - signal: the signal name in dynamic-graph,\n"
- " - topic: the topic name in ROS.\n"
- "\n";
- addCommand("add", new command::ros_subscribe::Add(*this, doc_string));
- doc_string =
- "\n"
- " Remove a signal reading data from a ROS topic\n"
- "\n"
- " Input:\n"
- " - name of the signal to remove (see method list for the list of "
- "signals).\n"
- "\n";
- addCommand("rm", new command::ros_subscribe::Rm(*this, doc_string));
- doc_string =
- "\n"
- " Remove all signals reading data from a ROS topic\n"
- "\n"
- " No input:\n"
- "\n";
- addCommand("clear", new command::ros_subscribe::Clear(*this, doc_string));
- doc_string =
- "\n"
- " List signals reading data from a ROS topic\n"
- "\n"
- " No input:\n"
- "\n";
- addCommand("list", new command::ros_subscribe::List(*this, doc_string));
+RosSubscribe::RosSubscribe(const std::string& n) : dynamicgraph::Entity(n) {
+ ros_node_ = get_ros_node(DG_ROS_NODE_NAME);
+ ros_add_node_to_executor(DG_ROS_NODE_NAME);
+ ros_spin_non_blocking();
+ binded_signals_.clear();
+ std::string doc_string =
+ "\n"
+ " Add a signal reading data from a ROS topic.\n"
+ "\n"
+ " Input:\n"
+ " - type: string among:\n";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i) {
+ doc_string += " - " + DgRosTypes::type_list[i] + "\n";
+ }
+ doc_string +=
+ " - signal: the signal name in dynamic-graph,\n"
+ " - topic: the topic name in ROS.\n"
+ "\n";
+ addCommand("add", new command::ros_subscribe::Add(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove a signal reading data from a ROS topic\n"
+ "\n"
+ " Input:\n"
+ " - name of the signal to remove (see method list for the list of "
+ "signals).\n"
+ "\n";
+ addCommand("rm", new command::ros_subscribe::Rm(*this, doc_string));
+ doc_string =
+ "\n"
+ " Remove all signals reading data from a ROS topic\n"
+ "\n"
+ " No input:\n"
+ "\n";
+ addCommand("clear", new command::ros_subscribe::Clear(*this, doc_string));
+ doc_string =
+ "\n"
+ " List signals reading data from a ROS topic\n"
+ "\n"
+ " No input:\n"
+ "\n";
+ addCommand("list", new command::ros_subscribe::List(*this, doc_string));
}
-RosSubscribe::~RosSubscribe()
-{
- clear();
-}
+RosSubscribe::~RosSubscribe() { clear(); }
-void RosSubscribe::display(std::ostream& os) const
-{
- os << CLASS_NAME << std::endl;
+void RosSubscribe::display(std::ostream& os) const {
+ os << CLASS_NAME << std::endl;
}
-void RosSubscribe::rm(const std::string& signal)
-{
- std::string signalTs = signal + "Timestamp";
+void RosSubscribe::rm(const std::string& signal) {
+ std::string signalTs = signal + "Timestamp";
- signalDeregistration(signal);
- binded_signals_.erase(signal);
+ signalDeregistration(signal);
+ binded_signals_.erase(signal);
- if (binded_signals_.find(signalTs) != binded_signals_.end())
- {
- signalDeregistration(signalTs);
- binded_signals_.erase(signalTs);
- }
+ if (binded_signals_.find(signalTs) != binded_signals_.end()) {
+ signalDeregistration(signalTs);
+ binded_signals_.erase(signalTs);
+ }
}
-std::string RosSubscribe::list()
-{
- std::string result("[");
- for (std::map<std::string, BindedSignal>::const_iterator it =
- binded_signals_.begin();
- it != binded_signals_.end();
- it++)
- {
- result += "'" + it->first + "',";
- }
- result += "]";
- return result;
+std::string RosSubscribe::list() {
+ std::string result("[");
+ for (std::map<std::string, BindedSignal>::const_iterator it =
+ binded_signals_.begin();
+ it != binded_signals_.end(); it++) {
+ result += "'" + it->first + "',";
+ }
+ result += "]";
+ return result;
}
-void RosSubscribe::clear()
-{
- std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
- for (; it != binded_signals_.end();)
- {
- rm(it->first);
- it = binded_signals_.begin();
- }
+void RosSubscribe::clear() {
+ std::map<std::string, BindedSignal>::iterator it = binded_signals_.begin();
+ for (; it != binded_signals_.end();) {
+ rm(it->first);
+ it = binded_signals_.begin();
+ }
}
-std::string RosSubscribe::getDocString() const
-{
- return doc_string_;
-}
+std::string RosSubscribe::getDocString() const { return doc_string_; }
/*
* Commands
*/
-namespace command
-{
-namespace ros_subscribe
-{
+namespace command {
+namespace ros_subscribe {
Clear::Clear(RosSubscribe& entity, const std::string& doc_string)
- : Command(entity, std::vector<Value::Type>(), doc_string)
-{
-}
+ : Command(entity, std::vector<Value::Type>(), doc_string) {}
-Value Clear::doExecute()
-{
- RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+Value Clear::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
- entity.clear();
- return Value();
+ entity.clear();
+ return Value();
}
List::List(RosSubscribe& entity, const std::string& doc_string)
- : Command(entity, std::vector<Value::Type>(), doc_string)
-{
-}
+ : Command(entity, std::vector<Value::Type>(), doc_string) {}
-Value List::doExecute()
-{
- RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
- return Value(entity.list());
+Value List::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ return Value(entity.list());
}
Add::Add(RosSubscribe& entity, const std::string& doc_string)
: Command(
entity,
boost::assign::list_of(Value::STRING)(Value::STRING)(Value::STRING),
- doc_string)
-{
-}
+ doc_string) {}
-Value Add::doExecute()
-{
+Value Add::doExecute() {
using namespace dynamicgraph;
- RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
- std::vector<Value> values = getParameterValues();
-
- const std::string& type = values[0].value();
- const std::string& signal_name = values[1].value();
- const std::string& topic_name = values[2].value();
-
- if (type == DgRosTypes::get_double())
- {
- entity.add<std_msgs::msg::Float64, double>(signal_name, topic_name);
- }
- else if (type == DgRosTypes::get_unsigned())
- {
- entity.add<std_msgs::msg::UInt32, unsigned int>(signal_name,
- topic_name);
- }
- else if (type == DgRosTypes::get_matrix())
- {
- entity.add<RosMatrix, DgMatrix>(signal_name, topic_name);
- }
- else if (type == DgRosTypes::get_vector())
- {
- entity.add<RosVector, DgVector>(signal_name, topic_name);
- }
- else if (type == DgRosTypes::get_vector3())
- {
- entity.add<geometry_msgs::msg::Vector3, DgVector>(signal_name,
- topic_name);
- }
- else if (type == DgRosTypes::get_vector3_stamped())
- {
- entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal_name,
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+
+ const std::string& type = values[0].value();
+ const std::string& signal_name = values[1].value();
+ const std::string& topic_name = values[2].value();
+
+ if (type == DgRosTypes::get_double()) {
+ entity.add<std_msgs::msg::Float64, double>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_unsigned()) {
+ entity.add<std_msgs::msg::UInt32, unsigned int>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_matrix()) {
+ entity.add<RosMatrix, DgMatrix>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_vector()) {
+ entity.add<RosVector, DgVector>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_vector3()) {
+ entity.add<geometry_msgs::msg::Vector3, DgVector>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_vector3_stamped()) {
+ entity.add<geometry_msgs::msg::Vector3Stamped, DgVector>(signal_name,
+ topic_name);
+ } else if (type == DgRosTypes::get_matrix_homogeneous()) {
+ entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(signal_name,
topic_name);
+ } else if (type == DgRosTypes::get_matrix_homogeneous_stamped()) {
+ entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(
+ signal_name, topic_name);
+ } else if (type == DgRosTypes::get_twist()) {
+ entity.add<geometry_msgs::msg::Twist, DgVector>(signal_name, topic_name);
+ } else if (type == DgRosTypes::get_twist_stamped()) {
+ entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal_name,
+ topic_name);
+ } else {
+ std::cerr << "RosSubscribe(" << entity.getName()
+ << ")::add(): bad type given (" << type << ")."
+ << " Possible choice is among:";
+ for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i) {
+ std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
}
- else if (type == DgRosTypes::get_matrix_homogeneous())
- {
- entity.add<geometry_msgs::msg::Transform, MatrixHomogeneous>(
- signal_name, topic_name);
- }
- else if (type == DgRosTypes::get_matrix_homogeneous_stamped())
- {
- entity.add<geometry_msgs::msg::TransformStamped, MatrixHomogeneous>(
- signal_name, topic_name);
- }
- else if (type == DgRosTypes::get_twist())
- {
- entity.add<geometry_msgs::msg::Twist, DgVector>(signal_name,
- topic_name);
- }
- else if (type == DgRosTypes::get_twist_stamped())
- {
- entity.add<geometry_msgs::msg::TwistStamped, DgVector>(signal_name,
- topic_name);
- }
- else
- {
- std::cerr << "RosSubscribe(" << entity.getName()
- << ")::add(): bad type given (" << type << ")."
- << " Possible choice is among:";
- for (unsigned int i = 0; i < DgRosTypes::type_list.size(); ++i)
- {
- std::cerr << " - " << DgRosTypes::type_list[i] << std::endl;
- }
- }
- return Value();
+ }
+ return Value();
}
Rm::Rm(RosSubscribe& entity, const std::string& doc_string)
- : Command(entity, boost::assign::list_of(Value::STRING), doc_string)
-{
-}
-
-Value Rm::doExecute()
-{
- RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
- std::vector<Value> values = getParameterValues();
- const std::string& signal = values[0].value();
- entity.rm(signal);
- return Value();
+ : Command(entity, boost::assign::list_of(Value::STRING), doc_string) {}
+
+Value Rm::doExecute() {
+ RosSubscribe& entity = static_cast<RosSubscribe&>(owner());
+ std::vector<Value> values = getParameterValues();
+ const std::string& signal = values[0].value();
+ entity.rm(signal);
+ return Value();
}
} // namespace ros_subscribe
} // end of namespace command.
diff --git a/src/ros_subscribe.hpp b/src/ros_subscribe.hpp
index c00bfa96e2eba69ac99bdad3288cb5beb534bc08..bf2dc0ee9d3214415b45052b422dd235fdcd8876 100644
--- a/src/ros_subscribe.hpp
+++ b/src/ros_subscribe.hpp
@@ -10,7 +10,6 @@
#pragma once
-#include "fwd.hpp"
#include <dynamic-graph/command.h>
#include <dynamic-graph/entity.h>
#include <dynamic-graph/signal-ptr.h>
@@ -20,128 +19,124 @@
#include <mutex>
#include "dg_ros_mapping.hpp"
+#include "fwd.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
/**
* @brief Publish ROS information in the dynamic-graph.
*/
-class RosSubscribe : public dynamicgraph::Entity
-{
- DYNAMIC_GRAPH_ENTITY_DECL();
-
-public:
- /**
- * @brief Tuple composed by the generated input signal and its callback
- * function. The callback function publishes the input signal content.
- */
- typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
- std::shared_ptr<dynamicgraph::SignalBase<int> >,
- rclcpp::SubscriptionBase::SharedPtr>
- BindedSignal;
-
- /**
- * @brief Construct a new Ros Subscribe object
- *
- * @param name entity name.
- */
- RosSubscribe(const std::string& name);
-
- /**
- * @brief Destroy the Ros Subscribe object.
- */
- virtual ~RosSubscribe();
-
- /**
- * @brief Get Doc String.
- *
- * @return std::string
- */
- virtual std::string getDocString() const;
-
- /**
- * @brief Display information about the entity.
- *
- * @param os
- */
- void display(std::ostream& os) const;
-
- /**
- * @brief Subscribe to a topic and add a signal containing the topic data.
- *
- * @tparam RosType
- * @tparam DgType
- * @param signal_name
- * @param topic_name
- */
- template <typename RosType, typename DgType>
- void add(const std::string& signal_name, const std::string& topic_name);
-
- /**
- * @brief Unsubscribe to a topic and remove the signal.
- *
- * @param signal
- */
- void rm(const std::string& signal);
-
- /**
- * @brief List of all subscribed topic and singals.
- *
- * @return std::string
- */
- std::string list();
-
- /**
- * @brief Unsubscribe to all topics and remove all signals.
- *
- */
- void clear();
-
-private:
- /**
- * @brief This callback feeds the data signal upon reception of a ROS
- * message.
- *
- * @tparam RosType
- * @tparam DgType
- * @param signal pointer to the dynamic graph signal.
- * @param data ROS data to copy from.
- */
- template <typename RosType, typename DgType>
- void callback(
- std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_out_t>
- signal_out,
- std::shared_ptr<
- typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t>
- signal_timestamp_out,
- const std::shared_ptr<typename DgRosMapping<RosType, DgType>::ros_t>
- ros_data);
-
-private:
- /** @brief Doc string associated to the entity. */
- static const std::string doc_string_;
-
- /** @brief ROS node pointer. */
- RosNodePtr ros_node_;
-
- /** @brief Named list of signals associated with it's callback functions. */
- std::map<std::string, BindedSignal> binded_signals_;
+class RosSubscribe : public dynamicgraph::Entity {
+ DYNAMIC_GRAPH_ENTITY_DECL();
+
+ public:
+ /**
+ * @brief Tuple composed by the generated input signal and its callback
+ * function. The callback function publishes the input signal content.
+ */
+ typedef std::tuple<std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ std::shared_ptr<dynamicgraph::SignalBase<int> >,
+ rclcpp::SubscriptionBase::SharedPtr>
+ BindedSignal;
+
+ /**
+ * @brief Construct a new Ros Subscribe object
+ *
+ * @param name entity name.
+ */
+ RosSubscribe(const std::string& name);
+
+ /**
+ * @brief Destroy the Ros Subscribe object.
+ */
+ virtual ~RosSubscribe();
+
+ /**
+ * @brief Get Doc String.
+ *
+ * @return std::string
+ */
+ virtual std::string getDocString() const;
+
+ /**
+ * @brief Display information about the entity.
+ *
+ * @param os
+ */
+ void display(std::ostream& os) const;
+
+ /**
+ * @brief Subscribe to a topic and add a signal containing the topic data.
+ *
+ * @tparam RosType
+ * @tparam DgType
+ * @param signal_name
+ * @param topic_name
+ */
+ template <typename RosType, typename DgType>
+ void add(const std::string& signal_name, const std::string& topic_name);
+
+ /**
+ * @brief Unsubscribe to a topic and remove the signal.
+ *
+ * @param signal
+ */
+ void rm(const std::string& signal);
+
+ /**
+ * @brief List of all subscribed topic and singals.
+ *
+ * @return std::string
+ */
+ std::string list();
+
+ /**
+ * @brief Unsubscribe to all topics and remove all signals.
+ *
+ */
+ void clear();
+
+ private:
+ /**
+ * @brief This callback feeds the data signal upon reception of a ROS
+ * message.
+ *
+ * @tparam RosType
+ * @tparam DgType
+ * @param signal pointer to the dynamic graph signal.
+ * @param data ROS data to copy from.
+ */
+ template <typename RosType, typename DgType>
+ void callback(
+ std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_out_t>
+ signal_out,
+ std::shared_ptr<
+ typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t>
+ signal_timestamp_out,
+ const std::shared_ptr<typename DgRosMapping<RosType, DgType>::ros_t>
+ ros_data);
+
+ private:
+ /** @brief Doc string associated to the entity. */
+ static const std::string doc_string_;
+
+ /** @brief ROS node pointer. */
+ RosNodePtr ros_node_;
+
+ /** @brief Named list of signals associated with it's callback functions. */
+ std::map<std::string, BindedSignal> binded_signals_;
};
-namespace command
-{
-namespace ros_subscribe
-{
+namespace command {
+namespace ros_subscribe {
using ::dynamicgraph::command::Command;
using ::dynamicgraph::command::Value;
-#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
- class CMD : public Command \
- { \
- public: \
- CMD(RosSubscribe& entity, const std::string& doc_string); \
- virtual Value doExecute(); \
- }
+#define ROS_SUBSCRIBE_MAKE_COMMAND(CMD) \
+ class CMD : public Command { \
+ public: \
+ CMD(RosSubscribe& entity, const std::string& doc_string); \
+ virtual Value doExecute(); \
+ }
ROS_SUBSCRIBE_MAKE_COMMAND(Add);
ROS_SUBSCRIBE_MAKE_COMMAND(Clear);
diff --git a/src/ros_subscribe.hxx b/src/ros_subscribe.hxx
index c3a4b412a1cb61c53dad2d0e7203f3161e4e3004..40fd488ad2ba96dfabb820aec8f66a257aa957f3 100644
--- a/src/ros_subscribe.hxx
+++ b/src/ros_subscribe.hxx
@@ -13,8 +13,7 @@
#include "dg_ros_mapping.hpp"
-namespace dynamic_graph_bridge
-{
+namespace dynamic_graph_bridge {
template <typename RosType, typename DgType>
void RosSubscribe::callback(
std::shared_ptr<typename DgRosMapping<RosType, DgType>::signal_out_t>
@@ -23,90 +22,82 @@ void RosSubscribe::callback(
typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t>
signal_timestamp_out,
const std::shared_ptr<typename DgRosMapping<RosType, DgType>::ros_t>
- ros_data)
-{
- // Some convenient shortcut.
- using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
- using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
+ ros_data) {
+ // Some convenient shortcut.
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
- // Create a dynamic graph object to copy the information in. Not real time
- // safe?
- dg_t value;
- // Convert the ROS topic into a dynamic graph type.
- DgRosMapping<RosType, DgType>::ros_to_dg(*ros_data, value);
- // Update the signal.
- signal_out->setConstant(value);
- // If he ROS message has a header then copy the header into the timestamp
- // signal
- if (signal_timestamp_out)
- {
- signal_timestamp_out->setConstant(
- DgRosMapping<RosType, DgType>::from_ros_time(
- message_filters::message_traits::TimeStamp<ros_t>::value(
- *ros_data)));
- }
+ // Create a dynamic graph object to copy the information in. Not real time
+ // safe?
+ dg_t value;
+ // Convert the ROS topic into a dynamic graph type.
+ DgRosMapping<RosType, DgType>::ros_to_dg(*ros_data, value);
+ // Update the signal.
+ signal_out->setConstant(value);
+ // If he ROS message has a header then copy the header into the timestamp
+ // signal
+ if (signal_timestamp_out) {
+ signal_timestamp_out->setConstant(
+ DgRosMapping<RosType, DgType>::from_ros_time(
+ message_filters::message_traits::TimeStamp<ros_t>::value(
+ *ros_data)));
+ }
}
template <typename RosType, typename DgType>
void RosSubscribe::add(const std::string& signal_name,
- const std::string& topic_name)
-{
- using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
- using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
- using signal_out_t = typename DgRosMapping<RosType, DgType>::signal_out_t;
- using signal_timestamp_out_t =
- typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t;
+ const std::string& topic_name) {
+ using dg_t = typename DgRosMapping<RosType, DgType>::dg_t;
+ using ros_t = typename DgRosMapping<RosType, DgType>::ros_t;
+ using signal_out_t = typename DgRosMapping<RosType, DgType>::signal_out_t;
+ using signal_timestamp_out_t =
+ typename DgRosMapping<RosType, DgType>::signal_timestamp_out_t;
- if (binded_signals_.find(signal_name) != binded_signals_.end())
- {
- std::cout << "Signal already created, nothing to be done." << std::endl;
- return;
- }
+ if (binded_signals_.find(signal_name) != binded_signals_.end()) {
+ std::cout << "Signal already created, nothing to be done." << std::endl;
+ return;
+ }
- // Initialize the binded_signal object.
- RosSubscribe::BindedSignal binded_signal;
+ // Initialize the binded_signal object.
+ RosSubscribe::BindedSignal binded_signal;
- // Initialize the signal.
- std::string full_signal_name =
- this->getClassName() + "(" + this->getName() + ")::" + signal_name;
- std::shared_ptr<signal_out_t> signal_ptr =
- std::make_shared<signal_out_t>(full_signal_name);
- DgRosMapping<RosType, DgType>::set_default(signal_ptr);
- signal_ptr->setDependencyType(
- dynamicgraph::TimeDependency<int>::ALWAYS_READY);
- std::get<0>(binded_signal) = signal_ptr;
- this->signalRegistration(*std::get<0>(binded_signal));
+ // Initialize the signal.
+ std::string full_signal_name =
+ this->getClassName() + "(" + this->getName() + ")::" + signal_name;
+ std::shared_ptr<signal_out_t> signal_ptr =
+ std::make_shared<signal_out_t>(full_signal_name);
+ DgRosMapping<RosType, DgType>::set_default(signal_ptr);
+ signal_ptr->setDependencyType(
+ dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ std::get<0>(binded_signal) = signal_ptr;
+ this->signalRegistration(*std::get<0>(binded_signal));
- // Initialize the time stamp signal if needed.
- std::shared_ptr<signal_timestamp_out_t> signal_timestamp_ptr = nullptr;
- if (message_filters::message_traits::HasHeader<ros_t>())
- {
- std::string full_time_stamp_signal_name =
- this->getClassName() + "(" + this->getName() + ")::" + signal_name +
- "_timestamp";
- signal_timestamp_ptr = std::make_shared<signal_timestamp_out_t>(
- full_time_stamp_signal_name);
- signal_timestamp_ptr->setConstant(
- DgRosMapping<RosType, DgType>::epoch_time());
- signal_timestamp_ptr->setDependencyType(
- dynamicgraph::TimeDependency<int>::ALWAYS_READY);
- this->signalRegistration(*signal_timestamp_ptr);
- }
- std::get<1>(binded_signal) = signal_timestamp_ptr;
+ // Initialize the time stamp signal if needed.
+ std::shared_ptr<signal_timestamp_out_t> signal_timestamp_ptr = nullptr;
+ if (message_filters::message_traits::HasHeader<ros_t>()) {
+ std::string full_time_stamp_signal_name =
+ this->getClassName() + "(" + this->getName() + ")::" + signal_name +
+ "_timestamp";
+ signal_timestamp_ptr =
+ std::make_shared<signal_timestamp_out_t>(full_time_stamp_signal_name);
+ signal_timestamp_ptr->setConstant(
+ DgRosMapping<RosType, DgType>::epoch_time());
+ signal_timestamp_ptr->setDependencyType(
+ dynamicgraph::TimeDependency<int>::ALWAYS_READY);
+ this->signalRegistration(*signal_timestamp_ptr);
+ }
+ std::get<1>(binded_signal) = signal_timestamp_ptr;
- // Initialize the subscriber.
- std::function<void(const std::shared_ptr<ros_t>)> callback =
- std::bind(&RosSubscribe::callback<ros_t, dg_t>,
- this,
- signal_ptr,
- signal_timestamp_ptr,
- std::placeholders::_1);
- typename rclcpp::Subscription<ros_t>::SharedPtr sub =
- ros_node_->create_subscription<ros_t>(topic_name, 10, callback);
- std::get<2>(binded_signal) = sub;
+ // Initialize the subscriber.
+ std::function<void(const std::shared_ptr<ros_t>)> callback =
+ std::bind(&RosSubscribe::callback<ros_t, dg_t>, this, signal_ptr,
+ signal_timestamp_ptr, std::placeholders::_1);
+ typename rclcpp::Subscription<ros_t>::SharedPtr sub =
+ ros_node_->create_subscription<ros_t>(topic_name, 10, callback);
+ std::get<2>(binded_signal) = sub;
- // Store the different pointers.
- binded_signals_[signal_name] = binded_signal;
+ // Store the different pointers.
+ binded_signals_[signal_name] = binded_signal;
}
} // namespace dynamic_graph_bridge
diff --git a/src/sot_loader.cpp b/src/sot_loader.cpp
index 81e57bada1d3a35320cc07243dceb95964d226c3..923a2a6e31d8f329a791c2d616afbe8527cdb150 100644
--- a/src/sot_loader.cpp
+++ b/src/sot_loader.cpp
@@ -10,9 +10,10 @@
/* -------------------------------------------------------------------------- */
#include <dynamic_graph_bridge/sot_loader.hh>
-#include "dynamic_graph_bridge/ros.hpp"
#include <std_msgs/msg/string.hpp>
+#include "dynamic_graph_bridge/ros.hpp"
+
// POSIX.1-2001
#include <dlfcn.h>
@@ -29,17 +30,11 @@ struct DataToLog {
std::vector<double> ittimes;
DataToLog(std::size_t N_)
- : N(N_)
- , idx(0)
- , iter(0)
- , iters(N, 0)
- , times(N, 0)
- , ittimes(N, 0)
- {}
+ : N(N_), idx(0), iter(0), iters(N, 0), times(N, 0), ittimes(N, 0) {}
void record(const double t, const double itt) {
- iters [idx] = iter;
- times [idx] = t;
+ iters[idx] = iter;
+ times[idx] = t;
ittimes[idx] = itt;
++idx;
++iter;
@@ -53,17 +48,14 @@ struct DataToLog {
std::ofstream aof(oss.str().c_str());
if (aof.is_open()) {
for (std::size_t k = 0; k < N; ++k) {
- aof
- << iters [(idx + k) % N] << ' '
- << times [(idx + k) % N] << ' '
- << ittimes[(idx + k) % N] << '\n';
+ aof << iters[(idx + k) % N] << ' ' << times[(idx + k) % N] << ' '
+ << ittimes[(idx + k) % N] << '\n';
}
}
aof.close();
}
};
-
SotLoader::SotLoader()
: SotLoaderBasic(),
sensorsIn_(),
@@ -74,9 +66,7 @@ SotLoader::SotLoader()
torques_(),
baseAtt_(),
accelerometer_(3),
- gyrometer_(3) {
-}
-
+ gyrometer_(3) {}
SotLoader::~SotLoader() {
dynamic_graph_stopped_ = true;
@@ -84,7 +74,7 @@ SotLoader::~SotLoader() {
}
void SotLoader::startControlLoop() {
- thread_ = std::make_shared<std::thread>(&SotLoader::workThreadLoader,this);
+ thread_ = std::make_shared<std::thread>(&SotLoader::workThreadLoader, this);
}
void SotLoader::initializeServices() {
@@ -93,19 +83,19 @@ void SotLoader::initializeServices() {
freeFlyerPose_.header.frame_id = "odom";
freeFlyerPose_.child_frame_id = "base_link";
- if (nh_==0) {
- logic_error aLogicError("SotLoaderBasic::initializeFromRosContext aRosCtxt is empty !");
+ if (nh_ == 0) {
+ logic_error aLogicError(
+ "SotLoaderBasic::initializeFromRosContext aRosCtxt is empty !");
throw aLogicError;
}
if (not nh_->has_parameter("tf_base_link"))
- nh_->declare_parameter("tf_base_link",std::string("base_link"));
+ nh_->declare_parameter("tf_base_link", std::string("base_link"));
- if (nh_->get_parameter("tf_base_link",
- freeFlyerPose_.child_frame_id)) {
+ if (nh_->get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
RCLCPP_INFO_STREAM(rclcpp::get_logger("dynamic_graph_bridge"),
- "Publishing " << freeFlyerPose_.child_frame_id <<
- " wrt " << freeFlyerPose_.header.frame_id);
+ "Publishing " << freeFlyerPose_.child_frame_id << " wrt "
+ << freeFlyerPose_.header.frame_id);
}
// Temporary fix. TODO: where should nbOfJoints_ be initialized from?
@@ -114,8 +104,7 @@ void SotLoader::initializeServices() {
control_.resize(static_cast<std::size_t>(nbOfJoints_));
// Creates a publisher for the free flyer transform.
- freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>
- (nh_);
+ freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>(nh_);
}
void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
@@ -123,7 +112,8 @@ void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
assert(control_.size() == angleEncoder_.size());
sensorsIn["joints"].setName("angle");
- for (unsigned int i = 0; i < control_.size(); i++) angleEncoder_[i] = control_[i];
+ for (unsigned int i = 0; i < control_.size(); i++)
+ angleEncoder_[i] = control_[i];
sensorsIn["joints"].setValues(angleEncoder_);
}
@@ -131,16 +121,17 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
// Update joint values.
std::map<std::string, dgs::ControlValues>::iterator it_ctrl_map;
it_ctrl_map = controlValues.find("control");
- if (it_ctrl_map == controlValues.end())
- {
- invalid_argument anInvalidArgument("control is not present in controlValues !");
- throw anInvalidArgument;
- }
+ if (it_ctrl_map == controlValues.end()) {
+ invalid_argument anInvalidArgument(
+ "control is not present in controlValues !");
+ throw anInvalidArgument;
+ }
control_ = controlValues["control"].getValues();
#ifdef NDEBUG
// Debug
- std::map<std::string, dgs::ControlValues>::iterator it = controlValues.begin();
+ std::map<std::string, dgs::ControlValues>::iterator it =
+ controlValues.begin();
sotDEBUG(30) << "ControlValues to be broadcasted:" << std::endl;
for (; it != controlValues.end(); it++) {
sotDEBUG(30) << it->first << ":";
@@ -162,35 +153,38 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
joint_state_.name = joint_state_names;
joint_state_.header.stamp = rclcpp::Clock().now();
- if (joint_state_.position.size()!=nbOfJoints_+parallel_joints_to_state_vector_.size())
- {
- joint_state_.position.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
- joint_state_.velocity.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
- joint_state_.effort.resize(nbOfJoints_+parallel_joints_to_state_vector_.size());
- }
+ if (joint_state_.position.size() !=
+ nbOfJoints_ + parallel_joints_to_state_vector_.size()) {
+ joint_state_.position.resize(nbOfJoints_ +
+ parallel_joints_to_state_vector_.size());
+ joint_state_.velocity.resize(nbOfJoints_ +
+ parallel_joints_to_state_vector_.size());
+ joint_state_.effort.resize(nbOfJoints_ +
+ parallel_joints_to_state_vector_.size());
+ }
for (int i = 0; i < nbOfJoints_; i++) {
joint_state_.position[i] = angleEncoder_[i];
}
std::cerr << "parallel_joints_to_state_vector_.size(): "
- << parallel_joints_to_state_vector_.size()
- << std::endl;
+ << parallel_joints_to_state_vector_.size() << std::endl;
for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
joint_state_.position[i + nbOfJoints_] =
- coefficient_parallel_joints_[i] * angleEncoder_[parallel_joints_to_state_vector_[i]];
+ coefficient_parallel_joints_[i] *
+ angleEncoder_[parallel_joints_to_state_vector_[i]];
}
joint_pub_->publish(joint_state_);
// Get the estimation of the robot base.
it_ctrl_map = controlValues.find("baseff");
- if (it_ctrl_map == controlValues.end())
- {
- invalid_argument anInvalidArgument("baseff is not present in controlValues !");
- throw anInvalidArgument;
- }
+ if (it_ctrl_map == controlValues.end()) {
+ invalid_argument anInvalidArgument(
+ "baseff is not present in controlValues !");
+ throw anInvalidArgument;
+ }
std::cerr << "Reached poseValue_" << std::endl;
std::vector<double> poseValue = controlValues["baseff"].getValues();
@@ -229,24 +223,22 @@ void SotLoader::oneIteration() {
readControl(controlValues_);
}
-void SotLoader::lthread_join() {
- if (thread_!=0)
- if (thread_->joinable())
- thread_->join();
+void SotLoader::lthread_join() {
+ if (thread_ != 0)
+ if (thread_->joinable()) thread_->join();
}
void SotLoader::workThreadLoader() {
double periodd;
/// Declare parameters
- if (not nh_->has_parameter("dt"))
- nh_->declare_parameter<double>("dt",0.01);
+ if (not nh_->has_parameter("dt")) nh_->declare_parameter<double>("dt", 0.01);
if (not nh_->has_parameter("paused"))
- nh_->declare_parameter<bool>("paused",false);
+ nh_->declare_parameter<bool>("paused", false);
//
- nh_->get_parameter_or("dt",periodd,0.001);
- rclcpp::Rate rate(1/periodd); // 1 kHz
+ nh_->get_parameter_or("dt", periodd, 0.001);
+ rclcpp::Rate rate(1 / periodd); // 1 kHz
DataToLog dataToLog(5000);
@@ -260,7 +252,6 @@ void SotLoader::workThreadLoader() {
rclcpp::Time timeOrigin = aClock.now();
rclcpp::Time time;
while (rclcpp::ok() && !isDynamicGraphStopped()) {
-
// Check if the user did not paused geometric_simu
nh_->get_parameter_or("paused", paused, false);
@@ -269,13 +260,12 @@ void SotLoader::workThreadLoader() {
oneIteration();
rclcpp::Duration d = aClock.now() - time;
- dataToLog.record((time - timeOrigin).nanoseconds()*1.0e9, d.nanoseconds()*1.0e9);
+ dataToLog.record((time - timeOrigin).nanoseconds() * 1.0e9,
+ d.nanoseconds() * 1.0e9);
}
rate.sleep();
}
dataToLog.save("/tmp/geometric_simu");
- std::cerr << "End of this thread: "
- << std::this_thread::get_id()
+ std::cerr << "End of this thread: " << std::this_thread::get_id()
<< std::endl;
-
}
diff --git a/src/sot_loader_basic.cpp b/src/sot_loader_basic.cpp
index 694147239cb5266539bcd233efca5a4b4cad564e..1672e81df98c1cbe461750b2232040ee5d02e2d3 100644
--- a/src/sot_loader_basic.cpp
+++ b/src/sot_loader_basic.cpp
@@ -10,12 +10,13 @@
/* -------------------------------------------------------------------------- */
#include "dynamic_graph_bridge/sot_loader_basic.hh"
-#include "dynamic_graph_bridge/ros_parameter.hpp"
#include <dynamic-graph/pool.h>
#include <exception>
+#include "dynamic_graph_bridge/ros_parameter.hpp"
+
// POSIX.1-2001
#include <dlfcn.h>
@@ -23,82 +24,80 @@ using namespace std;
using namespace dynamicgraph::sot;
namespace po = boost::program_options;
-SotLoaderBasic::SotLoaderBasic() : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
+SotLoaderBasic::SotLoaderBasic()
+ : dynamic_graph_stopped_(true), sotRobotControllerLibrary_(0) {
nh_ = dynamic_graph_bridge::get_ros_node("SotLoaderBasic");
}
-SotLoaderBasic::~SotLoaderBasic()
-{
+SotLoaderBasic::~SotLoaderBasic() {
dynamic_graph_bridge::ros_shutdown("SotLoaderBasic");
}
-void SotLoaderBasic::initialize()
-{
-}
+void SotLoaderBasic::initialize() {}
-rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() {
- return nh_;
-}
+rclcpp::Node::SharedPtr SotLoaderBasic::returnsNodeHandle() { return nh_; }
int SotLoaderBasic::initPublication() {
-
// Prepare message to be published
- joint_pub_ = nh_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
+ joint_pub_ =
+ nh_->create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
return 0;
}
void SotLoaderBasic::initializeServices() {
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
- "Ready to start dynamic graph.");
+ "Ready to start dynamic graph.");
using namespace std::placeholders;
- service_start_ = nh_->create_service<std_srvs::srv::Empty>("start_dynamic_graph",
- std::bind(&SotLoaderBasic::start_dg,
- this,std::placeholders::_1,
- std::placeholders::_2));
+ service_start_ = nh_->create_service<std_srvs::srv::Empty>(
+ "start_dynamic_graph",
+ std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
+ std::placeholders::_2));
- service_stop_ = nh_->create_service<std_srvs::srv::Empty>("stop_dynamic_graph",
- std::bind(&SotLoaderBasic::stop_dg,
- this,std::placeholders::_1,
- std::placeholders::_2));
+ service_stop_ = nh_->create_service<std_srvs::srv::Empty>(
+ "stop_dynamic_graph",
+ std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
+ std::placeholders::_2));
dynamicgraph::parameter_server_read_robot_description(nh_);
}
-void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) { dynamicLibraryName_ = afilename; }
+void SotLoaderBasic::setDynamicLibraryName(std::string& afilename) {
+ dynamicLibraryName_ = afilename;
+}
int SotLoaderBasic::readSotVectorStateParam() {
std::map<std::string, int> from_state_name_to_state_vector;
std::map<std::string, std::string> from_parallel_name_to_state_vector_name;
- if (!nh_) {
- throw std::logic_error("SotLoaderBasic::readSotVectorStateParam() nh_ not initialized");
+ if (!nh_) {
+ throw std::logic_error(
+ "SotLoaderBasic::readSotVectorStateParam() nh_ not initialized");
}
// It is necessary to declare parameters first
// before trying to access them.
if (not nh_->has_parameter("state_vector_map"))
- nh_->declare_parameter("state_vector_map",std::vector<std::string>{});
+ nh_->declare_parameter("state_vector_map", std::vector<std::string>{});
if (not nh_->has_parameter("joint_state_parallel"))
- nh_->declare_parameter("joint_state_parallel",std::vector<std::string>{});
+ nh_->declare_parameter("joint_state_parallel", std::vector<std::string>{});
// Read the state vector of the robot
// Defines the order in which the actuators are ordered
try {
std::string aParameterName("state_vector_map");
- if (!nh_->get_parameter(aParameterName,stateVectorMap_)) {
+ if (!nh_->get_parameter(aParameterName, stateVectorMap_)) {
logic_error aLogicError(
- "SotLoaderBasic::readSotVectorStateParam : State_vector_map is empty");
+ "SotLoaderBasic::readSotVectorStateParam : State_vector_map is "
+ "empty");
throw aLogicError;
}
RCLCPP_INFO(nh_->get_logger(), "state_vector_map parameter size %d",
stateVectorMap_.size());
+ } catch (exception& e) {
+ std::throw_with_nested(
+ std::logic_error("Unable to call nh_->get_parameter"));
}
- catch (exception &e)
- {
- std::throw_with_nested(std::logic_error(
- "Unable to call nh_->get_parameter"));
- }
nbOfJoints_ = static_cast<int>(stateVectorMap_.size());
nbOfParallelJoints_ = 0;
@@ -108,36 +107,35 @@ int SotLoaderBasic::readSotVectorStateParam() {
// Currently acts as a mimic or a an inverse mimic joint.
std::string prefix("joint_state_parallel");
- std::map<std::string,rclcpp::Parameter> joint_state_parallel;
- nh_->get_parameters(prefix,joint_state_parallel);
+ std::map<std::string, rclcpp::Parameter> joint_state_parallel;
+ nh_->get_parameters(prefix, joint_state_parallel);
// Iterates over the map joint_state_parallel
- for ( std::map<std::string,rclcpp::Parameter>::iterator
- it_map_expression = joint_state_parallel.begin();
- it_map_expression != joint_state_parallel.end();
- ++it_map_expression)
- {
- std::string key = it_map_expression->first;
- std::string map_expression = it_map_expression->second.as_string();
- std::string final_expression;
- double final_coefficient = 1.0;
- // deal with sign
- if (map_expression[0] == '-') {
- final_expression = map_expression.substr(1, map_expression.size() - 1);
- final_coefficient = -1.0;
- } else
- final_expression = map_expression;
-
- std::cout << key << ": " << final_coefficient << std::endl;
- from_parallel_name_to_state_vector_name[key] = final_expression;
- coefficient_parallel_joints_.push_back(final_coefficient);
- }
+ for (std::map<std::string, rclcpp::Parameter>::iterator it_map_expression =
+ joint_state_parallel.begin();
+ it_map_expression != joint_state_parallel.end(); ++it_map_expression) {
+ std::string key = it_map_expression->first;
+ std::string map_expression = it_map_expression->second.as_string();
+ std::string final_expression;
+ double final_coefficient = 1.0;
+ // deal with sign
+ if (map_expression[0] == '-') {
+ final_expression = map_expression.substr(1, map_expression.size() - 1);
+ final_coefficient = -1.0;
+ } else
+ final_expression = map_expression;
+
+ std::cout << key << ": " << final_coefficient << std::endl;
+ from_parallel_name_to_state_vector_name[key] = final_expression;
+ coefficient_parallel_joints_.push_back(final_coefficient);
+ }
nbOfParallelJoints_ = from_parallel_name_to_state_vector_name.size();
-
// Prepare joint_state according to robot description.
- joint_state_.name.resize(static_cast<unsigned long>(nbOfJoints_) + nbOfParallelJoints_);
- joint_state_.position.resize(static_cast<unsigned long>(nbOfJoints_) + nbOfParallelJoints_);
+ joint_state_.name.resize(static_cast<unsigned long>(nbOfJoints_) +
+ nbOfParallelJoints_);
+ joint_state_.position.resize(static_cast<unsigned long>(nbOfJoints_) +
+ nbOfParallelJoints_);
// Fill in the name of the joints from the state vector.
// and build local map from state name to state vector
@@ -151,10 +149,13 @@ int SotLoaderBasic::readSotVectorStateParam() {
// and build map from parallel name to state vector
std::size_t i = 0;
parallel_joints_to_state_vector_.resize(nbOfParallelJoints_);
- for (std::map<std::string, std::string>::iterator it = from_parallel_name_to_state_vector_name.begin();
+ for (std::map<std::string, std::string>::iterator it =
+ from_parallel_name_to_state_vector_name.begin();
it != from_parallel_name_to_state_vector_name.end(); it++, i++) {
- joint_state_.name[i + static_cast<std::size_t>(nbOfJoints_)] = it->first.c_str();
- parallel_joints_to_state_vector_[i] = from_state_name_to_state_vector[it->second];
+ joint_state_.name[i + static_cast<std::size_t>(nbOfJoints_)] =
+ it->first.c_str();
+ parallel_joints_to_state_vector_[i] =
+ from_state_name_to_state_vector[it->second];
}
return 0;
@@ -162,7 +163,8 @@ int SotLoaderBasic::readSotVectorStateParam() {
int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
po::options_description desc("Allowed options");
- desc.add_options()("help", "produce help message")("input-file", po::value<string>(), "library to load");
+ desc.add_options()("help", "produce help message")(
+ "input-file", po::value<string>(), "library to load");
vm_.clear();
@@ -184,7 +186,8 @@ int SotLoaderBasic::parseOptions(int argc, char* argv[]) {
void SotLoaderBasic::loadController() {
// Load the SotRobotBipedController library.
- sotRobotControllerLibrary_ = dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
+ sotRobotControllerLibrary_ =
+ dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
if (!sotRobotControllerLibrary_) {
std::cerr << "Cannot load library: " << dlerror() << '\n';
return;
@@ -194,13 +197,13 @@ void SotLoaderBasic::loadController() {
dlerror();
// Load the symbols.
- createSotExternalInterface_t* createSot = reinterpret_cast<createSotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
+ createSotExternalInterface_t* createSot =
+ reinterpret_cast<createSotExternalInterface_t*>(reinterpret_cast<long>(
+ dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
const char* dlsym_error = dlerror();
if (dlsym_error) {
- std::cerr << "Cannot load symbol create: " << dlsym_error
- << " from " << dynamicLibraryName_
- << '\n';
+ std::cerr << "Cannot load symbol create: " << dlsym_error << " from "
+ << dynamicLibraryName_ << '\n';
return;
}
@@ -217,8 +220,9 @@ void SotLoaderBasic::CleanUp() {
// SignalCaster singleton could probably be destroyed.
// Load the symbols.
- destroySotExternalInterface_t* destroySot = reinterpret_cast<destroySotExternalInterface_t*>(
- reinterpret_cast<long>(dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
+ destroySotExternalInterface_t* destroySot =
+ reinterpret_cast<destroySotExternalInterface_t*>(reinterpret_cast<long>(
+ dlsym(sotRobotControllerLibrary_, "destroySotExternalInterface")));
const char* dlsym_error = dlerror();
if (dlsym_error) {
std::cerr << "Cannot load symbol destroy: " << dlsym_error << '\n';
@@ -230,20 +234,21 @@ void SotLoaderBasic::CleanUp() {
/// Uncount the number of access to this library.
try {
- if (sotRobotControllerLibrary_ != 0)
- dlclose(sotRobotControllerLibrary_);
- }
- catch(exception &e) {
- std::throw_with_nested(std::logic_error("SotLoaderBasic::CleanUp() Unable to call dlclose"));
+ if (sotRobotControllerLibrary_ != 0) dlclose(sotRobotControllerLibrary_);
+ } catch (exception& e) {
+ std::throw_with_nested(
+ std::logic_error("SotLoaderBasic::CleanUp() Unable to call dlclose"));
}
}
-void SotLoaderBasic::start_dg(const std::shared_ptr<std_srvs::srv::Empty::Request>,
- std::shared_ptr<std_srvs::srv::Empty::Response>) {
+void SotLoaderBasic::start_dg(
+ const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::Empty::Response>) {
dynamic_graph_stopped_ = false;
}
-void SotLoaderBasic::stop_dg(const std::shared_ptr<std_srvs::srv::Empty::Request>,
- std::shared_ptr<std_srvs::srv::Empty::Response>) {
+void SotLoaderBasic::stop_dg(
+ const std::shared_ptr<std_srvs::srv::Empty::Request>,
+ std::shared_ptr<std_srvs::srv::Empty::Response>) {
dynamic_graph_stopped_ = true;
}
diff --git a/src/time_point_io.hpp b/src/time_point_io.hpp
index d9260f75d722dfc7ba8fd042247e30318e34fe6a..2da8517bc29c6c619238632f3a49bf67cecc9247 100644
--- a/src/time_point_io.hpp
+++ b/src/time_point_io.hpp
@@ -9,12 +9,11 @@
#pragma once
-#include "fwd.hpp"
-
#include <dynamic-graph/signal-caster.h>
-namespace dynamicgraph
-{
+#include "fwd.hpp"
+
+namespace dynamicgraph {
/**
* @brief Structure used to serialize/deserialize the time stamp.
@@ -23,12 +22,10 @@ namespace dynamicgraph
*/
template <>
struct signal_io<dynamic_graph_bridge::timestamp_t>
- : signal_io_base<dynamic_graph_bridge::timestamp_t>
-{
- inline static dynamic_graph_bridge::timestamp_t cast(std::istringstream &)
- {
- throw std::logic_error("this cast is not implemented.");
- }
+ : signal_io_base<dynamic_graph_bridge::timestamp_t> {
+ inline static dynamic_graph_bridge::timestamp_t cast(std::istringstream &) {
+ throw std::logic_error("this cast is not implemented.");
+ }
};
} // namespace dynamicgraph
diff --git a/tests/impl_test_sot_external_interface.cpp b/tests/impl_test_sot_external_interface.cpp
index e3ac88e70e031754e2ff5566e5f190cebd36d8dd..2b35ee9999731a1dd207a0eede001b65dd29d918 100644
--- a/tests/impl_test_sot_external_interface.cpp
+++ b/tests/impl_test_sot_external_interface.cpp
@@ -1,7 +1,6 @@
#include "impl_test_sot_external_interface.hh"
-ImplTestSotExternalInterface::ImplTestSotExternalInterface()
-{
+ImplTestSotExternalInterface::ImplTestSotExternalInterface() {
std::vector<double> ctrl_vector;
ctrl_vector.resize(2);
@@ -17,61 +16,52 @@ ImplTestSotExternalInterface::ImplTestSotExternalInterface()
named_base_ff_vec_.setName("baseff");
ctrl_vector.resize(7);
- for( std::vector<double>::size_type i=0;i<6;i++)
- ctrl_vector[i] = 0.0;
+ for (std::vector<double>::size_type i = 0; i < 6; i++) ctrl_vector[i] = 0.0;
ctrl_vector[6] = 0.0;
named_base_ff_vec_.setValues(ctrl_vector);
}
-ImplTestSotExternalInterface::~ImplTestSotExternalInterface()
-{}
+ImplTestSotExternalInterface::~ImplTestSotExternalInterface() {}
-void ImplTestSotExternalInterface::setupSetSensors
-(std::map<std::string,
- dynamicgraph::sot::SensorValues> &)
-{
+void ImplTestSotExternalInterface::setupSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) {
std::cout << "ImplTestSotExternalInterface::setupSetSensors" << std::endl;
}
-void ImplTestSotExternalInterface::nominalSetSensors
-(std::map<std::string,
- dynamicgraph::sot::SensorValues> &)
-{
+void ImplTestSotExternalInterface::nominalSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) {
std::cout << "ImplTestSotExternalInterface::nominalSetSensors" << std::endl;
}
-void ImplTestSotExternalInterface::cleanupSetSensors
-(std::map<std::string,
- dynamicgraph::sot::SensorValues> &)
-{
+void ImplTestSotExternalInterface::cleanupSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) {
std::cout << "ImplTestSotExternalInterface::cleanupSetSensors" << std::endl;
}
-void ImplTestSotExternalInterface::getControl
-(std::map<std::string,
- dynamicgraph::sot::ControlValues> & controlValues)
-{
+void ImplTestSotExternalInterface::getControl(
+ std::map<std::string, dynamicgraph::sot::ControlValues> &controlValues) {
// Put the default named_ctrl_vec inside the map controlValues.
controlValues["control"] = named_ctrl_vec_;
controlValues["baseff"] = named_base_ff_vec_;
}
-void ImplTestSotExternalInterface::setSecondOrderIntegration
-(void)
-{
- std::cout << "ImplTestSotExternalInterface::setSecondOrderIntegration" << std::endl;
+void ImplTestSotExternalInterface::setSecondOrderIntegration(void) {
+ std::cout << "ImplTestSotExternalInterface::setSecondOrderIntegration"
+ << std::endl;
}
-void ImplTestSotExternalInterface::setNoIntegration
-(void)
-{
+void ImplTestSotExternalInterface::setNoIntegration(void) {
std::cout << "setNoIntegration" << std::endl;
}
extern "C" {
- dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface()
- { return new ImplTestSotExternalInterface; }
+dynamicgraph::sot::AbstractSotExternalInterface *createSotExternalInterface() {
+ return new ImplTestSotExternalInterface;
+}
}
extern "C" {
- void destroySotExternalInterface(dynamicgraph::sot::AbstractSotExternalInterface *p) { delete p; }
+void destroySotExternalInterface(
+ dynamicgraph::sot::AbstractSotExternalInterface *p) {
+ delete p;
+}
}
diff --git a/tests/impl_test_sot_external_interface.hh b/tests/impl_test_sot_external_interface.hh
index 9b6a86b14d84082fc033652a1f31bb269350a75b..119a844de495c63c5fd6848f74c310406689f80d 100644
--- a/tests/impl_test_sot_external_interface.hh
+++ b/tests/impl_test_sot_external_interface.hh
@@ -4,36 +4,33 @@
#include <iostream>
#include <sot/core/abstract-sot-external-interface.hh>
-
-class ImplTestSotExternalInterface : public
-dynamicgraph::sot::AbstractSotExternalInterface {
-public:
+class ImplTestSotExternalInterface
+ : public dynamicgraph::sot::AbstractSotExternalInterface {
+ public:
ImplTestSotExternalInterface();
virtual ~ImplTestSotExternalInterface();
- virtual void setupSetSensors(std::map<std::string,
- dynamicgraph::sot::SensorValues> &) final;
+ virtual void setupSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) final;
- virtual void nominalSetSensors(std::map<std::string,
- dynamicgraph::sot::SensorValues> &) final;
+ virtual void nominalSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) final;
- virtual void cleanupSetSensors(std::map<std::string,
- dynamicgraph::sot::SensorValues> &) final;
- virtual void getControl(std::map<std::string,
- dynamicgraph::sot::ControlValues> &) final;
+ virtual void cleanupSetSensors(
+ std::map<std::string, dynamicgraph::sot::SensorValues> &) final;
+ virtual void getControl(
+ std::map<std::string, dynamicgraph::sot::ControlValues> &) final;
virtual void setSecondOrderIntegration(void);
virtual void setNoIntegration(void);
-protected:
+ protected:
// Named ctrl vector
dynamicgraph::sot::ControlValues named_ctrl_vec_;
// Named base free flyer vector
dynamicgraph::sot::ControlValues named_base_ff_vec_;
-
};
-
#endif
diff --git a/tests/main.cpp b/tests/main.cpp
index f4adc6a00400bb14ef2c1134513db2848368eb3c..2cf3131387b7744223abd0b60d6531d2fff94250 100644
--- a/tests/main.cpp
+++ b/tests/main.cpp
@@ -15,10 +15,9 @@
#include <rclcpp/rclcpp.hpp>
-int main(int argc, char **argv)
-{
- rclcpp::init(argc, argv);
- ::testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
- rclcpp::shutdown();
+int main(int argc, char **argv) {
+ rclcpp::init(argc, argv);
+ ::testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+ rclcpp::shutdown();
}
diff --git a/tests/test_common.hpp b/tests/test_common.hpp
index 0b13166082c92373d77e61427ccc3f9b6bfd2b1b..5d2243977d089512b1928f1bc3564d88189d4124 100644
--- a/tests/test_common.hpp
+++ b/tests/test_common.hpp
@@ -27,141 +27,135 @@ using namespace dynamic_graph_bridge;
*/
const std::string ROS_NODE_NAME = "test_dgm";
-void start_dg_ros_service()
-{
- // Service name.
- std::string service_name = "/dynamic_graph_bridge/start_dynamic_graph";
- // Create a client from a temporary node.
- RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
- auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
- ASSERT_TRUE(client->wait_for_service(1s));
-
- // Fill the command message.
- std_srvs::srv::Empty::Request::SharedPtr request =
- std::make_shared<std_srvs::srv::Empty::Request>();
- std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
- // Call the service.
- response = client->async_send_request(request);
- // Wait for the answer.
- ASSERT_TRUE(response.valid());
- // wait_for_response(response);
- ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
- rclcpp::FutureReturnCode::SUCCESS);
+void start_dg_ros_service() {
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/start_dynamic_graph";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ std_srvs::srv::Empty::Request::SharedPtr request =
+ std::make_shared<std_srvs::srv::Empty::Request>();
+ std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::FutureReturnCode::SUCCESS);
}
-void stop_dg_ros_service()
-{
- // Service name.
- std::string service_name = "/dynamic_graph_bridge/stop_dynamic_graph";
- // Create a client from a temporary node.
- RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
- auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
- ASSERT_TRUE(client->wait_for_service(1s));
-
- // Fill the command message.
- std_srvs::srv::Empty::Request::SharedPtr request =
- std::make_shared<std_srvs::srv::Empty::Request>();
- std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
- // Call the service.
- response = client->async_send_request(request);
- // Wait for the answer.
- ASSERT_TRUE(response.valid());
- // wait_for_response(response);
- ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
- rclcpp::FutureReturnCode::SUCCESS);
+void stop_dg_ros_service() {
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/stop_dynamic_graph";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client = ros_node->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ std_srvs::srv::Empty::Request::SharedPtr request =
+ std::make_shared<std_srvs::srv::Empty::Request>();
+ std::shared_future<std_srvs::srv::Empty::Response::SharedPtr> response;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::FutureReturnCode::SUCCESS);
}
void start_run_python_command_ros_service(const std::string& cmd,
std::string& result,
std::string& standarderror,
- std::string& standardoutput)
-{
- // Service name.
- std::string service_name = "/dynamic_graph_bridge/run_python_command";
- // Create a client from a temporary node.
- RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
- auto client =
- ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(service_name);
- ASSERT_TRUE(client->wait_for_service(1s));
-
- // Fill the command message.
- dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request::SharedPtr request =
- std::make_shared<dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request>();
- std::shared_future<dynamic_graph_bridge_msgs::srv::RunPythonCommand::Response::SharedPtr>
- response;
- request->input = cmd;
- // Call the service.
- response = client->async_send_request(request);
- // Wait for the answer.
- ASSERT_TRUE(response.valid());
- // wait_for_response(response);
- ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
- rclcpp::FutureReturnCode::SUCCESS);
- // Get the results.
- result = response.get()->result;
- standarderror = response.get()->standarderror;
- standardoutput = response.get()->standardoutput;
+ std::string& standardoutput) {
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/run_python_command";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client =
+ ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(
+ service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request::SharedPtr request =
+ std::make_shared<
+ dynamic_graph_bridge_msgs::srv::RunPythonCommand::Request>();
+ std::shared_future<
+ dynamic_graph_bridge_msgs::srv::RunPythonCommand::Response::SharedPtr>
+ response;
+ request->input = cmd;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::FutureReturnCode::SUCCESS);
+ // Get the results.
+ result = response.get()->result;
+ standarderror = response.get()->standarderror;
+ standardoutput = response.get()->standardoutput;
}
void start_run_python_script_ros_service(const std::string& file_name,
- std::string& result)
-{
- // Service name.
- std::string service_name = "/dynamic_graph_bridge/run_python_file";
- // Create a client from a temporary node.
- RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
- auto client =
- ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(service_name);
- ASSERT_TRUE(client->wait_for_service(1s));
-
- // Fill the command message.
- dynamic_graph_bridge_msgs::srv::RunPythonFile::Request::SharedPtr request =
- std::make_shared<dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
- std::shared_future<dynamic_graph_bridge_msgs::srv::RunPythonFile::Response::SharedPtr>
- response;
- request->input = file_name;
- // Call the service.
- response = client->async_send_request(request);
- // Wait for the answer.
- ASSERT_TRUE(response.valid());
- // wait_for_response(response);
- ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
- rclcpp::FutureReturnCode::SUCCESS);
- // Get the results.
- result = response.get()->result;
+ std::string& result) {
+ // Service name.
+ std::string service_name = "/dynamic_graph_bridge/run_python_file";
+ // Create a client from a temporary node.
+ RosNodePtr ros_node = get_ros_node(ROS_NODE_NAME);
+ auto client =
+ ros_node->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+ service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
+
+ // Fill the command message.
+ dynamic_graph_bridge_msgs::srv::RunPythonFile::Request::SharedPtr request =
+ std::make_shared<
+ dynamic_graph_bridge_msgs::srv::RunPythonFile::Request>();
+ std::shared_future<
+ dynamic_graph_bridge_msgs::srv::RunPythonFile::Response::SharedPtr>
+ response;
+ request->input = file_name;
+ // Call the service.
+ response = client->async_send_request(request);
+ // Wait for the answer.
+ ASSERT_TRUE(response.valid());
+ // wait_for_response(response);
+ ASSERT_TRUE(rclcpp::spin_until_future_complete(ros_node, response) ==
+ rclcpp::FutureReturnCode::SUCCESS);
+ // Get the results.
+ result = response.get()->result;
}
void display_services(
- std::map<std::string, std::vector<std::string> >& list_service_and_types)
-{
- for (std::map<std::string, std::vector<std::string> >::const_iterator
- const_it = list_service_and_types.begin();
- const_it != list_service_and_types.end();
- ++const_it)
- {
- std::cerr << const_it->first << "\t\t\t";
- for (unsigned i = 0; i < const_it->second.size(); ++i)
- {
- std::cerr << std::right;
- std::cerr << const_it->second[i] << std::endl;
- }
+ std::map<std::string, std::vector<std::string> >& list_service_and_types) {
+ for (std::map<std::string, std::vector<std::string> >::const_iterator
+ const_it = list_service_and_types.begin();
+ const_it != list_service_and_types.end(); ++const_it) {
+ std::cerr << const_it->first << "\t\t\t";
+ for (unsigned i = 0; i < const_it->second.size(); ++i) {
+ std::cerr << std::right;
+ std::cerr << const_it->second[i] << std::endl;
}
+ }
}
-void display_services()
-{
- std::map<std::string, std::vector<std::string> > list_service_and_types =
- get_ros_node("display_services")->get_service_names_and_types();
- for (std::map<std::string, std::vector<std::string> >::const_iterator
- const_it = list_service_and_types.begin();
- const_it != list_service_and_types.end();
- ++const_it)
- {
- std::cerr << const_it->first << "\t\t\t";
- for (unsigned i = 0; i < const_it->second.size(); ++i)
- {
- std::cerr << std::right;
- std::cerr << const_it->second[i] << std::endl;
- }
+void display_services() {
+ std::map<std::string, std::vector<std::string> > list_service_and_types =
+ get_ros_node("display_services")->get_service_names_and_types();
+ for (std::map<std::string, std::vector<std::string> >::const_iterator
+ const_it = list_service_and_types.begin();
+ const_it != list_service_and_types.end(); ++const_it) {
+ std::cerr << const_it->first << "\t\t\t";
+ for (unsigned i = 0; i < const_it->second.size(); ++i) {
+ std::cerr << std::right;
+ std::cerr << const_it->second[i] << std::endl;
}
+ }
}
diff --git a/tests/test_ros_init.cpp b/tests/test_ros_init.cpp
index b671466e8062facdc5d7c2561f893fad2ebb9ffc..1bb8b0c491ed18fa8ae64e1f3bbc756bd1a17125 100644
--- a/tests/test_ros_init.cpp
+++ b/tests/test_ros_init.cpp
@@ -19,31 +19,22 @@ using namespace dynamic_graph_bridge;
/**
* @brief This is the test environment
*/
-class TestRosInit : public ::testing::Test
-{
- /**
- * @brief SetUp, is executed before the unit tests
- */
- void SetUp()
- {
- ros_spin_non_blocking();
- }
-
- /**
- * @brief TearDown, is executed after the unit tests
- */
- void TearDown()
- {
- ros_clean();
- }
+class TestRosInit : public ::testing::Test {
+ /**
+ * @brief SetUp, is executed before the unit tests
+ */
+ void SetUp() { ros_spin_non_blocking(); }
+
+ /**
+ * @brief TearDown, is executed after the unit tests
+ */
+ void TearDown() { ros_clean(); }
};
/**
* @brief This is the test environment
*/
-class TestRosInitGlobalVar : public ::testing::Test
-{
-};
+class TestRosInitGlobalVar : public ::testing::Test {};
/*****************
* Start Testing *
@@ -52,54 +43,50 @@ class TestRosInitGlobalVar : public ::testing::Test
/**
* Here we check that we can create a ros node
*/
-TEST_F(TestRosInitGlobalVar, test_create_node)
-{
- /* Setup */
- std::string node_name = "my_global_node";
- get_ros_node(node_name);
-
- /* Test */
- ASSERT_TRUE(ros_node_exists(node_name));
+TEST_F(TestRosInitGlobalVar, test_create_node) {
+ /* Setup */
+ std::string node_name = "my_global_node";
+ get_ros_node(node_name);
+
+ /* Test */
+ ASSERT_TRUE(ros_node_exists(node_name));
}
/**
* Here we check that we still have the ros node created (global variable)
*/
-TEST_F(TestRosInitGlobalVar, test_global_variable_deleted_after_tear_down)
-{
- /* Setup */
- std::string node_name = "my_global_node";
+TEST_F(TestRosInitGlobalVar, test_global_variable_deleted_after_tear_down) {
+ /* Setup */
+ std::string node_name = "my_global_node";
- /* Test */
- ASSERT_TRUE(ros_node_exists(node_name));
+ /* Test */
+ ASSERT_TRUE(ros_node_exists(node_name));
}
/**
* Here we check that the destruction of the variable
*/
-TEST_F(TestRosInitGlobalVar, test_delete_global_variable)
-{
- /* Setup */
- std::string node_name = "my_global_node";
- ros_shutdown(node_name);
+TEST_F(TestRosInitGlobalVar, test_delete_global_variable) {
+ /* Setup */
+ std::string node_name = "my_global_node";
+ ros_shutdown(node_name);
- /* Test */
- ASSERT_FALSE(ros_node_exists(node_name));
+ /* Test */
+ ASSERT_FALSE(ros_node_exists(node_name));
}
/**
* Here we check if we can destruct the object several time
*/
-TEST_F(TestRosInit, test_multiple_shutdown_call)
-{
- /* Setup */
- std::string node_name = "my_global_node";
-
- /* Test */
- ASSERT_NO_THROW(for (unsigned i = 0; i < 20; ++i) {
- ros_shutdown(node_name);
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
- });
+TEST_F(TestRosInit, test_multiple_shutdown_call) {
+ /* Setup */
+ std::string node_name = "my_global_node";
+
+ /* Test */
+ ASSERT_NO_THROW(for (unsigned i = 0; i < 20; ++i) {
+ ros_shutdown(node_name);
+ std::this_thread::sleep_for(std::chrono::milliseconds(1));
+ });
}
/**
@@ -108,84 +95,77 @@ TEST_F(TestRosInit, test_multiple_shutdown_call)
* @return true
*/
bool simple_service(std_srvs::srv::Empty::Request::SharedPtr,
- std_srvs::srv::Empty::Response::SharedPtr)
-{
- return true;
+ std_srvs::srv::Empty::Response::SharedPtr) {
+ return true;
}
/**
* Check if services are available
*/
-TEST_F(TestRosInit, test_services_available)
-{
- /* Setup */
- // create a node and a service
- RosNodePtr n0 = get_ros_node("node_0");
- ros_add_node_to_executor("node_0");
- std::string service_name = "/simple_service";
- rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
- n0->create_service<std_srvs::srv::Empty>(service_name, &simple_service);
-
- /* Test */
- rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
- n0->create_client<std_srvs::srv::Empty>(service_name);
- ASSERT_TRUE(client->wait_for_service(1s));
+TEST_F(TestRosInit, test_services_available) {
+ /* Setup */
+ // create a node and a service
+ RosNodePtr n0 = get_ros_node("node_0");
+ ros_add_node_to_executor("node_0");
+ std::string service_name = "/simple_service";
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
+ n0->create_service<std_srvs::srv::Empty>(service_name, &simple_service);
+
+ /* Test */
+ rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+ n0->create_client<std_srvs::srv::Empty>(service_name);
+ ASSERT_TRUE(client->wait_for_service(1s));
}
/**
* Check if services are not longer available after shutdown
*/
-TEST_F(TestRosInit, test_services_shut_down)
-{
- /* Setup */
- std::string service_name = "/simple_service";
+TEST_F(TestRosInit, test_services_shut_down) {
+ /* Setup */
+ std::string service_name = "/simple_service";
- // create a node and a service and shutdown the node
- {
- RosNodePtr n0 = get_ros_node("node_0");
- ros_add_node_to_executor("node_0");
- rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
- n0->create_service<std_srvs::srv::Empty>(service_name,
- &simple_service);
- ros_shutdown("node_0");
- }
+ // create a node and a service and shutdown the node
+ {
+ RosNodePtr n0 = get_ros_node("node_0");
+ ros_add_node_to_executor("node_0");
+ rclcpp::Service<std_srvs::srv::Empty>::SharedPtr server =
+ n0->create_service<std_srvs::srv::Empty>(service_name, &simple_service);
+ ros_shutdown("node_0");
+ }
- // create a client to this service
- RosNodePtr n1 = get_ros_node("node_1");
- rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
- n1->create_client<std_srvs::srv::Empty>(service_name);
+ // create a client to this service
+ RosNodePtr n1 = get_ros_node("node_1");
+ rclcpp::Client<std_srvs::srv::Empty>::SharedPtr client =
+ n1->create_client<std_srvs::srv::Empty>(service_name);
- /* Test */
- ASSERT_FALSE(client->wait_for_service(100ms));
+ /* Test */
+ ASSERT_FALSE(client->wait_for_service(100ms));
}
/**
* Check if shutdown without name kills all created nodes.
*/
-TEST_F(TestRosInit, test_killall_nodes)
-{
- /* Setup */
- // creates tones of nodes
- unsigned int nb_nodes = 20;
- for (unsigned int i = 0; i < nb_nodes; ++i)
+TEST_F(TestRosInit, test_killall_nodes) {
+ /* Setup */
+ // creates tones of nodes
+ unsigned int nb_nodes = 20;
+ for (unsigned int i = 0; i < nb_nodes; ++i) {
{
- {
- std::ostringstream os;
- os << "node_" << i;
- get_ros_node(os.str());
- }
+ std::ostringstream os;
+ os << "node_" << i;
+ get_ros_node(os.str());
}
+ }
- // kill them all
- ros_clean();
+ // kill them all
+ ros_clean();
- /* Test if they are all killed */
- for (unsigned int i = 0; i < nb_nodes; ++i)
+ /* Test if they are all killed */
+ for (unsigned int i = 0; i < nb_nodes; ++i) {
{
- {
- std::ostringstream os;
- os << "node_" << i;
- ASSERT_FALSE(ros_node_exists(os.str()));
- }
+ std::ostringstream os;
+ os << "node_" << i;
+ ASSERT_FALSE(ros_node_exists(os.str()));
}
+ }
}
diff --git a/tests/test_ros_interpreter.cpp b/tests/test_ros_interpreter.cpp
index ebc73dbf7fd0712898b2c1909bf0dd1988369fa2..11b06e941a678fb05642533f67f743b363c4274f 100644
--- a/tests/test_ros_interpreter.cpp
+++ b/tests/test_ros_interpreter.cpp
@@ -9,8 +9,8 @@
#include <iomanip>
-#include "test_common.hpp"
#include "dynamic_graph_bridge/ros_python_interpreter_server.hpp"
+#include "test_common.hpp"
/***************************
* SETUP THE TESTING CLASS *
@@ -22,251 +22,236 @@ using namespace dynamic_graph_bridge;
/**
* @brief This is the test environment
*/
-class TestRosInterpreter : public ::testing::Test
-{
-protected:
- /**
- * @brief SetUp, is executed before the unit tests
- */
- void SetUp()
- {
- // Create the ros node
- node_name_ = "unittest_ros_python_interpreter";
- run_python_command_service_name_ =
- "/dynamic_graph_bridge/run_python_command";
- run_python_file_service_name_ =
- "/dynamic_graph_bridge/run_python_file";
- ros_spin_non_blocking();
- }
-
- /**
- * @brief TearDown, is executed after the unit tests
- */
- void TearDown()
- {
- // delete the ros node
- ros_clean();
- }
- /**
- * @brief Node name
- */
- std::string node_name_;
-
- /**
- * @brief Name of the ros service.
- */
- std::string run_python_command_service_name_;
-
- /**
- * @brief Name of the ros service.
- */
- std::string run_python_file_service_name_;
+class TestRosInterpreter : public ::testing::Test {
+ protected:
+ /**
+ * @brief SetUp, is executed before the unit tests
+ */
+ void SetUp() {
+ // Create the ros node
+ node_name_ = "unittest_ros_python_interpreter";
+ run_python_command_service_name_ =
+ "/dynamic_graph_bridge/run_python_command";
+ run_python_file_service_name_ = "/dynamic_graph_bridge/run_python_file";
+ ros_spin_non_blocking();
+ }
+
+ /**
+ * @brief TearDown, is executed after the unit tests
+ */
+ void TearDown() {
+ // delete the ros node
+ ros_clean();
+ }
+ /**
+ * @brief Node name
+ */
+ std::string node_name_;
+
+ /**
+ * @brief Name of the ros service.
+ */
+ std::string run_python_command_service_name_;
+
+ /**
+ * @brief Name of the ros service.
+ */
+ std::string run_python_file_service_name_;
};
/*****************
* Start Testing *
*****************/
-TEST_F(TestRosInterpreter, test_constructor_no_throw)
-{
- /* setup */
+TEST_F(TestRosInterpreter, test_constructor_no_throw) {
+ /* setup */
- /* test */
- ASSERT_NO_THROW(RosPythonInterpreterServer rpi;);
+ /* test */
+ ASSERT_NO_THROW(RosPythonInterpreterServer rpi;);
}
-TEST_F(TestRosInterpreter, test_destructor_no_throw)
-{
- ASSERT_NO_THROW({ RosPythonInterpreterServer rpi; });
+TEST_F(TestRosInterpreter, test_destructor_no_throw) {
+ ASSERT_NO_THROW({ RosPythonInterpreterServer rpi; });
}
-TEST_F(TestRosInterpreter, test_run_cmd_not_available_upon_construction)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_run_cmd_not_available_upon_construction) {
+ /* Setup. */
- // Init the class to test.
- RosPythonInterpreterServer rpi;
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
- // Fetch the information using ROS.
- std::map<std::string, std::vector<std::string>> list_service_and_types;
- RosNodePtr ros_node = get_ros_node(node_name_);
- bool simple_service_exists = false;
+ // Fetch the information using ROS.
+ std::map<std::string, std::vector<std::string>> list_service_and_types;
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ bool simple_service_exists = false;
- // Check if the service exists.
- list_service_and_types = ros_node->get_service_names_and_types();
- display_services(list_service_and_types);
- simple_service_exists =
- list_service_and_types.find(run_python_command_service_name_) !=
- list_service_and_types.end();
+ // Check if the service exists.
+ list_service_and_types = ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
+ simple_service_exists =
+ list_service_and_types.find(run_python_command_service_name_) !=
+ list_service_and_types.end();
- /* Test. */
- ASSERT_FALSE(simple_service_exists);
+ /* Test. */
+ ASSERT_FALSE(simple_service_exists);
}
-TEST_F(TestRosInterpreter, test_run_file_not_available_upon_construction)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_run_file_not_available_upon_construction) {
+ /* Setup. */
- // Init the class to test.
- RosPythonInterpreterServer rpi;
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
- // Fetch the information using ROS.
- std::map<std::string, std::vector<std::string>> list_service_and_types;
- RosNodePtr ros_node = get_ros_node(node_name_);
- bool simple_service_exists = false;
+ // Fetch the information using ROS.
+ std::map<std::string, std::vector<std::string>> list_service_and_types;
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ bool simple_service_exists = false;
- // Check if the service exists.
- list_service_and_types = ros_node->get_service_names_and_types();
- display_services(list_service_and_types);
- simple_service_exists =
- list_service_and_types.find(run_python_file_service_name_) !=
- list_service_and_types.end();
+ // Check if the service exists.
+ list_service_and_types = ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
+ simple_service_exists =
+ list_service_and_types.find(run_python_file_service_name_) !=
+ list_service_and_types.end();
- /* Test. */
- ASSERT_FALSE(simple_service_exists);
+ /* Test. */
+ ASSERT_FALSE(simple_service_exists);
}
-TEST_F(TestRosInterpreter, test_run_cmd_available_after_init)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_run_cmd_available_after_init) {
+ /* Setup. */
- // Init the class to test.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
- // print the service available
- RosNodePtr ros_node = get_ros_node(node_name_);
- std::map<std::string, std::vector<std::string>> list_service_and_types =
- ros_node->get_service_names_and_types();
- display_services(list_service_and_types);
+ // print the service available
+ RosNodePtr ros_node = get_ros_node(node_name_);
+ std::map<std::string, std::vector<std::string>> list_service_and_types =
+ ros_node->get_service_names_and_types();
+ display_services(list_service_and_types);
- // Fetch the information using ROS.
- auto client = get_ros_node(node_name_)
- ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(
- "/dynamic_graph_bridge/run_python_command");
+ // Fetch the information using ROS.
+ auto client =
+ get_ros_node(node_name_)
+ ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonCommand>(
+ "/dynamic_graph_bridge/run_python_command");
- /* Test. */
- ASSERT_TRUE(client->wait_for_service(1s));
+ /* Test. */
+ ASSERT_TRUE(client->wait_for_service(1s));
}
-TEST_F(TestRosInterpreter, test_run_file_available_after_init)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_run_file_available_after_init) {
+ /* Setup. */
- // Init the class to test.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
+ // Init the class to test.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
- // Fetch the information using ROS.
- auto client = get_ros_node(node_name_)
- ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
- "/dynamic_graph_bridge/run_python_file");
+ // Fetch the information using ROS.
+ auto client =
+ get_ros_node(node_name_)
+ ->create_client<dynamic_graph_bridge_msgs::srv::RunPythonFile>(
+ "/dynamic_graph_bridge/run_python_file");
- /* Test. */
- ASSERT_TRUE(client->wait_for_service(1s));
+ /* Test. */
+ ASSERT_TRUE(client->wait_for_service(1s));
}
-TEST_F(TestRosInterpreter, test_call_run_command_result)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_call_run_command_result) {
+ /* Setup. */
- // Create the ros python interpreter.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
- // Create and call the clients.
- std::string cmd = "1 + 1";
- std::string result = "";
- std::string standarderror = "";
- std::string standardoutput = "";
- start_run_python_command_ros_service(
- cmd, result, standarderror, standardoutput);
+ // Create and call the clients.
+ std::string cmd = "1 + 1";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(cmd, result, standarderror,
+ standardoutput);
- /* Tests. */
- ASSERT_EQ(result, "2");
+ /* Tests. */
+ ASSERT_EQ(result, "2");
}
-TEST_F(TestRosInterpreter, test_call_run_command_standardoutput)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_call_run_command_standardoutput) {
+ /* Setup. */
- // Create the ros python interpreter.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
- // Create and call the clients.
- std::string cmd = "print(\"Banana\")";
- std::string result = "";
- std::string standarderror = "";
- std::string standardoutput = "";
- start_run_python_command_ros_service(
- cmd, result, standarderror, standardoutput);
+ // Create and call the clients.
+ std::string cmd = "print(\"Banana\")";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(cmd, result, standarderror,
+ standardoutput);
- /* Tests. */
- ASSERT_EQ(standardoutput, "Banana\nprint(\"Banana\")\nOutput:Banana\n\n");
+ /* Tests. */
+ ASSERT_EQ(standardoutput, "Banana\nprint(\"Banana\")\nOutput:Banana\n\n");
}
-TEST_F(TestRosInterpreter, test_call_run_command_standarderror)
-{
- /* Setup. */
-
- // Create the ros python interpreter.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
-
- // Create and call the clients.
- std::string cmd = "a";
- std::string result = "";
- std::string standarderror = "";
- std::string standardoutput = "";
- start_run_python_command_ros_service(
- cmd, result, standarderror, standardoutput);
-
- /* Tests. */
- ASSERT_EQ(
- standarderror,
- "Traceback (most recent call last):\n File \"<string>\", line 1, "
- "in <module>\nNameError: name 'a' is not defined\n");
+TEST_F(TestRosInterpreter, test_call_run_command_standarderror) {
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string cmd = "a";
+ std::string result = "";
+ std::string standarderror = "";
+ std::string standardoutput = "";
+ start_run_python_command_ros_service(cmd, result, standarderror,
+ standardoutput);
+
+ /* Tests. */
+ ASSERT_EQ(standarderror,
+ "Traceback (most recent call last):\n File \"<string>\", line 1, "
+ "in <module>\nNameError: name 'a' is not defined\n");
}
-TEST_F(TestRosInterpreter, test_call_run_script_result)
-{
- /* Setup. */
+TEST_F(TestRosInterpreter, test_call_run_script_result) {
+ /* Setup. */
- // Create the ros python interpreter.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
- // Create and call the clients.
- std::string file_name = TEST_CONFIG_PATH + std::string("simple_add.py");
- std::string result = "";
- start_run_python_script_ros_service(file_name, result);
+ // Create and call the clients.
+ std::string file_name = TEST_CONFIG_PATH + std::string("simple_add.py");
+ std::string result = "";
+ start_run_python_script_ros_service(file_name, result);
- /* Tests. */
- ASSERT_EQ(result, "");
+ /* Tests. */
+ ASSERT_EQ(result, "");
}
-TEST_F(TestRosInterpreter, test_call_run_script_standarderror)
-{
- /* Setup. */
-
- // Create the ros python interpreter.
- RosPythonInterpreterServer rpi;
- rpi.start_ros_service();
-
- // Create and call the clients.
- std::string file_name =
- TEST_CONFIG_PATH + std::string("simple_add_fail");
- std::string result = "";
- start_run_python_script_ros_service(file_name, result);
-
- // Prepare the test.
- std::string error_first_part = "a = 1 + 1 + b";
- std::string error_second_part = "NameError: name \'b\' is not defined";
- std::size_t found_first_part = result.find(error_first_part);
- std::size_t found_second_part = result.find(error_second_part);
-
- /* test */
- ASSERT_TRUE(found_first_part != std::string::npos);
- ASSERT_TRUE(found_second_part != std::string::npos);
+TEST_F(TestRosInterpreter, test_call_run_script_standarderror) {
+ /* Setup. */
+
+ // Create the ros python interpreter.
+ RosPythonInterpreterServer rpi;
+ rpi.start_ros_service();
+
+ // Create and call the clients.
+ std::string file_name = TEST_CONFIG_PATH + std::string("simple_add_fail");
+ std::string result = "";
+ start_run_python_script_ros_service(file_name, result);
+
+ // Prepare the test.
+ std::string error_first_part = "a = 1 + 1 + b";
+ std::string error_second_part = "NameError: name \'b\' is not defined";
+ std::size_t found_first_part = result.find(error_first_part);
+ std::size_t found_second_part = result.find(error_second_part);
+
+ /* test */
+ ASSERT_TRUE(found_first_part != std::string::npos);
+ ASSERT_TRUE(found_second_part != std::string::npos);
}
diff --git a/tests/test_sot_loader.cpp b/tests/test_sot_loader.cpp
index e4b189764a21ef38603f0d752c914cb6ff069bac..1dfe8fdf658aa3539014027567d10f81ce48e7f4 100644
--- a/tests/test_sot_loader.cpp
+++ b/tests/test_sot_loader.cpp
@@ -1,15 +1,14 @@
#include <gmock/gmock.h>
+
#include "dynamic_graph_bridge/ros.hpp"
#include "dynamic_graph_bridge/sot_loader.hh"
-class MockSotLoaderTest: public ::testing::Test {
-
-public:
-
+class MockSotLoaderTest : public ::testing::Test {
+ public:
class MockSotLoader : public SotLoader {
- public:
- ~MockSotLoader(){}
+ public:
+ ~MockSotLoader() {}
void generateEvents() {
usleep(20000);
@@ -23,13 +22,13 @@ public:
void testLoadController() {
// Set input call
- int argc=2;
+ int argc = 2;
char *argv[2];
- char argv1[30]="mocktest";
- argv[0]=argv1;
- char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
- argv[1]=argv2;
- parseOptions(argc,argv);
+ char argv1[30] = "mocktest";
+ argv[0] = argv1;
+ char argv2[60] = "--input-file=libimpl_test_sot_external_interface.so";
+ argv[1] = argv2;
+ parseOptions(argc, argv);
std::string finalname("libimpl_test_sot_external_interface.so");
EXPECT_TRUE(finalname == dynamicLibraryName_);
@@ -42,8 +41,8 @@ public:
initializeServices();
// Constructor should default freeFlyerPose
- EXPECT_TRUE( freeFlyerPose_.header.frame_id == std::string("odom"));
- EXPECT_TRUE( freeFlyerPose_.child_frame_id == std::string("base_link"));
+ EXPECT_TRUE(freeFlyerPose_.header.frame_id == std::string("odom"));
+ EXPECT_TRUE(freeFlyerPose_.child_frame_id == std::string("base_link"));
// Initialie publication of sot joint states
// and base eff
@@ -53,16 +52,16 @@ public:
startControlLoop();
// Start the thread generating events.
- std::thread local_events(&MockSotLoader::generateEvents,this);
+ std::thread local_events(&MockSotLoader::generateEvents, this);
// Wait for each threads.
- SotLoader::lthread_join(); // Wait 100 ms
+ SotLoader::lthread_join(); // Wait 100 ms
local_events.join();
}
};
-public:
- MockSotLoader* mockSotLoader_ptr_;
+ public:
+ MockSotLoader *mockSotLoader_ptr_;
void SetUp() {
mockSotLoader_ptr_ = new MockSotLoader();
@@ -73,20 +72,17 @@ public:
delete mockSotLoader_ptr_;
mockSotLoader_ptr_ = nullptr;
}
-
};
-TEST_F(MockSotLoaderTest,TestLoadController)
-{
+TEST_F(MockSotLoaderTest, TestLoadController) {
mockSotLoader_ptr_->testLoadController();
}
-
int main(int argc, char **argv) {
::testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
- int r=RUN_ALL_TESTS();
+ int r = RUN_ALL_TESTS();
rclcpp::shutdown();
return r;
diff --git a/tests/test_sot_loader_basic.cpp b/tests/test_sot_loader_basic.cpp
index 20b0b1bf3d6013814a5ff20ba1759c253a34a900..99b6c0825c305d4e24255a6832302ecdc8052bf8 100644
--- a/tests/test_sot_loader_basic.cpp
+++ b/tests/test_sot_loader_basic.cpp
@@ -1,19 +1,18 @@
#include <gmock/gmock.h>
+
#include "dynamic_graph_bridge/ros.hpp"
#include "dynamic_graph_bridge/sot_loader_basic.hh"
-class MockSotLoaderBasicTest: public ::testing::Test {
-
-public:
-
- class MockSotLoaderBasic: public SotLoaderBasic {
- public:
- virtual ~MockSotLoaderBasic(){}
+class MockSotLoaderBasicTest : public ::testing::Test {
+ public:
+ class MockSotLoaderBasic : public SotLoaderBasic {
+ public:
+ virtual ~MockSotLoaderBasic() {}
void checkStateVectorMap() {
readSotVectorStateParam();
- ASSERT_EQ(static_cast<int>(stateVectorMap_.size()),2);
+ ASSERT_EQ(static_cast<int>(stateVectorMap_.size()), 2);
ASSERT_EQ(nbOfJoints_, 2);
std::string aJoint1("joint1");
EXPECT_TRUE(aJoint1 == stateVectorMap_[0]);
@@ -22,24 +21,23 @@ public:
}
void testParseOptions() {
-
// Void call
- int argc=1;
- char argv1[30]="mocktest";
+ int argc = 1;
+ char argv1[30] = "mocktest";
char *argv[3];
- argv[0]=argv1;
- EXPECT_EQ(parseOptions(argc,argv),-1);
+ argv[0] = argv1;
+ EXPECT_EQ(parseOptions(argc, argv), -1);
// Test help call
- argc=2;
- char argv2[30]="--help";
- argv[1]=argv2;
- EXPECT_EQ(parseOptions(argc,argv),-1);
+ argc = 2;
+ char argv2[30] = "--help";
+ argv[1] = argv2;
+ EXPECT_EQ(parseOptions(argc, argv), -1);
// Test input file
- char argv3[60]="--input-file=libimpl_test_sot_external_interface.so";
- argv[1]=argv3;
- EXPECT_EQ(parseOptions(argc,argv),0);
+ char argv3[60] = "--input-file=libimpl_test_sot_external_interface.so";
+ argv[1] = argv3;
+ EXPECT_EQ(parseOptions(argc, argv), 0);
// Check that the file is what we specified
std::string finalname("libimpl_test_sot_external_interface.so");
@@ -47,11 +45,11 @@ public:
}
void testInitializeRosNode() {
- int argc=1;
+ int argc = 1;
char *argv[1];
- char argv1[30]="mocktest";
+ char argv1[30] = "mocktest";
argv[0] = argv1;
- parseOptions(argc,argv);
+ parseOptions(argc, argv);
initializeServices();
EXPECT_TRUE(service_start_ != 0);
EXPECT_TRUE(service_stop_ != 0);
@@ -60,25 +58,23 @@ public:
}
void testJointStatesPublication() {
- int argc=1;
+ int argc = 1;
char *argv[1];
- char argv1[30]="mocktest";
+ char argv1[30] = "mocktest";
argv[0] = argv1;
- parseOptions(argc,argv);
-
+ parseOptions(argc, argv);
}
void testInitialization() {
-
// Set input arguments
- int argc=2;
+ int argc = 2;
char *argv[2];
- char argv1[30]="mocktest";
- argv[0]=argv1;
- char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
- argv[1]=argv2;
- parseOptions(argc,argv);
+ char argv1[30] = "mocktest";
+ argv[0] = argv1;
+ char argv2[60] = "--input-file=libimpl_test_sot_external_interface.so";
+ argv[1] = argv2;
+ parseOptions(argc, argv);
std::string finalname("libimpl_test_sot_external_interface.so");
EXPECT_TRUE(finalname == dynamicLibraryName_);
@@ -92,28 +88,27 @@ public:
void test_start_stop_dg() {
std::shared_ptr<std_srvs::srv::Empty::Request> empty_rqst;
std::shared_ptr<std_srvs::srv::Empty::Response> empty_resp;
- start_dg(empty_rqst,empty_resp);
+ start_dg(empty_rqst, empty_resp);
EXPECT_FALSE(dynamic_graph_stopped_);
- stop_dg(empty_rqst,empty_resp);
+ stop_dg(empty_rqst, empty_resp);
EXPECT_TRUE(dynamic_graph_stopped_);
}
void test_cleanup() {
CleanUp();
EXPECT_TRUE(sotController_ == NULL);
- std::cout << "sotController_: " << sotController_
- << std::endl;
+ std::cout << "sotController_: " << sotController_ << std::endl;
// Set input arguments
- int argc=2;
+ int argc = 2;
char *argv[2];
- char argv1[30]="mocktest";
- argv[0]=argv1;
- char argv2[60]="--input-file=libimpl_test_sot_external_interface.so";
- argv[1]=argv2;
- parseOptions(argc,argv);
+ char argv1[30] = "mocktest";
+ argv[0] = argv1;
+ char argv2[60] = "--input-file=libimpl_test_sot_external_interface.so";
+ argv[1] = argv2;
+ parseOptions(argc, argv);
std::string finalname("libimpl_test_sot_external_interface.so");
EXPECT_TRUE(finalname == dynamicLibraryName_);
@@ -125,9 +120,8 @@ public:
}
};
-public:
-
- MockSotLoaderBasic* mockSotLoaderBasic_ptr_;
+ public:
+ MockSotLoaderBasic *mockSotLoaderBasic_ptr_;
void SetUp() {
mockSotLoaderBasic_ptr_ = new MockSotLoaderBasic();
@@ -138,36 +132,29 @@ public:
delete mockSotLoaderBasic_ptr_;
mockSotLoaderBasic_ptr_ = nullptr;
}
-
};
-TEST_F(MockSotLoaderBasicTest, TestReadingParameters)
-{
+TEST_F(MockSotLoaderBasicTest, TestReadingParameters) {
mockSotLoaderBasic_ptr_->checkStateVectorMap();
}
-TEST_F(MockSotLoaderBasicTest,ParseOptions)
-{
+TEST_F(MockSotLoaderBasicTest, ParseOptions) {
mockSotLoaderBasic_ptr_->testParseOptions();
}
-TEST_F(MockSotLoaderBasicTest,TestInitialization)
-{
+TEST_F(MockSotLoaderBasicTest, TestInitialization) {
mockSotLoaderBasic_ptr_->testInitialization();
}
-TEST_F(MockSotLoaderBasicTest,TestStartStopDG)
-{
+TEST_F(MockSotLoaderBasicTest, TestStartStopDG) {
mockSotLoaderBasic_ptr_->test_start_stop_dg();
}
-TEST_F(MockSotLoaderBasicTest,TestInitializeRosNode)
-{
+TEST_F(MockSotLoaderBasicTest, TestInitializeRosNode) {
mockSotLoaderBasic_ptr_->testInitializeRosNode();
}
-TEST_F(MockSotLoaderBasicTest,CleanUp)
-{
+TEST_F(MockSotLoaderBasicTest, CleanUp) {
mockSotLoaderBasic_ptr_->test_cleanup();
}
@@ -175,7 +162,7 @@ int main(int argc, char **argv) {
::testing::InitGoogleTest(&argc, argv);
rclcpp::init(argc, argv);
- int r=RUN_ALL_TESTS();
+ int r = RUN_ALL_TESTS();
std::cout << "ros shutdown" << std::endl;
rclcpp::shutdown();