diff --git a/CHANGELOG.md b/CHANGELOG.md index 0b05a60804..e9e4b7f672 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ All notable changes to this project are documented in this file. ### Added ### Changed - Some improvements on the YarpRobotLoggerDevice (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910) +- Removed ROS1 device publisher and the corresponding example (https://github.com/ami-iit/bipedal-locomotion-framework/pull/910) ### Fixed ## [0.20.0] - 2024-12-16 diff --git a/CMakeLists.txt b/CMakeLists.txt index 677ebe8c4f..db9146fb65 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,8 +15,6 @@ option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) option(BUILD_TESTING "Create tests using CMake" OFF) include(CTest) -option(BUILD_DEVICE_EXAMPLES "Create example devices using CMake" OFF) - # Check BipedalLocomotionFramework dependencies, find necessary libraries. include(BipedalLocomotionFrameworkDependencies) diff --git a/cmake/AddBipedalLocomotionYARPDevice.cmake b/cmake/AddBipedalLocomotionYARPDevice.cmake index 78a703380e..e3df07c62e 100644 --- a/cmake/AddBipedalLocomotionYARPDevice.cmake +++ b/cmake/AddBipedalLocomotionYARPDevice.cmake @@ -2,11 +2,6 @@ # This software may be modified and distributed under the terms of the # BSD-3-Clause license. -framework_dependent_option(FRAMEWORK_COMPILE_example_devices - "Compile example devices?" ON - "BUILD_DEVICE_EXAMPLES" OFF) - - function(add_bipedal_yarp_device) set(options ) set(oneValueArgs NAME) diff --git a/cmake/BipedalLocomotionFrameworkDependencies.cmake b/cmake/BipedalLocomotionFrameworkDependencies.cmake index e744c35e07..8bee481fb4 100644 --- a/cmake/BipedalLocomotionFrameworkDependencies.cmake +++ b/cmake/BipedalLocomotionFrameworkDependencies.cmake @@ -141,10 +141,6 @@ framework_dependent_option(FRAMEWORK_COMPILE_YarpUtilities "Compile YarpHelper library?" ON "FRAMEWORK_USE_YARP" OFF) -framework_dependent_option(FRAMEWORK_COMPILE_Ros1Publisher - "Compile YarpUtilities::RosPublisher class?" ON - "FRAMEWORK_USE_YARP" OFF) - framework_dependent_option(FRAMEWORK_COMPILE_RosImplementation "Compile All the ROS implementations?" ON "FRAMEWORK_USE_rclcpp" OFF) diff --git a/devices/CMakeLists.txt b/devices/CMakeLists.txt index 9196e24973..8ee9c8e73d 100644 --- a/devices/CMakeLists.txt +++ b/devices/CMakeLists.txt @@ -7,8 +7,3 @@ add_subdirectory(JointTorqueControlDevice) add_subdirectory(RobotDynamicsEstimatorDevice) add_subdirectory(YarpRobotLoggerDevice) add_subdirectory(VectorsCollectionWrapper) - -if (FRAMEWORK_COMPILE_example_devices) - add_subdirectory(examples) -endif() - diff --git a/devices/examples/CMakeLists.txt b/devices/examples/CMakeLists.txt deleted file mode 100644 index d65eb24c9d..0000000000 --- a/devices/examples/CMakeLists.txt +++ /dev/null @@ -1,6 +0,0 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -add_subdirectory(ROSPublisherTestDevice) - diff --git a/devices/examples/ROSPublisherTestDevice/CMakeLists.txt b/devices/examples/ROSPublisherTestDevice/CMakeLists.txt deleted file mode 100644 index 88bce95198..0000000000 --- a/devices/examples/ROSPublisherTestDevice/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright (C) 2020 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -if(FRAMEWORK_COMPILE_YarpImplementation AND FRAMEWORK_COMPILE_YarpUtilities AND FRAMEWORK_COMPILE_Ros1Publisher) -# Warning: the option of yarp_configure_plugins_installation should be different from the plugin name -add_bipedal_yarp_device( - NAME ROSPublisherTestDevice - TYPE BipedalLocomotion::ROSPublisherTestDevice - SOURCES src/ROSPublisherTestDevice.cpp - PUBLIC_HEADERS include/BipedalLocomotion/ROSPublisherTestDevice.h - PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} BipedalLocomotion::YarpUtilities BipedalLocomotion::ParametersHandlerYarpImplementation - CONFIGURE_PACKAGE_NAME ros_publisher_test_device) -endif() - diff --git a/devices/examples/ROSPublisherTestDevice/README.md b/devices/examples/ROSPublisherTestDevice/README.md deleted file mode 100644 index 8f8a0b27bb..0000000000 --- a/devices/examples/ROSPublisherTestDevice/README.md +++ /dev/null @@ -1,28 +0,0 @@ -### ROS Publisher Test Device - -- Run the servers - - ```bash - roscore - yarpserver --ros - yarpdev --device transformServer --name tfServer --ROS::enable_ros_publisher true --ROS::enable_ros_subscriber true - ``` - - (This step can also be done using [ros-publisher-test-runner.xml](./ros-publisher-test-runner.xml) through the yarpmanager). - -- Run the device using - - ``` bash - YARP_ROBOT_NAME=iCubGazeboV2_5 yarprobotinterface --config launch-ros-publisher-test.xml - ``` - -- Check the outputs of the test device by running, - - ``` bash - rostopic echo /joint_states # to check the published joint states - name: [hello] position: [20] - rostopic echo /wrench/right # to check the published wrench - frame: right force: x:0.0 y:1.0 z:2.0 torque: x:0.0 y:0.0 z:0.0 - rostopic echo /tf # to check the published pose between /world and /dummy as identity (zero position and unit quaternion) - ``` - - - diff --git a/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt b/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt deleted file mode 100644 index 8397da4302..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. - -# Get list of models -subdirlist(subdirs ${CMAKE_CURRENT_SOURCE_DIR}/robots/) -# Install each model -foreach (dir ${subdirs}) - yarp_install(DIRECTORY robots/${dir} DESTINATION ${YARP_ROBOTS_INSTALL_DIR}) -endforeach () - diff --git a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml b/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml deleted file mode 100644 index bb9a274e01..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/launch-ros-publisher-test.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - diff --git a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml b/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml deleted file mode 100644 index ab2c0f6abf..0000000000 --- a/devices/examples/ROSPublisherTestDevice/app/robots/iCubGazeboV2_5/publisher/ros-publisher.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - - /joint_states - /tfServer - - ("right", "left") - ("/wrench/right", "/wrench/left") - - diff --git a/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h b/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h deleted file mode 100644 index f013ff2f6d..0000000000 --- a/devices/examples/ROSPublisherTestDevice/include/BipedalLocomotion/ROSPublisherTestDevice.h +++ /dev/null @@ -1,42 +0,0 @@ -/** - * @file ROSPublisherTestDevice.h - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H -#define BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H - -#include -#include -#include -#include - -#include - -namespace BipedalLocomotion -{ - class ROSPublisherTestDevice; -} - -class BipedalLocomotion::ROSPublisherTestDevice : public yarp::dev::DeviceDriver, - public yarp::os::PeriodicThread -{ -public: - ROSPublisherTestDevice(double period, - yarp::os::ShouldUseSystemClock useSystemClock - = yarp::os::ShouldUseSystemClock::No); - ROSPublisherTestDevice(); - ~ROSPublisherTestDevice(); - - virtual bool open(yarp::os::Searchable& config) final; - virtual bool close() final; - virtual void run() final; - - std::unique_ptr pub; -}; - - - -#endif //BIPEDAL_LOCOMOTION_FRAMEWORK_ROS_PUBLISHER_TEST_DEVICE_H diff --git a/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml b/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml deleted file mode 100644 index e7ac6cfc11..0000000000 --- a/devices/examples/ROSPublisherTestDevice/ros-publisher-test-runner.xml +++ /dev/null @@ -1,18 +0,0 @@ - - ros-publisher-test-launcher - - roscore - - - - yarpserver - --ros - - - - yarpdev - --device transformServer --name tfServer --ROS::enable_ros_publisher true --ROS::enable_ros_subscriber true - - - - diff --git a/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp b/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp deleted file mode 100644 index 2a99b80cc0..0000000000 --- a/devices/examples/ROSPublisherTestDevice/src/ROSPublisherTestDevice.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/** - * @file ROSPublisherTestDevice.cpp - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include -#include -#include -#include - -BipedalLocomotion::ROSPublisherTestDevice::ROSPublisherTestDevice(double period, - yarp::os::ShouldUseSystemClock useSystemClock) - : yarp::os::PeriodicThread(period, useSystemClock) -{ - pub = std::make_unique("/PublisherTest"); -} - -BipedalLocomotion::ROSPublisherTestDevice::ROSPublisherTestDevice() - : yarp::os::PeriodicThread(0.01, yarp::os::ShouldUseSystemClock::No) -{ - pub = std::make_unique("/PublisherTest"); -} - -BipedalLocomotion::ROSPublisherTestDevice::~ROSPublisherTestDevice() -{ -} - -bool BipedalLocomotion::ROSPublisherTestDevice::open(yarp::os::Searchable& config) -{ - std::shared_ptr configHandler = std::make_shared(); - configHandler->set(config); - - if (!pub->initialize(configHandler)) - { - return false; - } - - this->start(); - return true; -} - -void BipedalLocomotion::ROSPublisherTestDevice::run() -{ - std::vector jointList{"hello"}; - std::vector jointPos{20.0}; - - pub->publishJointStates(jointList, jointPos); - - std::vector wrench{0.0, 1.0, 2.0,0.0, 0.0, 0.0}; - pub->publishWrench("right", wrench); - - - std::vector pose{1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1., 0, 0, 0, 0, 1.}; - pub->publishTransform("/world", "/dummy", pose); -} - - -bool BipedalLocomotion::ROSPublisherTestDevice::close() -{ - this->stop(); - pub->stop(); - return true; -} - diff --git a/src/YarpUtilities/CMakeLists.txt b/src/YarpUtilities/CMakeLists.txt index a6eccba05b..0a382934e3 100644 --- a/src/YarpUtilities/CMakeLists.txt +++ b/src/YarpUtilities/CMakeLists.txt @@ -5,17 +5,10 @@ # set target name if(FRAMEWORK_COMPILE_YarpUtilities) - set(YarpUtilities_Ros1Publisher_PUBLIC_HEADERS "") - set(YarpUtilities_Ros1Publisher_SOURCES "") - if(FRAMEWORK_COMPILE_Ros1Publisher) - list(APPEND YarpUtilities_Ros1Publisher_PUBLIC_HEADERS "include/BipedalLocomotion/YarpUtilities/RosPublisher.h") - list(APPEND YarpUtilities_Ros1Publisher_SOURCES "src/RosPublisher.cpp") - endif() - add_bipedal_locomotion_library( NAME YarpUtilities - SOURCES src/Helper.cpp ${YarpUtilities_Ros1Publisher_SOURCES} - PUBLIC_HEADERS include/BipedalLocomotion/YarpUtilities/Helper.h include/BipedalLocomotion/YarpUtilities/Helper.tpp ${YarpUtilities_Ros1Publisher_PUBLIC_HEADERS} + SOURCES src/Helper.cpp + PUBLIC_HEADERS include/BipedalLocomotion/YarpUtilities/Helper.h include/BipedalLocomotion/YarpUtilities/Helper.tpp PUBLIC_LINK_LIBRARIES ${YARP_LIBRARIES} ${iDynTree_LIBRARIES} BipedalLocomotion::GenericContainer BipedalLocomotion::ParametersHandler BipedalLocomotion::TextLogging SUBDIRECTORIES tests) diff --git a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h b/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h deleted file mode 100644 index 26cfb801e7..0000000000 --- a/src/YarpUtilities/include/BipedalLocomotion/YarpUtilities/RosPublisher.h +++ /dev/null @@ -1,154 +0,0 @@ -/** - * @file RosPublisher.h - * @authors Prashanth Ramadoss - * @copyright 2019 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#ifndef BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H -#define BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H - -#include -#include -#include -#include -#include -namespace BipedalLocomotion -{ - -/** - * Helper for YARP library. - */ -namespace YarpUtilities -{ - -/** - * The class internally contains a YARP based ROS node and a set of publishers. - * Current implementation consists of - * - Joint states publisher - * - Wrenches publisher - * - Transform broadcaster - * Although the class might be ROS independent, in order to run the code, ROS is required and usual YARP-ROS connections need to be made. - */ -class [[deprecated("The ROS 1-based BipedalLocomotion::YarpUtilities::RosPublisher is deprecated, use ROS 2 instead.")]] RosPublisher -{ -public: - /** - * Constructor - * @param[in] nodeName Name of the ROS publisher node - */ - RosPublisher(const std::string& nodeName); - - /** - * Destructor - */ - ~RosPublisher(); - - /** - * Configures the publishers and ROS topics for publishing messages - * The parameters used to configure the RosPublisher, - * - "joint_states_topic" name of the topic over which the joint states need to be published - * - "transform_server_port" name of the already running transform server's remote port - * - "WrenchPublishers" is group with the following parameters, - * - "frame_names" a list containing the frames at which the published wrenches should be expressed - * - "topics" a list containing the topics over which the wrenches need to be published. Must be the same size and order as frame_names. - * @param[in] handler Parameter handler - * @note this method needs to be called after construction and before publishing - */ - bool initialize(std::weak_ptr handler); - - /** - * Clear internal configuration and close the node - */ - void stop(); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - * @param[in] jointVelocities list of joint velocities in m/s or rad/s with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions, - const BipedalLocomotion::GenericContainer::Vector::Ref jointVelocities); - - /** - * Publish the joint states over the configured joint states topic - * The joint velocities and joint efforts are set to zero. - * @param[in] jointList list of joints - * @param[in] jointPositions list of joint positions in m or rad with the same size as the joints list - * @param[in] jointVelocities list of joint velocities in m/s or rad/s with the same size as the joints list - * @param[in] jointPositions list of joint torques/forces in Nm or N with the same size as the joints list - */ - bool publishJointStates(const BipedalLocomotion::GenericContainer::Vector::Ref jointList, - const BipedalLocomotion::GenericContainer::Vector::Ref jointPositions, - const BipedalLocomotion::GenericContainer::Vector::Ref jointVelocities, - const BipedalLocomotion::GenericContainer::Vector::Ref jointEfforts); - - /** - * Publish the wrenches over the configured wrench topic associated to the frame - * @param[in] frameName frame at which the wrench will be expressed - * @param[in] wrench6d 6d wrench as force-torque in N-Nm with serialziation as fx fy fz tx ty tz - */ - bool publishWrench(const std::string& frameName, - BipedalLocomotion::GenericContainer::Vector::Ref wrench6d); - - /** - * Publish transforms to the transform server - * @param[in] target target frame for the transform - * @param[in] source source frame of the transform - * @param[in] transformAsVector16d 4x4 transform as a vector data in row-major order - */ - bool publishTransform(const std::string& target, const std::string& source, - const BipedalLocomotion::GenericContainer::Vector::Ref transformAsVector16d); - - /** - * Configure which transform server to connect to - * @param[in] transformServerPort name of the transform server port - */ - bool configureTransformServer(const std::string& transformServerPort); - - /** - * Configure the topic over which joint states are published - * @param[in] topicName topic name - */ - bool configureJointStatePublisher(const std::string& topicName); - - /** - * Add or reconfigure a wrench publisher - * @param[in] frameName frame name - * @param[in] topicName topic name - */ - bool configureWrenchPublisher(const std::string& frameName, - const std::string& topicName); - - /** - * Remove wrench publisher - * @param[in] frameName frame name - */ - bool removeWrenchPublisher(const std::string& frameName); - - RosPublisher(const RosPublisher& other) = delete; /**< delete copy constructor */ - RosPublisher(RosPublisher&& other) = delete; /**< delete move constructor */ - RosPublisher& operator=(const RosPublisher& other) = delete; /**< delete copy assignment operator */ - RosPublisher& operator=(RosPublisher&& other) = delete; /**< delete move assignment operator */ -private: - class Impl; - std::unique_ptr m_pimpl; - -}; - -} // namespace YarpUtilities -} // namespace BipedalLocomotion -#endif // BIPEDAL_LOCOMOTION_YARP_UTILITIES_ROS_PUBLISHER_H - diff --git a/src/YarpUtilities/src/RosPublisher.cpp b/src/YarpUtilities/src/RosPublisher.cpp deleted file mode 100644 index 4162746b4e..0000000000 --- a/src/YarpUtilities/src/RosPublisher.cpp +++ /dev/null @@ -1,497 +0,0 @@ -/** - * @file RosPublisher.cpp - * @authors Prashanth Ramadoss - * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and - * distributed under the terms of the BSD-3-Clause license. - */ - -#include "BipedalLocomotion/YarpUtilities/RosPublisher.h" - -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include -#include - -using namespace BipedalLocomotion::YarpUtilities; -using namespace BipedalLocomotion; - -template -struct PublisherDetails -{ - PublisherPtr ptr{nullptr}; /**< YARP object for a ROS publisher */ - std::string topic{"/topic"}; /**< topic over which the message is published */ - Msg msg; /**< YARP object for a ROS message */ -}; - -using JointStateMsg = yarp::rosmsg::sensor_msgs::JointState; -using JointStatePublisherPtr = std::unique_ptr< yarp::os::Publisher >; -using WrenchStampedMsg = yarp::rosmsg::geometry_msgs::WrenchStamped; -using WrenchStampedPublisherPtr = std::unique_ptr< yarp::os::Publisher >; -using WrenchPublisherDetails = PublisherDetails; -using JointStatePublisherDetails = PublisherDetails; - -class RosPublisher::Impl -{ -public: - /** - * Get ROS timestamp from Yarp time now - */ - yarp::rosmsg::TickTime getTimeStampFromYarp(); - - /** - * Open the ROS topic for the corresponding publisher - * In ROS terms, advertise the topic over which the node is going to publish - */ - template - bool openPublisher(Publisher* pub, const std::string& topicName) - { - std::string_view printPrefix = "[RosPublisher::Impl::openPublisher] "; - if (!pub->topic(topicName)) - { - std::cerr << printPrefix << "Could not open ROS topic " << topicName << std::endl; - return false; - } - return true; - } - - - template - void configurePublisher(std::unique_ptr >& ptr) - { - if (ptr == nullptr) - { - ptr = std::make_unique< yarp::os::Publisher >(); - } - - ptr->close(); - } - - - /** - * Utility function to load vector parameters - */ - template - bool setupParamV(std::weak_ptr handler, - const std::string& param, std::vector& vec, const std::string& prefix) - { - auto handle = handler.lock(); - if (handle == nullptr) - { - return false; - } - - if (!handle->getParameter(param, vec)) - { - std::cerr << prefix << "The parameter handler could not find \"" << param << "\" in the configuration file. This is a required parameter." << std::endl; - return false; - } - return true; - } - - void resizeJointStateBuffer(const std::size_t& size); - - std::string nodeName; /**< name of the ROS node */ - std::unique_ptr node; /**< YARP object for a ROS node */ - - JointStatePublisherDetails jointStatePublisher; - std::unordered_map wrenchPublisherMap; /**< map of wrench frame and corresponding publisher*/ - - yarp::dev::PolyDriver transformBroadcaster; /**< YARP Polydriver object to open a transform client */ - yarp::dev::IFrameTransform* transformInterface{nullptr}; /**< YARP IFrameTransform interface to publish transforms */ - - bool publishTF{false}; /**< flag to enable publishing transforms to YARP transform server */ - bool initialized{false}; /**< flag to chekc if the publisher was initialized */ - - // placeholder variables - yarp::sig::Matrix pose; /**< placeholder variable for publishing transform data*/ - std::vector jointStateZero; /**< placeholder variable for zero joint state data*/ -}; - -RosPublisher::RosPublisher(const std::string& nodeName) : m_pimpl(std::make_unique()) -{ - m_pimpl->nodeName = nodeName; - m_pimpl->node = std::make_unique(m_pimpl->nodeName); - - m_pimpl->pose.resize(4, 4); - - std::cout << "[RosPublisher] Ensure roscore is running and yarpserver was run with --ros option." << std::endl; -} - -RosPublisher::~RosPublisher() = default; - -bool RosPublisher::initialize(std::weak_ptr handler) -{ - std::string printPrefix{"[RosPublisher::initialize] "}; - bool ok{true}; - - auto handle = handler.lock(); - if (handle == nullptr) - { - std::cerr << printPrefix << "The parameter handler has expired. Please check its scope." << std::endl; - return false; - } - - // configure joint state publisher if it exists - if (handle->getParameter("joint_states_topic", m_pimpl->jointStatePublisher.topic)) - { - if (!configureJointStatePublisher(m_pimpl->jointStatePublisher.topic)) - { - std::cerr << printPrefix << "Could not configure joint states publisher." << std::endl; - return false; - } - } - - // configure transform publisher - std::string tfServerPort; - if (handle->getParameter("transform_server_port", tfServerPort)) - { - m_pimpl->publishTF = true; - if (!configureTransformServer(tfServerPort)) - { - return false; - } - } - - auto wrenchHandle = handle->getGroup("WrenchPublishers"); - auto wrenchHandler = wrenchHandle.lock(); - if (wrenchHandler != nullptr) - { - std::vector frames; - if (!m_pimpl->setupParamV(wrenchHandler, "frame_names", frames, printPrefix)) - { - return false; - } - - std::vector topics; - if (!m_pimpl->setupParamV(wrenchHandler, "topics", topics, printPrefix)) - { - return false; - } - - if (frames.size() != topics.size()) - { - std::cerr << printPrefix << " WrenchPublishers group: frame_names and topics must be of same size." << std::endl; - return false; - } - - for (std::size_t idx = 0; idx < frames.size(); idx++) - { - if (!configureWrenchPublisher(frames[idx], topics[idx])) - { - std::cerr << printPrefix << "Could not configure wrench publisher for frame: " - << frames[idx] << " and topic: " << topics[idx] << std::endl; - return false; - } - } - } - - if (!ok) - { - std::cerr << printPrefix << "Failed to initialize the ROSPublisher" << std::endl; - return false; - } - - m_pimpl->initialized = true; - - return true; -} - -bool RosPublisher::configureJointStatePublisher(const std::string& topicName) -{ - m_pimpl->configurePublisher(m_pimpl->jointStatePublisher.ptr); - - m_pimpl->jointStatePublisher.topic = topicName; - if (!m_pimpl->openPublisher(m_pimpl->jointStatePublisher.ptr.get(), m_pimpl->jointStatePublisher.topic)) - { - return false; - } - - return true; -} - -bool RosPublisher::configureWrenchPublisher(const std::string& frameName, - const std::string& topicName) -{ - std::string_view printPrefix = "[RosPublisher::configureWrenchPublisher] "; - if (m_pimpl->wrenchPublisherMap.find(frameName) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Wrench publisher does not already exist. Adding a wrench publisher for " << frameName << "." << std::endl; - m_pimpl->wrenchPublisherMap[frameName] = WrenchPublisherDetails(); - } - - m_pimpl->configurePublisher(m_pimpl->wrenchPublisherMap.at(frameName).ptr); - m_pimpl->wrenchPublisherMap.at(frameName).topic = topicName; - if (!m_pimpl->openPublisher(m_pimpl->wrenchPublisherMap.at(frameName).ptr.get(), m_pimpl->wrenchPublisherMap.at(frameName).topic)) - { - return false; - } - - return true; -} - -bool RosPublisher::removeWrenchPublisher(const std::string& frameName) -{ - std::string_view printPrefix = "[RosPublisher::removeWrenchPublisher] "; - if (m_pimpl->wrenchPublisherMap.find(frameName) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Wrench publisher does not already exist." << std::endl; - return false; - } - - m_pimpl->wrenchPublisherMap.at(frameName).ptr->close(); - m_pimpl->wrenchPublisherMap.at(frameName).ptr = nullptr; - m_pimpl->wrenchPublisherMap.at(frameName).msg.clear(); - - m_pimpl->wrenchPublisherMap.erase(frameName); - return true; -} - -bool RosPublisher::configureTransformServer(const std::string& transformServerPort) -{ - if (m_pimpl->transformBroadcaster.isValid()) - { - m_pimpl->transformBroadcaster.close(); - m_pimpl->transformInterface = nullptr; - } - - yarp::os::Property tfBroadcasterSettings{{"device", yarp::os::Value("transformClient")}, - {"remote", yarp::os::Value(transformServerPort)}, - {"local", yarp::os::Value(m_pimpl->nodeName + "/transformClient")}}; - - std::string_view printPrefix = "[RosPublisher::openTransformBroadcaster] "; - if (!m_pimpl->transformBroadcaster.open(tfBroadcasterSettings)) - { - std::cerr << printPrefix << "Unable to open transform broadcaster. Ensure transform server is already running." << std::endl; - return false; - } - - if (!m_pimpl->transformBroadcaster.view(m_pimpl->transformInterface)) - { - std::cerr << printPrefix << "Unable to access transform interface." << std::endl; - return false; - } - - return true; -} - -bool RosPublisher::publishTransform(const std::string& target, - const std::string& source, - const BipedalLocomotion::GenericContainer::Vector::Ref transformAsVector16d) -{ - std::string_view printPrefix = "[RosPublisher::publishTransform] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - if (!m_pimpl->publishTF || m_pimpl->transformInterface == nullptr) - { - std::cerr << printPrefix << "Transform broadcaster was not configured. Unable to publish transforms." << std::endl; - return false; - } - - if (transformAsVector16d.size() != 16) - { - std::cerr << printPrefix << "Malformed Transform data. Expecting a 16d vector." << std::endl; - return false; - } - - for (size_t i = 0; i < 4; i++) - { - for (size_t j = 0; j < 4; j++) - { - m_pimpl->pose(i, j) = transformAsVector16d( (i*4) + j); - } - } - - if (!m_pimpl->transformInterface->setTransform(target, source, m_pimpl->pose)) - { - std::cerr << printPrefix << "Could not publish transform for source: " - << source << " and target: " << target << " frames." << std::endl; - return false; - } - - return true; -} - -void RosPublisher::Impl::resizeJointStateBuffer(const std::size_t& size) -{ - if (jointStateZero.size() != size) - { - jointStateZero.resize(size, 0.0); - } -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions) -{ - m_pimpl->resizeJointStateBuffer(jointList.size()); - return publishJointStates(jointList, jointPositions, m_pimpl->jointStateZero, m_pimpl->jointStateZero); -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions, - const GenericContainer::Vector::Ref jointVelocities) -{ - m_pimpl->resizeJointStateBuffer(jointList.size()); - return publishJointStates(jointList, jointPositions, jointVelocities, m_pimpl->jointStateZero); -} - -bool RosPublisher::publishJointStates(const GenericContainer::Vector::Ref jointList, - const GenericContainer::Vector::Ref jointPositions, - const GenericContainer::Vector::Ref jointVelocities, - const GenericContainer::Vector::Ref jointEfforts) -{ - std::string_view printPrefix = "[RosPublisher::publishJointStates] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - auto& pub = m_pimpl->jointStatePublisher; - if (pub.ptr == nullptr) - { - std::cerr << printPrefix << "Joint state publisher was not configured. Unable to publish joint states." << std::endl; - return false; - } - - if ( (jointList.size() != jointPositions.size()) || - (jointList.size() != jointVelocities.size()) || - (jointList.size() != jointEfforts.size())) - { - std::cerr << printPrefix << "Joint states size mismatch. Unable to publish joint states." << std::endl; - return false; - } - - pub.msg.header.seq++; - pub.msg.header.stamp = m_pimpl->getTimeStampFromYarp(); - - pub.msg.name.clear(); - pub.msg.position.clear(); - pub.msg.velocity.clear(); - pub.msg.effort.clear(); - - for (std::size_t idx = 0; idx < jointList.size(); idx++) - { - pub.msg.name.emplace_back(jointList(idx)); - pub.msg.position.emplace_back(jointPositions(idx)); - pub.msg.velocity.emplace_back(jointVelocities(idx)); - pub.msg.effort.emplace_back(jointEfforts(idx)); - } - - pub.ptr->write(pub.msg); - return true; -} - -bool RosPublisher::publishWrench(const std::string& frame, - BipedalLocomotion::GenericContainer::Vector::Ref wrench6d) -{ - std::string_view printPrefix = "[RosPublisher::publishWrench] "; - - if (!m_pimpl->initialized) - { - std::cerr << printPrefix << "Please call initialize before publishing." << std::endl; - return false; - } - - if (m_pimpl->wrenchPublisherMap.find(frame) == m_pimpl->wrenchPublisherMap.end()) - { - std::cerr << printPrefix << "Frame does not exist. Please add wrench publisher before publishing." << std::endl; - return false; - } - - auto& pub = m_pimpl->wrenchPublisherMap.at(frame); - if (pub.ptr == nullptr) - { - std::cerr << printPrefix << "Wrench publisher was not configured. Unable to publish wrench stamped messages." << std::endl; - return false; - } - - if (wrench6d.size() != 6) - { - std::cerr << printPrefix << "Expecting a 6d input vector. Unable to publish wrench." << std::endl; - return false; - } - - pub.msg.header.seq++; - pub.msg.header.frame_id = frame; - pub.msg.header.stamp = m_pimpl->getTimeStampFromYarp(); - pub.msg.wrench.force.x = wrench6d(0); - pub.msg.wrench.force.y = wrench6d(1); - pub.msg.wrench.force.z = wrench6d(2); - - pub.msg.wrench.torque.x = wrench6d(3); - pub.msg.wrench.torque.y = wrench6d(4); - pub.msg.wrench.torque.z = wrench6d(5); - pub.ptr->write(pub.msg); - - return true; -} - -yarp::rosmsg::TickTime RosPublisher::Impl::getTimeStampFromYarp() -{ - std::string_view printPrefix = "[RosPublisher::Impl::getTimeStampFromYarp] "; - yarp::rosmsg::TickTime rosTickTime; - - double yarpTimeNow{yarp::os::Time::now()}; - std::chrono::duration timeStamp(yarpTimeNow); - uint64_t secPart = std::chrono::duration_cast(timeStamp).count(); - uint64_t nsecPart = std::chrono::duration_cast(std::chrono::duration(yarpTimeNow - secPart)).count(); - - if (secPart > std::numeric_limits::max()) { - std::cerr << printPrefix - << "Timestamp exceeded the 64 bit representation, resetting it to 0" << std::endl; - secPart = 0; - } - - rosTickTime.sec = static_cast(secPart); - rosTickTime.nsec = static_cast(nsecPart); - - return rosTickTime; -} - - -void RosPublisher::stop() -{ - if (!m_pimpl->wrenchPublisherMap.empty()) - { - std::vector frames; - // use two loops to avoid segfaults due to map::erase in a for loop - // avoid iterator invalidation - for (auto& pair : m_pimpl->wrenchPublisherMap) - { - frames.push_back(pair.first); - } - - for (auto& frame : frames) - { - removeWrenchPublisher(frame); - } - } - - m_pimpl->wrenchPublisherMap.clear(); - if (m_pimpl->jointStatePublisher.ptr != nullptr) - { - m_pimpl->jointStatePublisher.ptr->close(); - m_pimpl->jointStatePublisher.ptr = nullptr; - } - - if (m_pimpl->transformBroadcaster.isValid()) - { - m_pimpl->transformBroadcaster.close(); - } -}