Skip to content

Commit a8803ae

Browse files
olivier-stasseolivier-stasse
committed
Reuse the MultiThreaded::Executor to handle ROS Nodes.
1 parent a48149f commit a8803ae

File tree

4 files changed

+54
-44
lines changed

4 files changed

+54
-44
lines changed

‎include/dynamic_graph_bridge/sot_loader.hh

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -80,7 +80,7 @@ class SotLoader : public SotLoaderBasic {
8080
geometry_msgs::msg::TransformStamped freeFlyerPose_;
8181

8282
public:
83-
SotLoader(const std::string &aNodeName=std::string("SotLoader"));
83+
SotLoader(const std::string &aNodeName=std::string("sot_loader"));
8484
virtual ~SotLoader();
8585

8686
// \brief Create a thread for ROS and start the control loop.

‎include/dynamic_graph_bridge/sot_loader_basic.hh

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -38,8 +38,11 @@
3838
namespace po = boost::program_options;
3939
namespace dgs = dynamicgraph::sot;
4040

41-
class SotLoaderBasic : public rclcpp::Node {
41+
class SotLoaderBasic {
4242
protected:
43+
// RosNode
44+
dynamic_graph_bridge::RosNodePtr ros_node_;
45+
4346
// Dynamic graph is stopped.
4447
bool dynamic_graph_stopped_;
4548

@@ -80,8 +83,9 @@ class SotLoaderBasic : public rclcpp::Node {
8083
// Ordered list of joint names describing the robot state.
8184
std::vector<std::string> stateVectorMap_;
8285

86+
8387
public:
84-
SotLoaderBasic(const std::string& aNodeName = std::string("SotLoaderBasic"));
88+
SotLoaderBasic(const std::string& aNodeName = std::string("sot_loader_basic"));
8589
virtual ~SotLoaderBasic();
8690

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

‎src/sot_loader.cpp

Lines changed: 11 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -83,10 +83,10 @@ void SotLoader::initializeServices() {
8383
freeFlyerPose_.header.frame_id = "odom";
8484
freeFlyerPose_.child_frame_id = "base_link";
8585

86-
if (not has_parameter("tf_base_link"))
87-
declare_parameter("tf_base_link", std::string("base_link"));
86+
if (not ros_node_->has_parameter("tf_base_link"))
87+
ros_node_->declare_parameter("tf_base_link", std::string("base_link"));
8888

89-
if (get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
89+
if (ros_node_->get_parameter("tf_base_link", freeFlyerPose_.child_frame_id)) {
9090
RCLCPP_INFO_STREAM(rclcpp::get_logger("dynamic_graph_bridge"),
9191
"Publishing " << freeFlyerPose_.child_frame_id << " wrt "
9292
<< freeFlyerPose_.header.frame_id);
@@ -98,7 +98,8 @@ void SotLoader::initializeServices() {
9898
control_.resize(static_cast<std::size_t>(nbOfJoints_));
9999

100100
// Creates a publisher for the free flyer transform.
101-
freeFlyerPublisher_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
101+
freeFlyerPublisher_ =
102+
std::make_shared<tf2_ros::TransformBroadcaster>(ros_node_);
102103
}
103104

104105
void SotLoader::fillSensors(map<string, dgs::SensorValues> &sensorsIn) {
@@ -164,9 +165,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
164165
joint_state_.position[i] = angleEncoder_[i];
165166
}
166167

167-
std::cerr << "parallel_joints_to_state_vector_.size(): "
168-
<< parallel_joints_to_state_vector_.size() << std::endl;
169-
170168
for (unsigned int i = 0; i < parallel_joints_to_state_vector_.size(); i++) {
171169
joint_state_.position[i + nbOfJoints_std_s] =
172170
coefficient_parallel_joints_[i] *
@@ -184,7 +182,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
184182
throw anInvalidArgument;
185183
}
186184

187-
std::cerr << "Reached poseValue_" << std::endl;
188185
std::vector<double> poseValue = controlValues["baseff"].getValues();
189186

190187
freeFlyerPose_.transform.translation.x = poseValue[0];
@@ -199,7 +196,6 @@ void SotLoader::readControl(map<string, dgs::ControlValues> &controlValues) {
199196
freeFlyerPose_.header.stamp = joint_state_.header.stamp;
200197
// Publish
201198
freeFlyerPublisher_->sendTransform(freeFlyerPose_);
202-
std::cerr << "end of readControl" << std::endl;
203199
}
204200

205201
void SotLoader::setup() {
@@ -230,11 +226,11 @@ void SotLoader::workThreadLoader() {
230226
double periodd;
231227

232228
/// Declare parameters
233-
if (not has_parameter("dt")) declare_parameter<double>("dt", 0.01);
234-
if (not has_parameter("paused")) declare_parameter<bool>("paused", false);
229+
if (not ros_node_->has_parameter("dt")) ros_node_->declare_parameter<double>("dt", 0.01);
230+
if (not ros_node_->has_parameter("paused")) ros_node_->declare_parameter<bool>("paused", false);
235231

236232
//
237-
get_parameter_or("dt", periodd, 0.001);
233+
ros_node_->get_parameter_or("dt", periodd, 0.001);
238234
rclcpp::Rate rate(1 / periodd); // 1 kHz
239235

240236
DataToLog dataToLog(5000);
@@ -250,15 +246,15 @@ void SotLoader::workThreadLoader() {
250246
rclcpp::Time time;
251247
while (rclcpp::ok() && !isDynamicGraphStopped()) {
252248
// Check if the user did not paused geometric_simu
253-
get_parameter_or("paused", paused, false);
249+
ros_node_->get_parameter_or("paused", paused, false);
254250

255251
if (!paused) {
256252
time = aClock.now();
257253
oneIteration();
258254

259255
rclcpp::Duration d = aClock.now() - time;
260-
dataToLog.record((time - timeOrigin).nanoseconds() * 1.0e9,
261-
d.nanoseconds() * 1.0e9);
256+
dataToLog.record((double)((time - timeOrigin).nanoseconds()) * 1.0e9,
257+
(double)(d.nanoseconds()) * 1.0e9);
262258
}
263259
rate.sleep();
264260
}

‎src/sot_loader_basic.cpp

Lines changed: 36 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -27,10 +27,13 @@ using namespace dynamicgraph::sot;
2727
namespace po = boost::program_options;
2828

2929
SotLoaderBasic::SotLoaderBasic(const std::string& aNodeName)
30-
: rclcpp::Node(aNodeName),
30+
: ros_node_(nullptr),
3131
dynamic_graph_stopped_(true),
3232
sotController_(NULL),
3333
sotRobotControllerLibrary_(0) {
34+
35+
ros_node_ = dynamic_graph_bridge::get_ros_node(aNodeName);
36+
3437
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge::"),
3538
"Beginning of SotLoaderBasic");
3639
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -48,7 +51,7 @@ int SotLoaderBasic::initPublication() {
4851
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
4952
"SotLoaderBasic::initPublication - create joint_pub");
5053

51-
joint_pub_ =
54+
joint_pub_ = ros_node_->
5255
create_publisher<sensor_msgs::msg::JointState>("joint_states", 1);
5356

5457
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
@@ -62,19 +65,21 @@ void SotLoaderBasic::initializeServices() {
6265
"SotLoaderBasic::initializeServices - Ready to start dynamic graph.");
6366

6467
using namespace std::placeholders;
65-
service_start_ = create_service<std_srvs::srv::Empty>(
66-
"start_dynamic_graph",
67-
std::bind(&SotLoaderBasic::start_dg, this, std::placeholders::_1,
68-
std::placeholders::_2));
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));
6973
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
70-
"SotLoaderBasic::initializeServices - started dynamic graph.");
74+
"SotLoaderBasic::initializeServices - start_dynamic_graph.");
7175

72-
service_stop_ = create_service<std_srvs::srv::Empty>(
73-
"stop_dynamic_graph",
74-
std::bind(&SotLoaderBasic::stop_dg, this, std::placeholders::_1,
75-
std::placeholders::_2));
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));
7681
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
77-
"SotLoaderBasic::initializeServices - stopped dynamic graph.");
82+
"SotLoaderBasic::initializeServices - stop_dynamic_graph.");
7883

7984
parameter_server_read_robot_description();
8085
}
@@ -89,22 +94,22 @@ int SotLoaderBasic::readSotVectorStateParam() {
8994

9095
// It is necessary to declare parameters first
9196
// before trying to access them.
92-
if (not has_parameter("state_vector_map"))
93-
declare_parameter("state_vector_map", std::vector<std::string>{});
94-
if (not has_parameter("joint_state_parallel"))
95-
declare_parameter("joint_state_parallel", std::vector<std::string>{});
97+
if (not ros_node_->has_parameter("state_vector_map"))
98+
ros_node_->declare_parameter("state_vector_map", std::vector<std::string>{});
99+
if (not ros_node_->has_parameter("joint_state_parallel"))
100+
ros_node_->declare_parameter("joint_state_parallel", std::vector<std::string>{});
96101

97102
// Read the state vector of the robot
98103
// Defines the order in which the actuators are ordered
99104
try {
100105
std::string aParameterName("state_vector_map");
101-
if (!get_parameter(aParameterName, stateVectorMap_)) {
106+
if (!ros_node_->get_parameter(aParameterName, stateVectorMap_)) {
102107
logic_error aLogicError(
103108
"SotLoaderBasic::readSotVectorStateParam : State_vector_map is "
104109
"empty");
105110
throw aLogicError;
106111
}
107-
RCLCPP_INFO(get_logger(), "state_vector_map parameter size %ld",
112+
RCLCPP_INFO(ros_node_->get_logger(), "state_vector_map parameter size %ld",
108113
stateVectorMap_.size());
109114
} catch (exception& e) {
110115
std::throw_with_nested(std::logic_error("Unable to call get_parameter"));
@@ -119,7 +124,7 @@ int SotLoaderBasic::readSotVectorStateParam() {
119124

120125
std::string prefix("joint_state_parallel");
121126
std::map<std::string, rclcpp::Parameter> joint_state_parallel;
122-
get_parameters(prefix, joint_state_parallel);
127+
ros_node_->get_parameters(prefix, joint_state_parallel);
123128

124129
// Iterates over the map joint_state_parallel
125130
for (std::map<std::string, rclcpp::Parameter>::iterator it_map_expression =
@@ -202,7 +207,9 @@ void SotLoaderBasic::loadController() {
202207
sotRobotControllerLibrary_ =
203208
dlopen(dynamicLibraryName_.c_str(), RTLD_LAZY | RTLD_GLOBAL);
204209
if (!sotRobotControllerLibrary_) {
205-
std::cerr << "Cannot load library: " << dlerror() << '\n';
210+
RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
211+
"Cannot load library: %s",
212+
dlerror());
206213
return;
207214
}
208215

@@ -215,14 +222,17 @@ void SotLoaderBasic::loadController() {
215222
dlsym(sotRobotControllerLibrary_, "createSotExternalInterface")));
216223
const char* dlsym_error = dlerror();
217224
if (dlsym_error) {
218-
std::cerr << "Cannot load symbol create: " << dlsym_error << " from "
219-
<< dynamicLibraryName_ << '\n';
225+
RCLCPP_ERROR(rclcpp::get_logger("dynamic_graph_bridge"),
226+
"Cannot load symbol create: %s from %s",
227+
dlsym_error,
228+
dynamicLibraryName_.c_str() );
220229
return;
221230
}
222231

223232
// Create robot-controller
224233
sotController_ = createSot();
225-
cout << "Went out from loadController." << endl;
234+
RCLCPP_INFO(rclcpp::get_logger("dynamic_graph_bridge"),
235+
"Went out from loadController.");
226236
}
227237

228238
void SotLoaderBasic::CleanUp() {
@@ -269,13 +279,13 @@ void SotLoaderBasic::stop_dg(
269279
}
270280

271281
bool SotLoaderBasic::parameter_server_read_robot_description() {
272-
if (!has_parameter("robot_description")) {
273-
declare_parameter("robot_description", std::string(""));
282+
if (!ros_node_->has_parameter("robot_description")) {
283+
ros_node_->declare_parameter("robot_description", std::string(""));
274284
}
275285

276286
std::string robot_description;
277287
std::string parameter_name("robot_description");
278-
get_parameter(parameter_name, robot_description);
288+
ros_node_->get_parameter(parameter_name, robot_description);
279289
if (robot_description.empty()) {
280290
RCLCPP_FATAL(rclcpp::get_logger("dynamic_graph_bridge"),
281291
"Parameter robot_description is empty");

0 commit comments

Comments
 (0)