@@ -31,7 +31,6 @@ SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
3131 dynamic_graph_stopped_(true ),
3232 sotController_(NULL ),
3333 sotRobotControllerLibrary_(0 ) {
34-
3534 ros_node_ = dynamic_graph_bridge::get_ros_node (aNodeName);
3635
3736 RCLCPP_INFO (rclcpp::get_logger (" dynamic_graph_bridge::" ),
@@ -51,8 +50,8 @@ int SotLoaderBasic::initPublication() {
5150 RCLCPP_INFO (rclcpp::get_logger (" dynamic_graph_bridge" ),
5251 " SotLoaderBasic::initPublication - create joint_pub" );
5352
54- joint_pub_ = ros_node_->
55- create_publisher<sensor_msgs::msg::JointState>( " joint_states" , 1 );
53+ joint_pub_ = ros_node_->create_publisher <sensor_msgs::msg::JointState>(
54+ " joint_states" , 1 );
5655
5756 RCLCPP_INFO (rclcpp::get_logger (" dynamic_graph_bridge" ),
5857 " SotLoaderBasic::initPublication - after create joint_pub" );
@@ -65,19 +64,17 @@ void SotLoaderBasic::initializeServices() {
6564 " SotLoaderBasic::initializeServices - Ready to start dynamic graph." );
6665
6766 using namespace std ::placeholders;
68- service_start_ = ros_node_->
69- create_service<std_srvs::srv::Empty>(
70- " start_dynamic_graph" ,
71- std::bind (&SotLoaderBasic::start_dg, this , std::placeholders::_1,
72- std::placeholders::_2));
67+ service_start_ = ros_node_->create_service <std_srvs::srv::Empty>(
68+ " start_dynamic_graph" ,
69+ std::bind (&SotLoaderBasic::start_dg, this , std::placeholders::_1,
70+ std::placeholders::_2));
7371 RCLCPP_INFO (rclcpp::get_logger (" dynamic_graph_bridge" ),
7472 " SotLoaderBasic::initializeServices - start_dynamic_graph." );
7573
76- service_stop_ = ros_node_->
77- create_service<std_srvs::srv::Empty>(
78- " stop_dynamic_graph" ,
79- std::bind (&SotLoaderBasic::stop_dg, this , std::placeholders::_1,
80- std::placeholders::_2));
74+ service_stop_ = ros_node_->create_service <std_srvs::srv::Empty>(
75+ " stop_dynamic_graph" ,
76+ std::bind (&SotLoaderBasic::stop_dg, this , std::placeholders::_1,
77+ std::placeholders::_2));
8178 RCLCPP_INFO (rclcpp::get_logger (" dynamic_graph_bridge" ),
8279 " SotLoaderBasic::initializeServices - stop_dynamic_graph." );
8380
@@ -95,9 +92,11 @@ int SotLoaderBasic::readSotVectorStateParam() {
9592 // It is necessary to declare parameters first
9693 // before trying to access them.
9794 if (not ros_node_->has_parameter (" state_vector_map" ))
98- ros_node_->declare_parameter (" state_vector_map" , std::vector<std::string>{});
95+ ros_node_->declare_parameter (" state_vector_map" ,
96+ std::vector<std::string>{});
9997 if (not ros_node_->has_parameter (" joint_state_parallel" ))
100- ros_node_->declare_parameter (" joint_state_parallel" , std::vector<std::string>{});
98+ ros_node_->declare_parameter (" joint_state_parallel" ,
99+ std::vector<std::string>{});
101100
102101 // Read the state vector of the robot
103102 // Defines the order in which the actuators are ordered
@@ -208,8 +207,7 @@ void SotLoaderBasic::loadController() {
208207 dlopen (dynamicLibraryName_.c_str (), RTLD_LAZY | RTLD_GLOBAL);
209208 if (!sotRobotControllerLibrary_) {
210209 RCLCPP_ERROR (rclcpp::get_logger (" dynamic_graph_bridge" ),
211- " Cannot load library: %s" ,
212- dlerror ());
210+ " Cannot load library: %s" , dlerror ());
213211 return ;
214212 }
215213
@@ -223,9 +221,8 @@ void SotLoaderBasic::loadController() {
223221 const char * dlsym_error = dlerror ();
224222 if (dlsym_error) {
225223 RCLCPP_ERROR (rclcpp::get_logger (" dynamic_graph_bridge" ),
226- " Cannot load symbol create: %s from %s" ,
227- dlsym_error,
228- dynamicLibraryName_.c_str () );
224+ " Cannot load symbol create: %s from %s" , dlsym_error,
225+ dynamicLibraryName_.c_str ());
229226 return ;
230227 }
231228
0 commit comments