Skip to content

Commit

Permalink
changed parameter naming
Browse files Browse the repository at this point in the history
  • Loading branch information
yaswanth1701 committed Nov 17, 2023
1 parent eefa9fe commit 3027670
Showing 1 changed file with 35 additions and 36 deletions.
71 changes: 35 additions & 36 deletions ros_gz_sim/src/create.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,15 +35,14 @@
// If these are not needed, just use the `gz service` command line instead.
int main(int _argc, char ** _argv)
{
rclcpp::init(_argc, _argv);
rclcpp::init(_argc, _argv);
auto ros2_node = rclcpp::Node::make_shared("ros_gz_sim");

std::string world_,file_,param_,string_,topic_,name_;
bool allow_renaming_;
double x_,y_,z_,R_,P_,Y_;
std::string world_prm,file_prm,param_prm,string_prm,topic_prm,name_prm;
bool allow_renaming_prm;
double x_prm,y_prm,z_prm,R_prm,P_prm,Y_prm;

// declaring ros param

ros2_node->declare_parameter("world","");
ros2_node->declare_parameter("file","");
ros2_node->declare_parameter("param","");
Expand All @@ -59,22 +58,22 @@ int main(int _argc, char ** _argv)
ros2_node->declare_parameter("Y", 0.0);

// getting value of ros param (from param server or command line)
ros2_node->get_parameter("world", world_);
ros2_node->get_parameter("file", file_);
ros2_node->get_parameter("param", param_);
ros2_node->get_parameter("string", string_);
ros2_node->get_parameter("topic", topic_);
ros2_node->get_parameter("name", name_);
ros2_node->get_parameter("allow_renaming", allow_renaming_);
ros2_node->get_parameter("x", x_);
ros2_node->get_parameter("y", y_);
ros2_node->get_parameter("z", z_);
ros2_node->get_parameter("R", R_);
ros2_node->get_parameter("P", P_);
ros2_node->get_parameter("Y", Y_);
ros2_node->get_parameter("world", world_prm);
ros2_node->get_parameter("file", file_prm);
ros2_node->get_parameter("param", param_prm);
ros2_node->get_parameter("string", string_prm);
ros2_node->get_parameter("topic", topic_prm);
ros2_node->get_parameter("name", name_prm);
ros2_node->get_parameter("allow_renaming", allow_renaming_prm);
ros2_node->get_parameter("x", x_prm);
ros2_node->get_parameter("y", y_prm);
ros2_node->get_parameter("z", z_prm);
ros2_node->get_parameter("R", R_prm);
ros2_node->get_parameter("P", P_prm);
ros2_node->get_parameter("Y", Y_prm);

// World
std::string world_name = world_;
std::string world_name = world_prm;
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 @@ -109,22 +108,22 @@ int main(int _argc, char ** _argv)
gz::msgs::EntityFactory req;

// File
if (!file_.empty()) {
req.set_sdf_filename(file_);
} else if (!param_.empty()) { // Param
ros2_node->declare_parameter<std::string>(param_);
if (!file_prm.empty()) {
req.set_sdf_filename(file_prm);
} else if (!param_prm.empty()) { // Param
ros2_node->declare_parameter<std::string>(param_prm);

std::string xmlStr;
if (ros2_node->get_parameter(param_, xmlStr)) {
if (ros2_node->get_parameter(param_prm, xmlStr)) {
req.set_sdf(xmlStr);
} else {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from param [%s].", param_.c_str());
ros2_node->get_logger(), "Failed to get XML from param [%s].", param_prm.c_str());
return -1;
}
} else if (!string_.empty()) { // string
req.set_sdf(string_);
} else if (!topic_.empty()) { // topic
} else if (!string_prm.empty()) { // string
req.set_sdf(string_prm);
} else if (!topic_prm.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());
Expand All @@ -139,19 +138,19 @@ int main(int _argc, char ** _argv)
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_, rclcpp::QoS(1).transient_local(), fun);
topic_prm, rclcpp::QoS(1).transient_local(), fun);

rclcpp::FutureReturnCode future_ret;
do {
RCLCPP_INFO(ros2_node->get_logger(), "Waiting messages on topic [%s].", topic_.c_str());
RCLCPP_INFO(ros2_node->get_logger(), "Waiting messages on topic [%s].", topic_prm.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 {
RCLCPP_ERROR(
ros2_node->get_logger(), "Failed to get XML from topic [%s].", topic_.c_str());
ros2_node->get_logger(), "Failed to get XML from topic [%s].", topic_prm.c_str());
return -1;
}
} else {
Expand All @@ -160,16 +159,16 @@ int main(int _argc, char ** _argv)
}

// Pose
gz::math::Pose3d pose{x_, y_, z_, R_, P_, Y_};
gz::math::Pose3d pose{x_prm, y_prm, z_prm, R_prm, P_prm, Y_prm};
gz::msgs::Set(req.mutable_pose(), pose);

// Name
if (!name_.empty()) {
req.set_name(name_);
if (!name_prm.empty()) {
req.set_name(name_prm);
}

if (allow_renaming_) {
req.set_allow_renaming(allow_renaming_);
if (allow_renaming_prm) {
req.set_allow_renaming(allow_renaming_prm);
}

// Request
Expand Down

0 comments on commit 3027670

Please sign in to comment.