Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Added robotParamNode to gazebo_ros2_control plugin #8

Merged
merged 5 commits into from
Jun 26, 2020
Merged

Added robotParamNode to gazebo_ros2_control plugin #8

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
c16ef17
Added rosParamNode to gazebo_ros2_control plugin
ahcorde Jun 4, 2020
d520c1f
Merge remote-tracking branch 'origin/master' into ahcorde/feature/rob…
ahcorde Jun 25, 2020
8f00941
Fixed merge
ahcorde Jun 25, 2020
c7162a8
Merge remote-tracking branch 'origin/master' into ahcorde/feature/rob…
ahcorde Jun 26, 2020
82249c0
Fixed debug traces
ahcorde Jun 26, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 15 additions & 5 deletions gazebo_ros2_control/src/gazebo_ros2_control_plugin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,7 @@ class GazeboRosControlPrivate
// Strings
std::string robot_namespace_;
std::string robot_description_;
std::string robot_description_node_;
std::string param_file_;

// Transmissions in this plugin's scope
Expand Down Expand Up @@ -169,6 +170,13 @@ void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::Element
impl_->robot_description_ = "robot_description"; // default
}

// Get robot_description ROS param name
if (impl_->sdf_->HasElement("robotParamNode")) {
impl_->robot_description_node_ = impl_->sdf_->GetElement("robotParamNode")->Get<std::string>();
} else {
impl_->robot_description_node_ = "robot_state_publisher"; // default
}

// Get the robot simulation interface type
if (impl_->sdf_->HasElement("robotSimType")) {
impl_->robot_hw_sim_type_str_ = impl_->sdf_->Get<std::string>("robotSimType");
Expand Down Expand Up @@ -487,21 +495,23 @@ std::string GazeboRosControlPrivate::getURDF(std::string param_name) const
{
std::string urdf_string;

using namespace std::chrono_literals;
auto parameters_client = std::make_shared<rclcpp::AsyncParametersClient>(
model_nh_,
"robot_state_publisher");
model_nh_, robot_description_node_);
while (!parameters_client->wait_for_service(0.5s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(
model_nh_->get_logger(), "Interrupted while waiting for the service. Exiting.");
model_nh_->get_logger(), "Interrupted while waiting for %s service. Exiting.",
robot_description_node_.c_str());
return 0;
}
RCLCPP_ERROR(
model_nh_->get_logger(), "service not available, waiting again...");
model_nh_->get_logger(), "%s service not available, waiting again...",
robot_description_node_.c_str());
}

RCLCPP_ERROR(
model_nh_->get_logger(), "connected to service!! /robot_state_publisher");
model_nh_->get_logger(), "connected to service!! %s", robot_description_node_.c_str());

// search and wait for robot_description on param server
while (urdf_string.empty()) {
Expand Down