Skip to content

Commit

Permalink
Fix vector reallocation issue
Browse files Browse the repository at this point in the history
  • Loading branch information
@roncapat
roncapat committed May 18, 2023
1 parent 4ced175 commit a332f0c
Showing 1 changed file with 113 additions and 36 deletions.
149 changes: 113 additions & 36 deletions gazebo_ros2_control/src/gazebo_system.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -472,58 +472,126 @@ void GazeboSystem::registerGazeboTopics(
const hardware_interface::HardwareInfo & hardware_info,
gazebo::physics::ModelPtr parent_model)
{
for (unsigned int j = 0; j < hardware_info.joints.size(); j++) {
auto & info = hardware_info.joints[j];
// This is split in two steps: Count the number and type of sensor and associate the interfaces
// So we have resize only once the structures where the data will be stored, and we can safely
// use pointers to the structures

int n_commands = 0;
int n_states = 0;

// STEP 1
for (unsigned int j = 0; j < hardware_info.joints.size(); j++)
{
auto &info = hardware_info.joints[j];

for (unsigned int i = 0; i < info.command_interfaces.size(); i++)
{
std::string if_name = info.command_interfaces[i].name;

if (if_name != "position" && if_name != "velocity" && if_name != "effort")
{
++n_commands;
}
}

for (unsigned int i = 0; i < info.state_interfaces.size(); i++)
{
std::string if_name = info.state_interfaces[i].name;

if (if_name != "position" && if_name != "velocity" && if_name != "effort")
{
++n_states;
}
}
}

for (unsigned int j = 0; j < hardware_info.sensors.size(); j++)
{
auto &info = hardware_info.sensors[j];

for (unsigned int i = 0; i < info.state_interfaces.size(); i++)
{
std::string if_name = info.state_interfaces[i].name;

if (if_name != "position" && if_name != "velocity" && if_name != "effort"
&& if_name != "orientation.x" && if_name != "orientation.y" && if_name != "orientation.z"
&& if_name != "orientation.w" && if_name != "angular_velocity.x" && if_name != "angular_velocity.y"
&& if_name != "angular_velocity.z" && if_name != "linear_acceleration.x" && if_name != "linear_acceleration.y"
&& if_name != "linear_acceleration.z" && if_name != "force.x" && if_name != "force.y" && if_name != "force.z"
&& if_name != "torque.x" && if_name != "torque.y" && if_name != "torque.z")
{
++n_states;
}
}
}

dataPtr->gazebo_topic_cmd_.resize(n_commands);
dataPtr->gz_pubs_.reserve(n_commands);
dataPtr->gazebo_topic_fbk_.resize(n_states);
dataPtr->gz_subs_.reserve(n_states);
unsigned int cc = 0;
unsigned int cs = 0;

// STEP 2
for (unsigned int j = 0; j < hardware_info.joints.size(); j++)
{
auto &info = hardware_info.joints[j];
std::string joint_name = this->dataPtr->joint_names_[j] = info.name;

for (unsigned int i = 0; i < info.command_interfaces.size(); i++) {
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Registering gazebo topics for joint: " << joint_name);

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:");

for (unsigned int i = 0; i < info.command_interfaces.size(); i++)
{
std::string if_name = info.command_interfaces[i].name;
std::string initial_v = info.command_interfaces[i].initial_value;

if (if_name != "position" && if_name != "velocity" && if_name != "effort") {
auto & ref = this->dataPtr->gazebo_topic_cmd_.emplace_back(0.0);
if (if_name != "position" && if_name != "velocity" && if_name != "effort")
{
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << if_name);

this->dataPtr->command_interfaces_.emplace_back(
joint_name,
if_name,
&ref);
joint_name,
if_name,
&(this->dataPtr->gazebo_topic_cmd_[cc]));

if (!initial_v.empty()) {
ref = std::stod(initial_v);
if (!initial_v.empty())
{
this->dataPtr->gazebo_topic_cmd_[cc] = std::stod(initial_v);
}

dataPtr->gz_pubs_.push_back(
this->dataPtr->transport_nh_->Advertise<gazebo::msgs::Any>(
"~/" + joint_name + "/" +
if_name)
);
cc++;
}
}

this->dataPtr->gazebo_topic_fbk_.reserve(100);
this->dataPtr->gz_subs_.reserve(100);
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");

for (unsigned int i = 0; i < info.state_interfaces.size(); i++) {
std::string if_name = info.state_interfaces[i].name;

if (if_name != "position" && if_name != "velocity" && if_name != "effort") {
this->dataPtr->gazebo_topic_fbk_.emplace_back(0.0);

int idx = this->dataPtr->gazebo_topic_fbk_.size() - 1;
if (if_name != "position" && if_name != "velocity" && if_name != "effort"){
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << if_name);

this->dataPtr->state_interfaces_.emplace_back(
joint_name,
if_name,
&this->dataPtr->gazebo_topic_fbk_[idx]);

joint_name,
if_name,
&(this->dataPtr->gazebo_topic_fbk_[cs]));
dataPtr->gz_subs_.push_back(
this->dataPtr->transport_nh_->Subscribe<gazebo::msgs::Any>(
"~/" + joint_name + "/" + if_name,
[this, idx](ConstAnyPtr & msg) mutable -> void {
this->dataPtr->gazebo_topic_fbk_[idx] = msg->double_value();
"~/" + joint_name + "/" + if_name,
[this, cs](ConstAnyPtr & msg) mutable -> void {
this->dataPtr->gazebo_topic_fbk_[cs] = msg->double_value();
}
)
);
cs++;
}
}
}
Expand All @@ -532,27 +600,37 @@ void GazeboSystem::registerGazeboTopics(
auto & info = hardware_info.sensors[j];
std::string sensor_name = info.name;

for (unsigned int i = 0; i < info.state_interfaces.size(); i++) {
std::string if_name = info.state_interfaces[i].name;
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Registering gazebo topics for sensor: " << sensor_name);

RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");

if (if_name != "position" && if_name != "velocity" && if_name != "effort") {
this->dataPtr->gazebo_topic_fbk_.emplace_back(0.0);
for (unsigned int i = 0; i < info.state_interfaces.size(); i++)
{
std::string if_name = info.state_interfaces[i].name;

int idx = this->dataPtr->gazebo_topic_fbk_.size() - 1;
if (if_name != "position" && if_name != "velocity" && if_name != "effort"
&& if_name != "orientation.x" && if_name != "orientation.y" && if_name != "orientation.z"
&& if_name != "orientation.w" && if_name != "angular_velocity.x" && if_name != "angular_velocity.y"
&& if_name != "angular_velocity.z" && if_name != "linear_acceleration.x" && if_name != "linear_acceleration.y"
&& if_name != "linear_acceleration.z" && if_name != "force.x" && if_name != "force.y" && if_name != "force.z"
&& if_name != "torque.x" && if_name != "torque.y" && if_name != "torque.z")
{
RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << if_name);

this->dataPtr->state_interfaces_.emplace_back(
sensor_name,
if_name,
&this->dataPtr->gazebo_topic_fbk_[idx]);

sensor_name,
if_name,
&(this->dataPtr->gazebo_topic_fbk_[cs]));
dataPtr->gz_subs_.push_back(
this->dataPtr->transport_nh_->Subscribe<gazebo::msgs::Any>(
"~/" + sensor_name + "/" + if_name,
[this, idx](ConstAnyPtr & msg) mutable -> void {
this->dataPtr->gazebo_topic_fbk_[idx] = msg->double_value();
"~/" + sensor_name + "/" + if_name,
[this, cs](ConstAnyPtr & msg) mutable -> void {
this->dataPtr->gazebo_topic_fbk_[cs] = msg->double_value();
}
)
);
cs++;
}
}
}
Expand Down Expand Up @@ -733,7 +811,6 @@ hardware_interface::return_type GazeboSystem::write(
counterMsg.set_double_value(this->dataPtr->gazebo_topic_cmd_[j]);
dataPtr->gz_pubs_[j]->Publish(counterMsg);
}

this->dataPtr->last_update_sim_time_ros_ = sim_time_ros;

return hardware_interface::return_type::OK;
Expand Down

0 comments on commit a332f0c

Please sign in to comment.