From 4f3fe9cf9e431133f3f58cd4ecc88b6b07fa544a Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 10:34:27 +0200 Subject: [PATCH 1/7] Remove messages from moveit_kinematics CMakelist --- moveit_kinematics/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 784385d7c2..2a0aaaa988 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(class_loader REQUIRED) find_package(pluginlib REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) -find_package(moveit_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DIRS kdl_kinematics_plugin/include From b3f2e1302b05c83f47fd1f8f471835170ec5673e Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 10:34:55 +0200 Subject: [PATCH 2/7] Add moveit_core to kd_kinematics_plugin From 18e4e87969921e888ac85eb758e45a7326ee9710 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 17:09:14 +0200 Subject: [PATCH 3/7] Update moveit_kinematics cmakelist to start porting lma_kinematics --- moveit_kinematics/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 2a0aaaa988..784385d7c2 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -15,6 +15,7 @@ find_package(class_loader REQUIRED) find_package(pluginlib REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_ros_planning REQUIRED) +find_package(moveit_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DIRS kdl_kinematics_plugin/include From 8fcf6536dfbc7f5d24fe01f8d8d5c4c3f425fd6a Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 17:09:27 +0200 Subject: [PATCH 4/7] Update lma_kinematics cmakelist From 40d8e245bb12ac8c692338cda394c814b7656d01 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 18:24:39 +0200 Subject: [PATCH 5/7] Port lma_kinematics --- .../lma_kinematics_plugin/src/lma_kinematics_plugin.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp index 1f57640317..924d88393c 100644 --- a/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp +++ b/moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp @@ -144,7 +144,7 @@ bool LMAKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model bool LMAKinematicsPlugin::timedOut(const std::chrono::system_clock::time_point& start_time, double duration) const { - return (std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() *1e-9 >= duration); + return (std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count() /1000.0 >= duration); } bool LMAKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, From 2c3cf04940d1e1d41dc4335dc633a69e323c61e1 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Thu, 16 May 2019 17:09:14 +0200 Subject: [PATCH 6/7] Update moveit_kinematics cmakelist to start porting lma_kinematics From abf3d6c2823fc3286cdc7a895bafed91956212e3 Mon Sep 17 00:00:00 2001 From: Anasarrak Date: Fri, 17 May 2019 11:41:50 +0200 Subject: [PATCH 7/7] Port srv_kinematics_plugin --- moveit_kinematics/CMakeLists.txt | 4 +- .../srv_kinematics_plugin/CMakeLists.txt | 6 +- .../srv_kinematics_plugin.h | 37 ++--- .../src/srv_kinematics_plugin.cpp | 139 +++++++++--------- 4 files changed, 94 insertions(+), 92 deletions(-) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 784385d7c2..0c6dc330ff 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -20,7 +20,7 @@ find_package(moveit_msgs REQUIRED) set(THIS_PACKAGE_INCLUDE_DIRS kdl_kinematics_plugin/include lma_kinematics_plugin/include - # srv_kinematics_plugin/include + srv_kinematics_plugin/include # cached_ik_kinematics_plugin/include ) @@ -40,7 +40,7 @@ include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} add_subdirectory(kdl_kinematics_plugin) add_subdirectory(lma_kinematics_plugin) -# add_subdirectory(srv_kinematics_plugin) +add_subdirectory(srv_kinematics_plugin) # add_subdirectory(ikfast_kinematics_plugin) # add_subdirectory(cached_ik_kinematics_plugin) add_subdirectory(test) diff --git a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt index 1d9cd5d228..b94f9f303b 100644 --- a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt @@ -3,7 +3,7 @@ set(MOVEIT_LIB_NAME moveit_srv_kinematics_plugin) add_library(${MOVEIT_LIB_NAME} SHARED src/srv_kinematics_plugin.cpp) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(${MOVEIT_LIB_NAME} ${catkin_LIBRARIES}) +target_link_libraries(${MOVEIT_LIB_NAME} ${rclcpp_LIBRARIES}) -install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) +install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION lib) +install(DIRECTORY include/ DESTINATION include) 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 83429df7cc..3379236f51 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 @@ -42,18 +42,18 @@ #ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ #define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_ -// ROS -#include +// ROS2 +#include "rclcpp/rclcpp.hpp" // System #include // ROS msgs -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include // MoveIt! #include @@ -74,34 +74,34 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase SrvKinematicsPlugin(); bool getPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool searchPositionIK( - const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const override; bool getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const override; + std::vector& poses) const override; bool initialize(const moveit::core::RobotModel& robot_model, const std::string& group_name, const std::string& base_name, const std::vector& tip_frames, @@ -124,13 +124,13 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase protected: virtual bool - searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, + searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const std::vector& consistency_limits, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; virtual bool - searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, + searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const; @@ -138,7 +138,7 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase bool setRedundantJoints(const std::vector& redundant_joint_indices) override; private: - bool timedOut(const ros::WallTime& start_time, double duration) const; + bool timedOut(const std::chrono::system_clock::time_point& start_time, double duration) const; int getJointIndex(const std::string& name) const; @@ -156,7 +156,10 @@ class SrvKinematicsPlugin : public kinematics::KinematicsBase int num_possible_redundant_joints_; - std::shared_ptr ik_service_client_; + rclcpp::Client::SharedPtr ik_service_client_; + + rclcpp::Node::SharedPtr node_; + }; } 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 8798e55d54..9738cda453 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -37,6 +37,7 @@ #include #include #include +#include // Eigen #include @@ -57,7 +58,7 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model { bool debug = false; - ROS_INFO_STREAM_NAMED("srv", "SrvKinematicsPlugin initializing"); + RCLCPP_INFO(node_->get_logger(), "SrvKinematicsPlugin initializing"); storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization); joint_model_group_ = robot_model_->getJointModelGroup(group_name); @@ -74,10 +75,8 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model // Get the dimension of the planning group dimension_ = joint_model_group_->getVariableCount(); - ROS_INFO_STREAM_NAMED("srv", "Dimension planning group '" - << group_name << "': " << dimension_ - << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size() - << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size()); + RCLCPP_INFO(node_->get_logger(), "Dimension planning group '%s': %d . Active Joints Models: %d . Mimic Joint Models: %d", + group_name, dimension_, joint_model_group_->getActiveJointModels().size(), joint_model_group_->getMimicJointModels().size()); // Copy joint names for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i) @@ -87,7 +86,7 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model if (debug) { - ROS_ERROR_STREAM_NAMED("srv", "tip links available:"); + RCLCPP_ERROR(node_->get_logger(), "tip links available:"); std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator(std::cout, "\n")); } @@ -96,7 +95,7 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model { if (!joint_model_group_->hasLinkModel(tip_frames_[i])) { - ROS_ERROR_NAMED("srv", "Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), + RCLCPP_ERROR(node_->get_logger(), "Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(), group_name.c_str()); return false; } @@ -104,27 +103,27 @@ bool SrvKinematicsPlugin::initialize(const moveit::core::RobotModel& robot_model } // Choose what ROS service to send IK requests to - ROS_DEBUG_STREAM_NAMED("srv", "Looking for ROS service name on rosparam server with param: " - << "/kinematics_solver_service_name"); + RCLCPP_DEBUG(node_->get_logger(), "Looking for ROS service name on rosparam server with param: " + "/kinematics_solver_service_name"); std::string ik_service_name; - lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik")); + lookupParam(node_,"kinematics_solver_service_name", ik_service_name, std::string("solve_ik")); // Setup the joint state groups that we need robot_state_.reset(new robot_state::RobotState(robot_model_)); robot_state_->setToDefaultValues(); - // Create the ROS service client - ros::NodeHandle nonprivate_handle(""); - ik_service_client_ = std::make_shared( - nonprivate_handle.serviceClient(ik_service_name)); - if (!ik_service_client_->waitForExistence(ros::Duration(0.1))) // wait 0.1 seconds, blocking - ROS_WARN_STREAM_NAMED("srv", - "Unable to connect to ROS service client with name: " << ik_service_client_->getService()); + // Create the ROS2 service client + ik_service_client_ = + node_->create_client(ik_service_name); + + if (!ik_service_client_->wait_for_service(std::chrono::seconds(1))) // wait 0.1 seconds, blocking + RCLCPP_WARN(node_->get_logger(), + "Unable to connect to ROS service client with name: %s", ik_service_client_->get_service_name()); else - ROS_INFO_STREAM_NAMED("srv", "Service client started with ROS service name: " << ik_service_client_->getService()); + RCLCPP_INFO(node_->get_logger(), "Service client started with ROS service name: %s", ik_service_client_->get_service_name()); active_ = true; - ROS_DEBUG_NAMED("srv", "ROS service-based kinematics solver initialized"); + RCLCPP_DEBUG(node_->get_logger(), "ROS service-based kinematics solver initialized"); return true; } @@ -132,12 +131,12 @@ bool SrvKinematicsPlugin::setRedundantJoints(const std::vector& re { if (num_possible_redundant_joints_ < 0) { - ROS_ERROR_NAMED("srv", "This group cannot have redundant joints"); + RCLCPP_ERROR(node_->get_logger(), "This group cannot have redundant joints"); return false; } if (int(redundant_joints.size()) > num_possible_redundant_joints_) { - ROS_ERROR_NAMED("srv", "This group can only have %d redundant joints", num_possible_redundant_joints_); + RCLCPP_ERROR(node_->get_logger(), "This group can only have %d redundant joints", num_possible_redundant_joints_); return false; } @@ -162,12 +161,12 @@ int SrvKinematicsPlugin::getJointIndex(const std::string& name) const return -1; } -bool SrvKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const +bool SrvKinematicsPlugin::timedOut(const std::chrono::system_clock::time_point& start_time, double duration) const { - return ((ros::WallTime::now() - start_time).toSec() >= duration); + return (std::chrono::duration_cast(std::chrono::system_clock::now() - start_time).count()*1e-9 >= duration); } -bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const { @@ -177,7 +176,7 @@ bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, cons consistency_limits, options); } -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const @@ -188,7 +187,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c options); } -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, moveit_msgs::msg::MoveItErrorCodes& error_code, const kinematics::KinematicsQueryOptions& options) const @@ -197,7 +196,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c options); } -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, @@ -208,7 +207,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c options); } -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, @@ -218,7 +217,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c options); } -bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector& ik_seed_state, +bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose, const std::vector& ik_seed_state, double timeout, std::vector& solution, const IKCallbackFn& solution_callback, moveit_msgs::msg::MoveItErrorCodes& error_code, @@ -226,14 +225,14 @@ bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, c const kinematics::KinematicsQueryOptions& options) const { // Convert single pose into a vector of one pose - std::vector ik_poses; + std::vector ik_poses; ik_poses.push_back(ik_pose); return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options); } -bool SrvKinematicsPlugin::searchPositionIK(const std::vector& ik_poses, +bool SrvKinematicsPlugin::searchPositionIK(const std::vector& ik_poses, const std::vector& ik_seed_state, double timeout, const std::vector& consistency_limits, std::vector& solution, const IKCallbackFn& solution_callback, @@ -243,7 +242,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_logger(), "kinematics not active"); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -251,8 +250,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_logger(), "Seed state must have size %d instead of size %d",dimension_, ik_seed_state.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } @@ -260,24 +258,22 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_logger(), "Mismatched number of pose requests (%d) to tip frames (%d) in searchPositionIK",ik_poses.size(),tip_frames_.size()); error_code.val = error_code.NO_IK_SOLUTION; return false; } // Create the service message - moveit_msgs::srv::GetPositionIK ik_srv; - ik_srv.request.ik_request.avoid_collisions = true; - ik_srv.request.ik_request.group_name = getGroupName(); + auto ik_srv = std::make_shared(); + ik_srv->ik_request.avoid_collisions = true; + ik_srv->ik_request.group_name = getGroupName(); // Copy seed state into virtual robot state and convert into moveit_msg robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state); - moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state); + moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv->ik_request.robot_state); // Load the poses into the request in difference places depending if there is more than one or not - geometry_msgs::PoseStamped ik_pose_st; + geometry_msgs::msg::PoseStamped ik_pose_st; ik_pose_st.header.frame_id = base_frame_; if (tip_frames_.size() > 1) { @@ -285,8 +281,8 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorik_request.pose_stamped_vector.push_back(ik_pose_st); + ik_srv->ik_request.ik_link_names.push_back(tip_frames_[i]); } } else @@ -294,46 +290,49 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorik_request.pose_stamped = ik_pose_st; + ik_srv->ik_request.ik_link_name = getTipFrames()[0]; } - ROS_DEBUG_STREAM_NAMED("srv", "Calling service: " << ik_service_client_->getService()); - if (ik_service_client_->call(ik_srv)) - { + RCLCPP_DEBUG(node_->get_logger(), "Calling service: %s", ik_service_client_->get_service_name()); + auto result_future = ik_service_client_->async_send_request(ik_srv); + auto response = result_future.get(); + if (rclcpp::spin_until_future_complete(node_, result_future) == + rclcpp::executor::FutureReturnCode::SUCCESS) + { // Check error code - error_code.val = ik_srv.response.error_code.val; + error_code.val = response->error_code.val; if (error_code.val != error_code.SUCCESS) { - ROS_DEBUG_STREAM_NAMED("srv", "An IK that satisifes the constraints and is collision free could not be found." - << "\nRequest was: \n" - << ik_srv.request.ik_request << "\nResponse was: \n" - << ik_srv.response.solution); + //TODO (anasarrak) Print the entire message for ROS2? + // RCLCPP_DEBUG("srv", "An IK that satisifes the constraints and is collision free could not be found." + // << "\nRequest was: \n" + // << ik_srv.request.ik_request << "\nResponse was: \n" + // << ik_srv.response.solution); switch (error_code.val) { case moveit_msgs::msg::MoveItErrorCodes::FAILURE: - ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: FAILURE"); + RCLCPP_ERROR(node_->get_logger(), "Service failed with with error code: FAILURE"); break; case moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION: - ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: NO IK SOLUTION"); + RCLCPP_DEBUG(node_->get_logger(), "Service failed with with error code: NO IK SOLUTION"); break; default: - ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: " << error_code.val); + RCLCPP_DEBUG(node_->get_logger(), "Service failed with with error code: %d", error_code.val); } return false; } } else { - ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService()); + RCLCPP_DEBUG(node_->get_logger(),"Service call failed to connect to service: %s", ik_service_client_->get_service_name()); error_code.val = error_code.FAILURE; return false; } - // Convert the robot state message to our robot_state representation - if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_)) + if (!moveit::core::robotStateMsgToRobotState(response->solution, *robot_state_)) { - ROS_ERROR_STREAM_NAMED("srv", + RCLCPP_ERROR(node_->get_logger(), "An error occured converting received robot state message into internal robot state."); error_code.val = error_code.FAILURE; return false; @@ -345,7 +344,7 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_logger(), "Calling solution callback on IK solution"); // hack: should use all poses, not just the 0th solution_callback(ik_poses[0], solution, error_code); @@ -355,40 +354,40 @@ bool SrvKinematicsPlugin::searchPositionIK(const std::vectorget_logger(), "IK solution callback failed with with error code: FAILURE"); break; case moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION: - ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: " + RCLCPP_ERROR(node_->get_logger(), "IK solution callback failed with with error code: " "NO IK SOLUTION"); break; default: - ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: " << error_code.val); + RCLCPP_ERROR(node_->get_logger(), "IK solution callback failed with with error code: %d", error_code.val); } return false; } } - ROS_INFO_STREAM_NAMED("srv", "IK Solver Succeeded!"); + RCLCPP_INFO(node_->get_logger(), "IK Solver Succeeded!"); return true; } bool SrvKinematicsPlugin::getPositionFK(const std::vector& link_names, const std::vector& joint_angles, - std::vector& poses) const + std::vector& poses) const { if (!active_) { - ROS_ERROR_NAMED("srv", "kinematics not active"); + RCLCPP_ERROR(node_->get_logger(), "kinematics not active"); return false; } poses.resize(link_names.size()); if (joint_angles.size() != dimension_) { - ROS_ERROR_NAMED("srv", "Joint angles vector must have size: %d", dimension_); + RCLCPP_ERROR(node_->get_logger(), "Joint angles vector must have size: %d", dimension_); return false; } - ROS_ERROR_STREAM_NAMED("srv", "Forward kinematics not implemented"); + RCLCPP_ERROR(node_->get_logger(), "Forward kinematics not implemented"); return false; }