From 6c6f024f12ee80997e2d7906b46aed7f2b4c79f4 Mon Sep 17 00:00:00 2001 From: Abishalini Date: Sun, 30 Oct 2022 23:15:01 -0600 Subject: [PATCH] Remove use of lookupParam function --- .../srv_kinematics_plugin/srv_kinematics_plugin.h | 3 +++ .../src/srv_kinematics_plugin.cpp | 14 +++++++------- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h index 491dd2298c..2283bb510b 100644 --- a/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h +++ b/moveit_kinematics/srv_kinematics_plugin/include/moveit/srv_kinematics_plugin/srv_kinematics_plugin.h @@ -152,5 +152,8 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase rclcpp::Client::SharedPtr ik_service_client_; rclcpp::Node::SharedPtr node_; + + std::shared_ptr param_listener_; + srv_kinematics::Params params_; }; } // namespace srv_kinematics_plugin diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index 1ea85b1355..20a195ca53 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -64,6 +64,11 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const RCLCPP_INFO(LOGGER, "SrvKinematicsPlugin initializing"); + // Get Solver Parameters + std::string kinematics_param_prefix = "robot_description_kinematics." + group_name; + param_listener_ = std::make_shared(node, kinematics_param_prefix); + params_ = param_listener_->get_params(); + storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); joint_model_group_ = robot_model_->getJointModelGroup(group_name); if (!joint_model_group_) @@ -107,18 +112,13 @@ bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const ik_group_info_.link_names.push_back(tip_frame); } - // Choose what ROS service to send IK requests to - RCLCPP_DEBUG(LOGGER, "Looking for ROS service name on rosparam server with param: " - "/kinematics_solver_service_name"); - std::string ik_service_name; - lookupParam(node_, "kinematics_solver_service_name", ik_service_name, std::string("solve_ik")); - // Setup the joint state groups that we need robot_state_ = std::make_shared(robot_model_); robot_state_->setToDefaultValues(); // Create the ROS2 service client - ik_service_client_ = node_->create_client(ik_service_name); + RCLCPP_DEBUG(LOGGER, "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str()); + ik_service_client_ = node_->create_client(params_.kinematics_solver_service_name); if (!ik_service_client_->wait_for_service(std::chrono::seconds(1))) // wait 0.1 seconds, blocking RCLCPP_WARN_STREAM(LOGGER,