Skip to content

Commit 1b33fc3

Browse files
olivier-stasseolivier-stasse
committed
Cleaning
1 parent 3048347 commit 1b33fc3

20 files changed

+208
-125
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,8 +24,6 @@ set(CATKIN_ENABLE_TESTING OFF)
2424
# JRL-cmakemodule setup
2525
include(cmake/base.cmake)
2626
include(cmake/boost.cmake)
27-
include(cmake/python.cmake)
28-
include(cmake/ros.cmake)
2927

3028
# Project definition
3129
compute_project_args(PROJECT_ARGS LANGUAGES CXX)
@@ -44,13 +42,13 @@ find_package(std_srvs REQUIRED)
4442
find_package(geometry_msgs REQUIRED)
4543
find_package(sensor_msgs REQUIRED)
4644
find_package(tf2_ros REQUIRED)
45+
find_package(urdfdom REQUIRED)
4746

4847
add_project_dependency(Boost REQUIRED COMPONENTS program_options)
4948
add_project_dependency(dynamic_graph_bridge_msgs 0.3.0 REQUIRED)
5049
add_project_dependency(sot-core REQUIRED)
5150

5251
if(BUILD_PYTHON_INTERFACE)
53-
findpython()
5452
search_for_boost_python()
5553
string(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
5654
add_project_dependency(dynamic-graph-python 4.0.0 REQUIRED)

‎include/dynamic_graph_bridge/ros.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,8 @@
99

1010
#include <memory>
1111

12+
#include <boost/chrono.hpp>
13+
1214
// ROS includes
1315
#include "dynamic_graph_bridge_msgs/srv/run_python_command.hpp"
1416
#include "dynamic_graph_bridge_msgs/srv/run_python_file.hpp"

‎include/dynamic_graph_bridge/sot_loader_basic.hh

Lines changed: 2 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@
4141
namespace po = boost::program_options;
4242
namespace dgs = dynamicgraph::sot;
4343

44-
class SotLoaderBasic {
44+
class SotLoaderBasic : public rclcpp::Node {
4545
protected:
4646
// Dynamic graph is stopped.
4747
bool dynamic_graph_stopped_;
@@ -73,9 +73,6 @@ class SotLoaderBasic {
7373
// Joint state publication.
7474
rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr joint_pub_;
7575

76-
// Node reference
77-
rclcpp::Node::SharedPtr nh_;
78-
7976
// Joint state to be published.
8077
sensor_msgs::msg::JointState joint_state_;
8178

@@ -87,7 +84,7 @@ class SotLoaderBasic {
8784
std::vector<std::string> stateVectorMap_;
8885

8986
public:
90-
SotLoaderBasic();
87+
SotLoaderBasic(const std::string & aNodeName=std::string("SotLoaderBasic"));
9188
virtual ~SotLoaderBasic();
9289

9390
// \brief Read user input to extract the path of the SoT dynamic library.

‎src/CMakeLists.txt

Lines changed: 9 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,9 @@ ament_target_dependencies(
3838
geometry_msgs
3939
sensor_msgs
4040
tf2_ros
41-
rcutils)
41+
rcutils
42+
urdfdom)
43+
4244
if(SUFFIX_SO_VERSION)
4345
set_target_properties(ros_bridge PROPERTIES SOVERSION ${PROJECT_VERSION})
4446
endif(SUFFIX_SO_VERSION)
@@ -62,9 +64,12 @@ foreach(plugin ${plugins})
6264
${PROJECT_VERSION})
6365
endif(SUFFIX_SO_VERSION)
6466
target_link_libraries(
65-
${plugin_library_name} ${${plugin_library_name}_deps} ${catkin_LIBRARIES}
66-
ros_bridge
67-
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
67+
${plugin_library_name} ${${plugin_library_name}_deps}
68+
ros_bridge
69+
dynamic_graph_bridge_msgs::dynamic_graph_bridge_msgs__rosidl_typesupport_cpp
70+
rclcpp::rclcpp
71+
rcl_interfaces::rcl_interfaces__rosidl_typesupport_cpp
72+
std_srvs::std_srvs__rosidl_typesupport_cpp
6873
)
6974
target_include_directories(
7075
${plugin_library_name}

‎src/dg_ros_mapping.cpp

Lines changed: 8 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -66,22 +66,25 @@ DG_TO_ROS_IMPL(DgRosMappingMatrix) {
6666
// For simplicity and easiness of maintenance we implement a slower version
6767
// of the copy here.
6868
ros_dst.width = static_cast<unsigned int>(dg_src.rows());
69-
ros_dst.data.resize(dg_src.size());
70-
for (int row = 0; row < dg_src.rows(); ++row) {
69+
ros_dst.data.resize(static_cast<std::size_t>(dg_src.size()));
70+
for (int row = 0 ; row < dg_src.rows(); ++row) {
7171
for (int col = 0; col < dg_src.cols(); ++col) {
72-
ros_dst.data[row * ros_dst.width + col] = dg_src(row, col);
72+
ros_dst.data[static_cast<unsigned int>(row) * ros_dst.width +
73+
static_cast<unsigned int>(col)] =
74+
dg_src(row, col);
7375
}
7476
}
7577
}
7678
ROS_TO_DG_IMPL(DgRosMappingMatrix) {
7779
// For simplicity and easiness of maintenance we implement a slower version
7880
// of the copy here.
79-
int rows = ros_src.width;
81+
int rows = static_cast<int>(ros_src.width);
8082
int cols = static_cast<int>(ros_src.data.size()) / static_cast<int>(rows);
8183
dg_dst.resize(rows, cols);
8284
for (int row = 0; row < dg_dst.rows(); ++row) {
8385
for (int col = 0; col < dg_dst.cols(); ++col) {
84-
dg_dst(row, col) = ros_src.data[row * rows + col];
86+
dg_dst(row, col) =
87+
ros_src.data[static_cast<std::vector<double>::size_type>(row * rows + col)];
8588
}
8689
}
8790
}

‎src/dg_ros_mapping.hpp

Lines changed: 10 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -8,11 +8,15 @@
88
*/
99
#pragma once
1010

11+
#include "fwd.hpp"
12+
1113
// Standard includes
1214
#include <sstream>
1315
#include <utility>
1416
#include <vector>
1517

18+
#include <boost/chrono.hpp>
19+
1620
// Dynamic Graph types.
1721
#include <dynamic-graph/linear-algebra.h>
1822
#include <dynamic-graph/signal-ptr.h>
@@ -107,7 +111,7 @@ class DgRosMapping {
107111
/** @brief Output signal type. */
108112
typedef dg::SignalTimeDependent<dg_t, dg::sigtime_t> signal_out_t;
109113
/** @brief Output signal type. */
110-
typedef dg::SignalTimeDependent<timestamp_t, dg::sigtime_t>
114+
typedef dg::SignalTimeDependent<dynamic_graph_bridge::timestamp_t, dg::sigtime_t>
111115
signal_timestamp_out_t;
112116
/** @brief Input signal type. */
113117
typedef dg::SignalPtr<dg_t, dg::sigtime_t> signal_in_t;
@@ -126,7 +130,7 @@ class DgRosMapping {
126130
*/
127131
template <typename SignalTypePtr>
128132
static void set_default(SignalTypePtr s) {
129-
s->setConstant(DgRosMapping<RosType, DgType>::default_value);
133+
s->setConstant(default_value);
130134
}
131135

132136
/**
@@ -135,17 +139,17 @@ class DgRosMapping {
135139
* @param s
136140
*/
137141
static void set_default(dg_t& d) {
138-
d = DgRosMapping<RosType, DgType>::default_value;
142+
d = default_value;
139143
}
140144

141145
/**
142-
* @brief Convert ROS time to std::chrono.
146+
* @brief Convert ROS time to boost::chrono.
143147
*
144148
* @param ros_time
145149
* @return timestamp_t
146150
*/
147151
static timestamp_t from_ros_time(rclcpp::Time ros_time) {
148-
return epoch_time() + std::chrono::nanoseconds(ros_time.nanoseconds());
152+
return epoch_time() + boost::chrono::nanoseconds(ros_time.nanoseconds());
149153
}
150154

151155
/**
@@ -154,7 +158,7 @@ class DgRosMapping {
154158
* @return timestamp_t
155159
*/
156160
static timestamp_t epoch_time() {
157-
return std::chrono::time_point<std::chrono::high_resolution_clock>{};
161+
return boost::chrono::time_point<boost::chrono::high_resolution_clock>{};
158162
}
159163

160164
/**

‎src/fwd.hpp

Lines changed: 13 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -12,14 +12,16 @@
1212

1313
#include <chrono>
1414
#include <ostream>
15+
#include <iostream>
16+
#include <boost/chrono.hpp>
1517

1618
namespace dynamic_graph_bridge {
1719
/** @brief Time stamp type. */
18-
typedef std::chrono::time_point<std::chrono::high_resolution_clock> timestamp_t;
20+
typedef boost::chrono::time_point<boost::chrono::high_resolution_clock> timestamp_t;
1921

2022
} // namespace dynamic_graph_bridge
2123

22-
namespace dynamicgraph {
24+
//namespace dynamicgraph {
2325
/**
2426
* @brief out stream the time stamp data.
2527
*
@@ -30,13 +32,16 @@ namespace dynamicgraph {
3032
* For clang this function needs to be forward declared before the template
3133
* using it. This is more in accordance to the standard.
3234
*/
33-
inline std::ostream &operator<<(
34-
std::ostream &os, const dynamic_graph_bridge::timestamp_t &time_stamp) {
35-
std::chrono::time_point<std::chrono::high_resolution_clock,
36-
std::chrono::milliseconds>
35+
std::ostream &operator<<(
36+
std::ostream &os,
37+
const dynamic_graph_bridge::timestamp_t &time_stamp) {
38+
boost::chrono::time_point<boost::chrono::high_resolution_clock,
39+
boost::chrono::milliseconds>
3740
time_stamp_nanosec =
38-
std::chrono::time_point_cast<std::chrono::milliseconds>(time_stamp);
41+
boost::chrono::time_point_cast<boost::chrono::milliseconds>(time_stamp);
3942
os << time_stamp_nanosec.time_since_epoch().count();
4043
return os;
4144
}
42-
} // namespace dynamicgraph
45+
46+
47+
//} // namespace dynamicgraph

‎src/ros_parameter.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -38,7 +38,7 @@ bool parameter_server_read_robot_description(rclcpp::Node::SharedPtr nh) {
3838
// Then set the robot model.
3939
aRobotUtil->set_parameter(parameter_name, robot_description);
4040
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
41-
"Set parameter_name : %s.", parameter_name.c_str());
41+
"parameter_server_read_robot_description : Set parameter_name : %s.", parameter_name.c_str());
4242
// Everything went fine.
4343
return true;
4444
}

‎src/ros_python_interpreter_client.cpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -71,7 +71,7 @@ std::string RosPythonInterpreterClient::run_python_command(
7171
return_string += response.get()->result;
7272
}
7373
} catch (const std::exception& ex) {
74-
RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), ex.what());
74+
RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"), "%s", ex.what());
7575
connect_to_rosservice_run_python_command(timeout_connection_s_);
7676
} catch (...) {
7777
RCLCPP_INFO(rclcpp::get_logger("RosPythonInterpreterClient"),

‎src/ros_python_interpreter_server.cpp

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,6 @@
1414
#include <memory>
1515

1616
namespace dynamic_graph_bridge {
17-
static const int queueSize = 5;
1817

1918
RosPythonInterpreterServer::RosPythonInterpreterServer()
2019
: interpreter_(),

0 commit comments

Comments
 (0)