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 support for using ROS 2 parameters to spawn entities in Gazebo using ros_gz_sim::create #475

Merged
merged 5 commits into from
Dec 28, 2023
Merged

Added support for using ROS 2 parameters to spawn entities in Gazebo using ros_gz_sim::create #475

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
65628d1
Added support for ROS 2 parameters
Ayush1285 Dec 25, 2023
1b1b091
Allowed renaming
Ayush1285 Dec 25, 2023
d2499fa
included <memory> header
Ayush1285 Dec 26, 2023
6314ba4
included TODO
Ayush1285 Dec 26, 2023
acf2f4c
Fixed cpplint error
Ayush1285 Dec 27, 2023
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
174 changes: 125 additions & 49 deletions ros_gz_sim/src/create.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,10 @@

#include <sstream>
#include <string>
#include <chrono>
Ayush1285 marked this conversation as resolved.
Show resolved Hide resolved
#include <future>
#include <functional>
#include <memory>

#include <gz/math/Pose3.hh>
#include <gz/msgs/Utility.hh>
Expand All @@ -29,7 +33,11 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

// ROS interface for spawning entities into Gazebo.
// Suggested for use with roslaunch and loading entities from ROS param.
// If these are not needed, just use the `gz service` command line instead.

// \TODO(anyone) Remove GFlags in ROS-J
DEFINE_string(world, "", "World name.");
DEFINE_string(file, "", "Load XML from a file.");
DEFINE_string(param, "", "Load XML from a ROS param.");
Expand All @@ -44,9 +52,44 @@ DEFINE_double(R, 0, "Roll component of initial orientation, in radians.");
DEFINE_double(P, 0, "Pitch component of initial orientation, in radians.");
DEFINE_double(Y, 0, "Yaw component of initial orientation, in radians.");

// ROS interface for spawning entities into Gazebo.
// Suggested for use with roslaunch and loading entities from ROS param.
// If these are not needed, just use the `gz service` command line instead.
// Utility Function to avoid code duplication

bool set_XML_from_topic(
const std::string & topic_name, const rclcpp::Node::SharedPtr ros2_node,
gz::msgs::EntityFactory & req)
{
const auto timeout = std::chrono::seconds(1);
std::promise<std::string> xml_promise;
std::shared_future<std::string> xml_future(xml_promise.get_future());

std::function<void(const std_msgs::msg::String::SharedPtr)> fun =
[&xml_promise](const std_msgs::msg::String::SharedPtr msg) {
xml_promise.set_value(msg->data);
};

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(ros2_node);
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr description_subs;
// Transient local is similar to latching in ROS 1.
description_subs = ros2_node->create_subscription<std_msgs::msg::String>(
topic_name, rclcpp::QoS(1).transient_local(), fun);

rclcpp::FutureReturnCode future_ret;
do {
RCLCPP_INFO(ros2_node->get_logger(), "Waiting messages on topic [%s].", topic_name.c_str());
future_ret = executor.spin_until_future_complete(xml_future, timeout);
} while (rclcpp::ok() && future_ret != rclcpp::FutureReturnCode::SUCCESS);

if (future_ret == rclcpp::FutureReturnCode::SUCCESS) {
req.set_sdf(xml_future.get());
return true;
} else {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from topic [%s].", topic_name.c_str());
return false;
}
}

int main(int _argc, char ** _argv)
{
auto filtered_arguments = rclcpp::init_and_remove_ros_arguments(_argc, _argv);
Expand Down Expand Up @@ -78,8 +121,25 @@ int main(int _argc, char ** _argv)
}
delete[] filtered_argv;

// Declare ROS 2 parameters to be passed from launch file
ros2_node->declare_parameter("world", "");
ros2_node->declare_parameter("file", "");
ros2_node->declare_parameter("string", "");
ros2_node->declare_parameter("topic", "");
ros2_node->declare_parameter("name", "");
ros2_node->declare_parameter("allow_renaming", false);
ros2_node->declare_parameter("x", static_cast<double>(0));
ros2_node->declare_parameter("y", static_cast<double>(0));
ros2_node->declare_parameter("z", static_cast<double>(0));
ros2_node->declare_parameter("R", static_cast<double>(0));
ros2_node->declare_parameter("P", static_cast<double>(0));
ros2_node->declare_parameter("Y", static_cast<double>(0));

// World
std::string world_name = FLAGS_world;
std::string world_name = ros2_node->get_parameter("world").as_string();
if (world_name.empty() && !FLAGS_world.empty()) {
world_name = FLAGS_world;
}
if (world_name.empty()) {
// If caller doesn't provide a world name, get list of worlds from gz-sim server
gz::transport::Node node;
Expand Down Expand Up @@ -113,69 +173,85 @@ int main(int _argc, char ** _argv)
// Request message
gz::msgs::EntityFactory req;

// File
if (!FLAGS_file.empty()) {
req.set_sdf_filename(FLAGS_file);
} else if (!FLAGS_param.empty()) { // Param
ros2_node->declare_parameter<std::string>(FLAGS_param);

std::string xmlStr;
if (ros2_node->get_parameter(FLAGS_param, xmlStr)) {
req.set_sdf(xmlStr);
} else {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from param [%s].", FLAGS_param.c_str());
// Get ROS parameters
std::string file_name = ros2_node->get_parameter("file").as_string();
std::string xml_string = ros2_node->get_parameter("string").as_string();
std::string topic_name = ros2_node->get_parameter("topic").as_string();
// Check for the SDF filename or XML string or topic name
if (!file_name.empty()) {
req.set_sdf_filename(file_name);
} else if (!xml_string.empty()) {
req.set_sdf(xml_string);
} else if (!topic_name.empty()) {
// set XML string by fetching it from the given topic
if (!set_XML_from_topic(topic_name, ros2_node, req)) {
return -1;
}
} else if (!FLAGS_string.empty()) { // string
req.set_sdf(FLAGS_string);
} else if (!FLAGS_topic.empty()) { // topic
const auto timeout = std::chrono::seconds(1);
std::promise<std::string> xml_promise;
std::shared_future<std::string> xml_future(xml_promise.get_future());

std::function<void(const std_msgs::msg::String::SharedPtr)> fun =
[&xml_promise](const std_msgs::msg::String::SharedPtr msg) {
xml_promise.set_value(msg->data);
};

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(ros2_node);
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr description_subs;
// Transient local is similar to latching in ROS 1.
description_subs = ros2_node->create_subscription<std_msgs::msg::String>(
FLAGS_topic, rclcpp::QoS(1).transient_local(), fun);

rclcpp::FutureReturnCode future_ret;
do {
RCLCPP_INFO(ros2_node->get_logger(), "Waiting messages on topic [%s].", FLAGS_topic.c_str());
future_ret = executor.spin_until_future_complete(xml_future, timeout);
} while (rclcpp::ok() && future_ret != rclcpp::FutureReturnCode::SUCCESS);

if (future_ret == rclcpp::FutureReturnCode::SUCCESS) {
req.set_sdf(xml_future.get());
} else if (filtered_arguments.size() > 1) {
// Revert to Gflags, if ROS parameters aren't specified
// File
if (!FLAGS_file.empty()) {
req.set_sdf_filename(FLAGS_file);
} else if (!FLAGS_param.empty()) { // Param
ros2_node->declare_parameter<std::string>(FLAGS_param);
std::string xmlStr;
if (ros2_node->get_parameter(FLAGS_param, xmlStr)) {
req.set_sdf(xmlStr);
} else {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from param [%s].", FLAGS_param.c_str());
return -1;
}
} else if (!FLAGS_string.empty()) { // string
req.set_sdf(FLAGS_string);
} else if (!FLAGS_topic.empty()) { // topic
// set XML string by fetching it from the given topic
if (!set_XML_from_topic(FLAGS_topic, ros2_node, req)) {
return -1;
}
} else {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from topic [%s].", FLAGS_topic.c_str());
ros2_node->get_logger(), "Must specify either -file, -param, -string or -topic");
return -1;
}
RCLCPP_WARN(
ros2_node->get_logger(),
"Usage of Commandline flags for spawning entities is deprecated. Please use ROS 2 parameters."
);
} else {
RCLCPP_ERROR(ros2_node->get_logger(), "Must specify either -file, -param, -stdin or -topic");
RCLCPP_ERROR(
ros2_node->get_logger(), "Must specify either file, string or topic as ROS 2 parameters");
return -1;
}

// Pose
double x_coords = ros2_node->get_parameter("x").as_double();
double y_coords = ros2_node->get_parameter("y").as_double();
double z_coords = ros2_node->get_parameter("z").as_double();
double roll = ros2_node->get_parameter("R").as_double();
double pitch = ros2_node->get_parameter("P").as_double();
double yaw = ros2_node->get_parameter("Y").as_double();

FLAGS_x = (x_coords != 0.0) ? x_coords : FLAGS_x;
FLAGS_y = (y_coords != 0.0) ? y_coords : FLAGS_y;
FLAGS_z = (z_coords != 0.0) ? z_coords : FLAGS_z;
FLAGS_R = (roll != 0.0) ? roll : FLAGS_R;
FLAGS_P = (pitch != 0.0) ? pitch : FLAGS_P;
FLAGS_Y = (yaw != 0.0) ? yaw : FLAGS_Y;
gz::math::Pose3d pose{FLAGS_x, FLAGS_y, FLAGS_z, FLAGS_R, FLAGS_P, FLAGS_Y};
gz::msgs::Set(req.mutable_pose(), pose);

// Name
if (!FLAGS_name.empty()) {
std::string entity_name = ros2_node->get_parameter("name").as_string();
if (!entity_name.empty()) {
req.set_name(entity_name);
} else {
req.set_name(FLAGS_name);
}

if (FLAGS_allow_renaming) {
req.set_allow_renaming(FLAGS_allow_renaming);
}
// Allow Renaming
bool allow_renaming = ros2_node->get_parameter("allow_renaming").as_bool();
req.set_allow_renaming((allow_renaming || FLAGS_allow_renaming));

// Request
gz::transport::Node node;
Expand Down
Loading