diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index 0ff00904..c16c01d7 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -7,14 +7,15 @@ if(${pybind11_FOUND}) message(STATUS "compiling python bindings") + # get python install path + message(STATUS "Found python (${Python3_EXECUTABLE}) will compile python bindings") + # get python install path execute_process( - COMMAND ${PYTHON_EXECUTABLE} -c "import sys; version = sys.version_info[:3]; print('{0}.{1}'.format(*version))" - OUTPUT_VARIABLE PYTHON_VERSION_MIN + COMMAND ${Python3_EXECUTABLE} -c "import site, os; print(os.path.relpath(site.USER_SITE, site.USER_BASE))" + OUTPUT_VARIABLE PYTHON_SITE OUTPUT_STRIP_TRAILING_WHITESPACE) - set(PYTHON_SITE "lib/python${PYTHON_VERSION_MIN}/site-packages") - message(STATUS "Python install dir: ${PYTHON_SITE}") include_directories(${CMAKE_CURRENT_SOURCE_DIR}) @@ -22,34 +23,34 @@ if(${pybind11_FOUND}) target_link_libraries(pyest PUBLIC estimation_utils::payload_estimation) install(TARGETS pyest - DESTINATION ${PYTHON_SITE}) - -# include_directories(${CMAKE_CURRENT_SOURCE_DIR}) -# pybind11_add_module(pyci pyRosImpl.cpp) -# target_link_libraries(pyci PRIVATE CartesianInterface) + DESTINATION ${PYTHON_SITE}/cartesian_interface) -# install(TARGETS pyci -# DESTINATION ${PYTHON_SITE}) - -# pybind11_add_module(roscpp_utils pyRosInit.cpp) -# target_link_libraries(roscpp_utils PRIVATE ${catkin_LIBRARIES}) - -# install(TARGETS roscpp_utils -# DESTINATION ${PYTHON_SITE}) + include_directories(${CMAKE_CURRENT_SOURCE_DIR}) + pybind11_add_module(pyci pyRosImpl.cpp) + target_link_libraries(pyci PRIVATE CartesianInterface CartesianInterfaceRos) + + install(TARGETS pyci + DESTINATION ${PYTHON_SITE}/cartesian_interface) + pybind11_add_module(roscpp_utils pyRosInit.cpp) + #target_link_libraries(roscpp_utils PRIVATE ${catkin_LIBRARIES}) + target_link_libraries(roscpp_utils PRIVATE rclcpp::rclcpp) + + install(TARGETS roscpp_utils + DESTINATION ${PYTHON_SITE}/cartesian_interface) pybind11_add_module(impedance pyImpedance.cpp) target_link_libraries(impedance PRIVATE CartesianInterfaceCore) install(TARGETS impedance - DESTINATION ${PYTHON_SITE}) + DESTINATION ${PYTHON_SITE}/cartesian_interface) install(FILES pyci_all.py - DESTINATION ${PYTHON_SITE}) + DESTINATION ${PYTHON_SITE}/cartesian_interface) install(PROGRAMS interactive_client.py - DESTINATION ${PYTHON_SITE}) + DESTINATION ${PYTHON_SITE}/cartesian_interface) else() message(STATUS "pybind not found") diff --git a/bindings/python/pyRosImpl.cpp b/bindings/python/pyRosImpl.cpp index afc3c453..e0a6099f 100644 --- a/bindings/python/pyRosImpl.cpp +++ b/bindings/python/pyRosImpl.cpp @@ -14,7 +14,7 @@ #include "problem/Com.h" #include "../../examples/plugin/angular_mom/src/AngularMomentum.h" -#include +#include PYBIND11_MODULE(pyci, m) { @@ -138,11 +138,11 @@ PYBIND11_MODULE(pyci, m) { py::class_(m, "PosturalRos", py::multiple_inheritance()) + ClientApi::PosturalRos::Ptr>(m, "PosturalRos", py::multiple_inheritance()); - py::class_(m, "AngularMomentumRos", py::multiple_inheritance()); +// py::class_(m, "AngularMomentumRos", py::multiple_inheritance()); py::class_(m, "PosturalTaskImpl", py::multiple_inheritance()); - py::class_(m, "AngularMomentumImpl", py::multiple_inheritance()); +// py::class_(m, "AngularMomentumImpl", py::multiple_inheritance()); py::class_(m, "CartesianInterface") @@ -235,14 +235,14 @@ PYBIND11_MODULE(pyci, m) { /* Robot state pub util */ - py::class_(m, "RobotStatePublisher") - .def(py::init()) - .def("publishTransforms", - [](Utils::RobotStatePublisher& r, std::string pref) - { - r.publishTransforms(ros::Time::now(), pref); - }) - ; +// py::class_(m, "RobotStatePublisher") +// .def(py::init()) +// .def("publishTransforms", +// [](Utils::RobotStatePublisher& r, std::string pref) +// { +// r.publishTransforms(ros::Time::now(), pref); +// }) +// ; } diff --git a/bindings/python/pyRosInit.cpp b/bindings/python/pyRosInit.cpp index ee26ff3e..3254bb14 100644 --- a/bindings/python/pyRosInit.cpp +++ b/bindings/python/pyRosInit.cpp @@ -1,14 +1,16 @@ #include #include -#include +#include #include bool init(std::string name, std::list args) { - if(ros::ok()) + if(rclcpp::ok()) { - ROS_ERROR("Ros node already initialized with name %s", - ros::this_node::getName().c_str()); + //ROS_ERROR("Ros node already initialized with name %s", + // ros::this_node::getName().c_str()); + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Ros node already initialized"); + return false; } @@ -24,11 +26,13 @@ bool init(std::string name, std::list args) name += "_cpp"; - ros::init(argc, argv, name, ros::init_options::NoSigintHandler); - - ROS_INFO("Initialized roscpp under namespace %s with name %s", - ros::this_node::getNamespace().c_str(), - ros::this_node::getName().c_str() + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared(name); + + RCLCPP_INFO(node->get_logger(), + "Initialized roscpp under namespace %s with name %s", + node->get_namespace(), + node->get_name() ); return true; @@ -36,10 +40,11 @@ bool init(std::string name, std::list args) bool shutdown() { - if(ros::ok()) + if(rclcpp::ok()) { - ROS_INFO("Shutting down ros node"); - ros::shutdown(); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutting down ros node"); + + rclcpp::shutdown(); return true; } diff --git a/include/cartesian_interface/ros/RosClient.h b/include/cartesian_interface/ros/RosClient.h index 106571e0..fab126b3 100644 --- a/include/cartesian_interface/ros/RosClient.h +++ b/include/cartesian_interface/ros/RosClient.h @@ -4,11 +4,13 @@ #include #include -#include -#include +#include +#include +#include #include +#include namespace XBot { namespace Cartesian { @@ -16,13 +18,14 @@ struct RosInitializer { RosInitializer(std::string ns); - ros::NodeHandle& nh(); + rclcpp::Node::SharedPtr node(); void callAvailable(); private: - ros::CallbackQueue _queue; - std::unique_ptr _nh; + rclcpp::CallbackGroup::SharedPtr _ros2_cbg; + rclcpp::Node::SharedPtr _node; + std::shared_ptr _exec; }; class RosClient : public CartesianInterfaceImpl, @@ -77,12 +80,13 @@ class RosClient : public CartesianInterfaceImpl, const std::string& target_frame, Eigen::Affine3d& t_T_s); + std::string _ns; private: - - tf::TransformListener _listener; - ros::ServiceClient _load_ctrl_srv; - + + std::shared_ptr _listener; + std::unique_ptr _tf_buffer; + rclcpp::Client::SharedPtr _load_ctrl_srv; }; diff --git a/include/cartesian_interface/ros/RosExecutor.h.orig b/include/cartesian_interface/ros/RosExecutor.h.orig deleted file mode 100644 index 9963b609..00000000 --- a/include/cartesian_interface/ros/RosExecutor.h.orig +++ /dev/null @@ -1,170 +0,0 @@ -#ifndef __XBOT_CARTESIAN_ROS_EXECUTOR_H__ -#define __XBOT_CARTESIAN_ROS_EXECUTOR_H__ - -#include -#include - -<<<<<<< HEAD -#include - -#include -#include -#include -#include -======= -#include -#include -#include - ->>>>>>> b292c1da7e602733408ce5678a5eafdeafcde447 -#include -#include -#include -#include - -#include -#include - -namespace XBot { -namespace Cartesian { - -using namespace cartesian_interface_ros::msg; -using namespace cartesian_interface_ros::srv; -using namespace geometry_msgs::msg; -using namespace sensor_msgs::msg; -using namespace std_srvs::srv; -using namespace std_msgs::msg; - -class RosExecutor -{ - -public: - -<<<<<<< HEAD - bool reset_joints_callback(cartesian_interface::ResetJointsRequest& req, - cartesian_interface::ResetJointsResponse& res); - - bool pause_cartesio_callback(std_srvs::SetBoolRequest& req, - std_srvs::SetBoolRequest& res); - - void timer_callback(const ros::TimerEvent& timer_ev); -======= - RosExecutor(std::string ns = "cartesian"); ->>>>>>> b292c1da7e602733408ce5678a5eafdeafcde447 - - void spin(); - -<<<<<<< HEAD - ros::CallbackQueue _fb_queue; - ros::NodeHandle _nh, _nh_priv; - - XBot::ConfigOptions _xbot_cfg, _xbot_cfg_robot; - - RobotInterface::Ptr _robot; - ros::Publisher _fb_pub; - bool _visual_mode; - ModelInterface::Ptr _model; - - Eigen::VectorXd _q, _qdot, _qddot, _tau, _tau_offset; - - std::map _impl_map; - std::vector _zombies; - CartesianInterfaceImpl::Ptr _current_impl; - - RosServerClass::Ptr _ros_api; - - ros::Publisher _ctrl_changed_pub; - ros::ServiceServer _loader_srv; - ros::ServiceServer _reset_srv; - ros::ServiceServer _reset_joints_srv; - ros::ServiceServer _pause_ci_srv; - ros::Subscriber _fb_sub; - - ros::Timer _loop_timer; - double _time, _period; - - MatLogger2::Ptr _logger; - - bool _pause_command; - - }; -======= - void spin_once(); ->>>>>>> b292c1da7e602733408ce5678a5eafdeafcde447 - - CartesianInterfaceImpl &solver(); - - const ModelInterface &model(); - - ~RosExecutor(); - -private: - - void init_ros(); - void init_load_config(); - void init_load_robot(); - void init_customize_command(); - void init_load_model(); - void init_load_torque_offset(); - void init_load_world_frame(); - void init_create_loop_timer(); - - CartesianInterfaceImpl::Ptr load_controller(std::string impl_name, - ProblemDescription ik_problem); - void reset_model_state(); - void load_ros_api(); - - void floating_base_pose_callback(PoseStamped::ConstSharedPtr msg); - - bool loader_callback(LoadController::Request::ConstSharedPtr req, - LoadController::Response::SharedPtr res); - - bool reset_callback(Trigger::Request::ConstSharedPtr req, - Trigger::Response::SharedPtr res); - - bool reset_joints_callback(ResetJoints::Request::ConstSharedPtr req, - ResetJoints::Response::SharedPtr res); - - void timer_callback(); - - void publish_fb_cmd_vel(); - - Context::Ptr _ctx; - - rclcpp::Node::SharedPtr _node, _prnode, _cnode; - - Utils::LoadFrom _options_source; - - XBot::ConfigOptions _xbot_cfg, _xbot_cfg_robot; - - RobotInterface::Ptr _robot; - rclcpp::Publisher::SharedPtr _fb_pub; - bool _visual_mode; - ModelInterface::Ptr _model; - - Eigen::VectorXd _q, _qdot, _qddot, _tau, _tau_offset; - - std::map _impl_map; - std::vector _zombies; - CartesianInterfaceImpl::Ptr _current_impl; - - RosServerClass::Ptr _ros_api; - - rclcpp::Publisher::SharedPtr _ctrl_changed_pub; - rclcpp::ServiceBase::SharedPtr _loader_srv; - rclcpp::ServiceBase::SharedPtr _reset_srv; - rclcpp::ServiceBase::SharedPtr _reset_joints_srv; - rclcpp::SubscriptionBase::SharedPtr _fb_sub; - - bool _fb_feedback; - - rclcpp::TimerBase::SharedPtr _loop_timer; - double _time, _period; - - MatLogger2::Ptr _logger; -}; - -} // namespace Cartesian -} // namespace XBot - -#endif diff --git a/src/ros/CMakeLists.txt b/src/ros/CMakeLists.txt index cf9978ad..75228b5c 100644 --- a/src/ros/CMakeLists.txt +++ b/src/ros/CMakeLists.txt @@ -93,7 +93,7 @@ set(CartesianInterfaceRos_SRC client_api/CartesianRos.cpp client_api/InteractionRos.cpp client_api/PosturalRos.cpp - # RosImpl.cpp + RosImpl.cpp RosServerClass.cpp CartesianMarker.cpp # JoyStick.cpp diff --git a/src/ros/RosImpl.cpp b/src/ros/RosImpl.cpp index b4f18fe5..884df836 100644 --- a/src/ros/RosImpl.cpp +++ b/src/ros/RosImpl.cpp @@ -1,12 +1,12 @@ #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +//#include +//#include +#include #include "fmt/format.h" @@ -19,57 +19,82 @@ #define THROW_NOT_IMPL throw std::runtime_error("Not implemented function " + std::string(__func__)); using namespace XBot::Cartesian; -using namespace cartesian_interface; + +using namespace std::chrono_literals; namespace { -ProblemDescription construct_problem(ros::NodeHandle nh) +ProblemDescription construct_problem(rclcpp::Node::SharedPtr node) { - auto get_task_list_srv = nh.serviceClient("get_task_list"); + auto get_task_list_srv = node->create_client("get_task_list"); + // int attempts = 100; + // while(attempts-- && !get_task_list_srv.exists()) + // { + // if(attempts % 10 == 0) + // { + // fmt::print("Trying to contact server for service '{}' ... \n", + // get_task_list_srv->get_service_name()); + // } - int attempts = 100; - while(attempts-- && !get_task_list_srv.exists()) + // usleep(0.1 * 1e6); + // } + + if(!get_task_list_srv->wait_for_service(1s)) { - if(attempts % 10 == 0) - { - fmt::print("Trying to contact server for service '{}' ... \n", - get_task_list_srv.getService()); + if (!rclcpp::ok()) { + throw std::runtime_error("Interrupted while waiting for the service. Exiting."); } - - usleep(0.1 * 1e6); + throw std::runtime_error(fmt::format("Service '{}' not available", + get_task_list_srv->get_service_name())); } - GetTaskList srv_list; - if(!get_task_list_srv.waitForExistence(ros::Duration(1.0)) || - !get_task_list_srv.call(srv_list)) + auto srv_list_req = std::make_shared(); + auto srv_list_res = get_task_list_srv->async_send_request(srv_list_req); + + if (rclcpp::spin_until_future_complete(node, srv_list_res) != + rclcpp::FutureReturnCode::SUCCESS) { - throw std::runtime_error(fmt::format("Unable to call service '{}'", - get_task_list_srv.getService())); - } + throw std::runtime_error(fmt::format("Failed to call service '{}'", + get_task_list_srv->get_service_name())); + } AggregatedTask tasks; - - for(int i = 0; i < srv_list.response.names.size(); i++) + auto srv_list = srv_list_res.get(); + for(int i = 0; i < srv_list->names.size(); i++) { - std::string name = srv_list.response.names[i]; + std::string name = srv_list->names[i]; - auto get_task_info_srv = nh.serviceClient(name + "/get_task_properties"); + auto get_task_info_srv = node->create_client( + name + "/get_task_properties"); - GetTaskInfo srv_info; - if(!get_task_info_srv.waitForExistence(ros::Duration(1.0)) || - !get_task_info_srv.call(srv_info)) + if(!get_task_info_srv->wait_for_service(1s)) { - throw std::runtime_error(fmt::format("Unable to call service '{}'", - get_task_info_srv.getService())); + if (!rclcpp::ok()) { + throw std::runtime_error("Interrupted while waiting for the service. Exiting."); + } + throw std::runtime_error(fmt::format("Service '{}' not available", + get_task_info_srv->get_service_name())); } + auto srv_info_req = std::make_shared(); + auto srv_info_res = get_task_info_srv->async_send_request(srv_info_req); + + if (rclcpp::spin_until_future_complete(node, srv_info_res) != + rclcpp::FutureReturnCode::SUCCESS) + { + throw std::runtime_error(fmt::format("Failed to call service '{}'", + get_task_info_srv->get_service_name())); + } + + auto srv_info = srv_info_res.get(); + auto t = ClientApi::TaskRos::MakeInstance(name, - srv_info.response.type, - srv_info.response.lib_name, - nh); + srv_info->type, + srv_info->lib_name, + node); tasks.push_back(t); @@ -77,13 +102,23 @@ ProblemDescription construct_problem(ros::NodeHandle nh) ProblemDescription ik_pb(tasks); - attempts = 100; + int attempts = 100; + //TODO how to use callback group, when it must be given to the subscribers? + //is it so bad to use the generic rclcpp::spin here? + + //auto cbg = node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + //rclcpp::executors::SingleThreadedExecutor exec; + //exec.add_callback_group(cbg, node->get_node_base_interface()); + while(--attempts && !ik_pb.validate()) { if(attempts % 10 == 0) fmt::print("Waiting for all tasks to become valid... \n"); - auto queue = static_cast(nh.getCallbackQueue()); - queue->callAvailable(); + // auto queue = static_cast(nh.getCallbackQueue()); + // queue->callAvailable(); + + //exec.spin_all(1s); //or spin_all(); or spin_once??? + rclcpp::spin_some(node); usleep(0.1 * 1e6); } @@ -100,7 +135,7 @@ ProblemDescription construct_problem(ros::NodeHandle nh) std::ostream& XBot::Cartesian::operator<<(std::ostream& os, const RosClient& r) { - os << "CartesianInterfaceRos running inside ROS node " << ros::this_node::getName() << "\n"; + os << "CartesianInterfaceRos running inside ROS node " << r._ns << "\n"; auto tasklist = r.getTaskList(); os << "Defined tasks: \n"; for(auto t : tasklist) @@ -115,7 +150,9 @@ std::ostream& XBot::Cartesian::operator<<(std::ostream& os, const RosClient& r) RosInitializer::RosInitializer(std::string ns) { - if(!ros::ok()) + + bool first_init = false; + if(!rclcpp::ok()) { std::string ns_arg = "__ns:="; ns_arg += ""; @@ -123,35 +160,60 @@ RosInitializer::RosInitializer(std::string ns) int argc = args.size(); - ros::init(argc, (char **)args.data(), "cartesio_ros", - ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); + rclcpp::init(argc, (char **)args.data()); + + first_init = true; + } - ROS_WARN("Initializing roscpp under namespace '%s' with anonymous name '%s'", - ros::this_node::getNamespace().c_str(), - ros::this_node::getName().c_str() - ); + //ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); + _node = rclcpp::Node::make_shared("cartesio_ros", ns); + _ros2_cbg = _node->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + if (first_init) { + RCLCPP_WARN(_node->get_logger(), + "Initializing rclcpp under namespace '%s' with name '%s'", + _node->get_namespace(), + _node->get_name() + ); + } else { + RCLCPP_WARN(_node->get_logger(), + "Recreating the node under namespace '%s' with name '%s'. Ported from ROS1, ask Arturo if this is correct", + _node->get_namespace(), + _node->get_name() + ); } - _nh.reset(new ros::NodeHandle(ns)); - _nh->setCallbackQueue(&_queue); + // TODO correct porting? + // _nh.reset(new ros::NodeHandle(ns)); + // _nh->setCallbackQueue(&_queue); + _exec = std::make_shared(); + _exec->add_callback_group(_ros2_cbg, _node->get_node_base_interface()); + } -ros::NodeHandle & RosInitializer::nh() +rclcpp::Node::SharedPtr RosInitializer::node() { - return *_nh; + return _node; } void RosInitializer::callAvailable() { - _queue.callAvailable(); + // TODO correcT? + //_queue.callAvailable(); + _exec->spin_some(); } RosClient::RosClient(std::string ns): + _ns(ns), RosInitializer(ns), - CartesianInterfaceImpl(::construct_problem(nh())) + CartesianInterfaceImpl(::construct_problem(node())) { - _load_ctrl_srv = nh().serviceClient("load_controller"); + _load_ctrl_srv = node()->create_client("load_controller"); + + _tf_buffer = std::make_unique(node()->get_clock()); + _listener = std::make_shared(*_tf_buffer); + } void RosClient::set_async_mode(bool async) @@ -192,27 +254,35 @@ bool XBot::Cartesian::RosClient::getPoseFromTf(const std::string& source_frame, Eigen::Affine3d& t_T_s) { - tf::StampedTransform T; + geometry_msgs::msg::TransformStamped T; + // if(!_tf_buffer->waitForTransform(target_frame, source_frame, rclcpp::Time(0), rclcpp::Duration::from_seconds(1.0))) + // { + // RCLCPP_ERROR(this->get_logger(), "Wait for transform timed out"); + // return false; + // } - if(!_listener.waitForTransform(target_frame, source_frame, ros::Time(0), ros::Duration(1.0))) - { - ROS_ERROR("Wait for transform timed out"); - return false; - } - + try { - try - { - _listener.lookupTransform(target_frame, source_frame, ros::Time(0), T); - } - catch(tf::TransformException ex) - { - ROS_ERROR("%s", ex.what()); + T = _tf_buffer->lookupTransform(target_frame, source_frame, tf2::TimePointZero); + + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), "Could not transform %s to %s: %s", + source_frame.c_str(), target_frame.c_str(), ex.what()); return false; } - tf::transformTFToEigen(T, t_T_s); + //tf::transformTFToEigen(T, t_T_s); + t_T_s.translation().x() = T.transform.translation.x; + t_T_s.translation().y() = T.transform.translation.y; + t_T_s.translation().z() = T.transform.translation.z; + + t_T_s.linear() = Eigen::Quaterniond(T.transform.rotation.w, + T.transform.rotation.x, + T.transform.rotation.y, + T.transform.rotation.z).toRotationMatrix(); + return true; } @@ -227,25 +297,38 @@ void RosClient::loadController(const std::string& controller_name, const std::string& problem_description_string, const bool force_reload) { - cartesian_interface::LoadController srv; - srv.request.controller_name = controller_name; - srv.request.force_reload = force_reload; - srv.request.problem_description_name = problem_description_name; - srv.request.problem_description_string = problem_description_string; + auto load_controller_req = std::make_shared(); + load_controller_req->controller_name = controller_name; + load_controller_req->force_reload = force_reload; + load_controller_req->problem_description_name = problem_description_name; + load_controller_req->problem_description_string = problem_description_string; + + while (!_load_ctrl_srv->wait_for_service(1s)) { + if (!rclcpp::ok()) { + throw std::runtime_error("Interrupted while waiting for the service. Exiting."); + } + RCLCPP_INFO(node()->get_logger(), "service not available, waiting again..."); + } - if(!_load_ctrl_srv.call(srv)) + auto load_controller_res = _load_ctrl_srv->async_send_request(load_controller_req); + // Wait for the result. + if (rclcpp::spin_until_future_complete(node(), load_controller_res) != + rclcpp::FutureReturnCode::SUCCESS) { - throw std::runtime_error("Unable to connect to " + _load_ctrl_srv.getService()); - } + throw std::runtime_error(fmt::format("Unable to connect to '{}'", + _load_ctrl_srv->get_service_name())); + } - if(!srv.response.success) + if(!load_controller_res.get()->success) { - throw std::runtime_error(_load_ctrl_srv.getService() + " responded with an error:\n\t" + srv.response.message); + throw std::runtime_error(fmt::format("{} responded with an error:\n\t{}", + _load_ctrl_srv->get_service_name(), + load_controller_res.get()->message)); } - ROS_INFO("%s", srv.response.message.c_str()); + RCLCPP_INFO(node()->get_logger(), "%s", load_controller_res.get()->message.c_str()); - std::string ns = nh().getNamespace(); + std::string ns = node()->get_namespace(); this->~RosClient(); new(this) RosClient(ns); @@ -292,28 +375,41 @@ bool RosClient::abortStiffnessTransition(const std::string& end_effector) namespace { - bool call_reset_world_service(ros::NodeHandle& nh, + bool call_reset_world_service(rclcpp::Node::SharedPtr node, const Eigen::Affine3d& w_T_new_world, const std::string& ee_name) { - auto client = nh.serviceClient("reset_world"); - if(!client.waitForExistence(ros::Duration(3.0))) + + + auto client = node->create_client("reset_world"); + if(!client->wait_for_service(3s)) { throw std::runtime_error("unable to reset world, service unavailable"); } - cartesian_interface::ResetWorld srv; - tf::poseEigenToMsg(w_T_new_world, srv.request.new_world); - srv.request.from_link = ee_name; - - if(!client.call(srv)) + auto reset_world_req = std::make_shared(); + //tf::poseEigenToMsg(w_T_new_world, srv.request.new_world); + reset_world_req->new_world.position.x = w_T_new_world.translation().x(); + reset_world_req->new_world.position.y = w_T_new_world.translation().y(); + reset_world_req->new_world.position.z = w_T_new_world.translation().z(); + Eigen::Quaterniond q(w_T_new_world.linear()); + reset_world_req->new_world.orientation.x = q.x(); + reset_world_req->new_world.orientation.y = q.y(); + reset_world_req->new_world.orientation.z = q.z(); + reset_world_req->new_world.orientation.w = q.w(); + + reset_world_req->from_link = ee_name; + + auto reset_world_res = client->async_send_request(reset_world_req); + if (rclcpp::spin_until_future_complete(node, reset_world_res) != + rclcpp::FutureReturnCode::SUCCESS) { throw std::runtime_error("unable to reset world, service call failed"); - } + } - ROS_INFO("%s", srv.response.message.c_str()); + RCLCPP_INFO(node->get_logger(), "%s", reset_world_res.get()->message.c_str()); - if(!srv.response.success) + if(!reset_world_res.get()->success) { throw std::runtime_error("unable to reset world, service responded with an error"); } @@ -324,12 +420,12 @@ namespace bool RosClient::resetWorld(const Eigen::Affine3d& w_T_new_world) { - return ::call_reset_world_service(nh(), w_T_new_world, ""); + return ::call_reset_world_service(node(), w_T_new_world, ""); } bool XBot::Cartesian::RosClient::resetWorld(const std::string& ee_name) { - return ::call_reset_world_service(nh(), Eigen::Affine3d::Identity(), ee_name); + return ::call_reset_world_service(node(), Eigen::Affine3d::Identity(), ee_name); } diff --git a/src/ros/client_api/CartesianRos.cpp b/src/ros/client_api/CartesianRos.cpp index e4ace5cb..00a8bde0 100644 --- a/src/ros/client_api/CartesianRos.cpp +++ b/src/ros/client_api/CartesianRos.cpp @@ -19,14 +19,14 @@ CartesianRos::CartesianRos(std::string name, TaskRos(name, node), _Tref_recv(false), _vref_recv(false) { - _action_cli = rclcpp_action::create_client(_node, name + "/reach"); + _action_cli = rclcpp_action::create_client(_node, "/" + name + "/reach"); _cart_info_cli = create_client(node, name + "/get_cartesian_task_properties"); while(!_action_cli->wait_for_action_server(1s)) { RCLCPP_INFO_STREAM(_node->get_logger(), - "waiting for action server" << name << "/reach" + "waiting for action server " << name << "/reach" ); } diff --git a/src/ros/client_api/InteractionRos.cpp b/src/ros/client_api/InteractionRos.cpp index b4b280c2..29fca883 100644 --- a/src/ros/client_api/InteractionRos.cpp +++ b/src/ros/client_api/InteractionRos.cpp @@ -16,17 +16,20 @@ InteractionRos::InteractionRos(std::string name, rclcpp::Node::SharedPtr node): CartesianRos(name, node) { - _action_cli = rclcpp_action::create_client(_node, name + "/stiffness"); - _interaction_info_cli = _node->create_client(name + "/get_interaction_task_properties"); + //NOTE: namespace not correctly detected with actions, so we need to prepend the slash before the name, for actions + _action_cli = rclcpp_action::create_client(_node, "/" + name + "/stiffness"); // Ugly: Too many blocking calls? while(!_action_cli->wait_for_action_server(1s)) { RCLCPP_INFO_STREAM(_node->get_logger(), - "Waiting for action server" << name << "/stiffness" + fmt::format("Waiting for action server '{}'", + "/"+name+"/stiffness") ); } + _interaction_info_cli = _node->create_client(name + "/get_interaction_task_properties"); + while(!_interaction_info_cli->wait_for_service(1s)) { RCLCPP_INFO_STREAM(_node->get_logger(), @@ -139,15 +142,18 @@ const Impedance & InteractionRos::getImpedance() Eigen::Vector3d temp1, temp2; Eigen::Vector6d temp3; - tf2::fromMsg(fut.get()->impedance.linear.stiffness, temp1); - tf2::fromMsg(fut.get()->impedance.angular.stiffness, temp2); + //issue https://github.com/ros2/rclcpp/issues/1968 + auto result = *fut.get(); + + tf2::fromMsg(result.impedance.linear.stiffness, temp1); + tf2::fromMsg(result.impedance.angular.stiffness, temp2); temp3.head(3) = temp1; temp3.tail(3) = temp2; _impedance.stiffness = temp3.asDiagonal(); - tf2::fromMsg(fut.get()->impedance.linear.damping_ratio, temp1); - tf2::fromMsg(fut.get()->impedance.angular.damping_ratio, temp2); + tf2::fromMsg(result.impedance.linear.damping_ratio, temp1); + tf2::fromMsg(result.impedance.angular.damping_ratio, temp2); temp3.head(3) = temp1; temp3.tail(3) = temp2; @@ -187,9 +193,12 @@ bool InteractionRos::setImpedance (const Impedance & impedance) if((rclcpp::spin_until_future_complete(_node, fut, 1s) == rclcpp::FutureReturnCode::SUCCESS)) { - RCLCPP_INFO_STREAM(_node->get_logger(), fut.get()->message); - - return fut.get()->success; + + //issue https://github.com/ros2/rclcpp/issues/1968 + auto result = *fut.get(); + RCLCPP_INFO_STREAM(_node->get_logger(), result.message); + + return result.success; } else { @@ -214,9 +223,12 @@ void InteractionRos::getForceLimits (Eigen::Vector6d& fmax) const { // Note: get current state for task (should it be getPoseReference instead?) Eigen::Vector3d force, torque; + + //issue https://github.com/ros2/rclcpp/issues/1968 + auto result = *fut.get(); - tf2::fromMsg(fut.get()->fmax.force, force); - tf2::fromMsg(fut.get()->fmax.torque, torque); + tf2::fromMsg(result.fmax.force, force); + tf2::fromMsg(result.fmax.torque, torque); fmax << force, torque; } @@ -240,9 +252,11 @@ bool InteractionRos::setForceLimits (const Eigen::Vector6d& fmax) if (rclcpp::spin_until_future_complete(_node, fut, 1s) == rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO_STREAM(_node->get_logger(), fut.get()->message); + //issue https://github.com/ros2/rclcpp/issues/1968 + auto result = *fut.get(); + RCLCPP_INFO_STREAM(_node->get_logger(), result.message); - return fut.get()->success; + return result.success; } else { @@ -339,9 +353,11 @@ bool InteractionRos::setImpedanceRefLink(const std::string & new_impedance_ref_l if (rclcpp::spin_until_future_complete(_node, fut, 1s) == rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_INFO_STREAM(_node->get_logger(), fut.get()->message); + //issue https://github.com/ros2/rclcpp/issues/1968 + auto result = *fut.get(); + RCLCPP_INFO_STREAM(_node->get_logger(), result.message); - return fut.get()->success; + return result.success; } else { diff --git a/src/ros/client_api/TaskRos.cpp b/src/ros/client_api/TaskRos.cpp index 3c0c6c07..fb59a1cb 100644 --- a/src/ros/client_api/TaskRos.cpp +++ b/src/ros/client_api/TaskRos.cpp @@ -4,9 +4,9 @@ #include "../utils/RosUtils.h" -// #include "ros/client_api/CartesianRos.h" -// #include "ros/client_api/InteractionRos.h" -// #include "ros/client_api/PosturalRos.h" +#include "ros/client_api/CartesianRos.h" +#include "ros/client_api/InteractionRos.h" +#include "ros/client_api/PosturalRos.h" #include @@ -394,18 +394,18 @@ TaskDescription::Ptr TaskRos::MakeInstance(std::string name, { return task; } - // else if(type == "Interaction") - // { - // return std::make_shared(name, nh); - // } - // else if(type == "Cartesian") - // { - // return std::make_shared(name, nh); - // } - // else if(type == "Postural") - // { - // return std::make_shared(name, nh); - // } + else if(type == "Interaction") + { + return std::make_shared(name, node); + } + else if(type == "Cartesian") + { + return std::make_shared(name, node); + } + else if(type == "Postural") + { + return std::make_shared(name, node); + } else { return std::make_shared(name, node);