From 69a928f59be7a674938f98be6039fc6de9782000 Mon Sep 17 00:00:00 2001 From: "Barcis, Michal" Date: Mon, 15 Jun 2020 18:55:03 +0200 Subject: [PATCH] moving to ROS2 --- CMakeLists.txt | 109 +++--- README.md | 21 ++ config/sample.params.yaml | 8 + include/vrpn_client_ros/vrpn_client_ros.h | 44 +-- launch/sample.launch | 25 -- launch/sample.launch.py | 26 ++ package.xml | 13 +- src/vrpn_client_node.cpp | 8 +- src/vrpn_client_ros.cpp | 434 +++++++++++----------- src/vrpn_tracker_node.cpp | 15 +- 10 files changed, 381 insertions(+), 322 deletions(-) create mode 100644 README.md create mode 100644 config/sample.params.yaml delete mode 100644 launch/sample.launch create mode 100644 launch/sample.launch.py diff --git a/CMakeLists.txt b/CMakeLists.txt index c8eb07f..af6da17 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,66 +1,77 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.5) project(vrpn_client_ros) -find_package(catkin REQUIRED COMPONENTS geometry_msgs roscpp tf2_ros) +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +# find_package(catkin REQUIRED COMPONENTS geometry_msgs tf2_ros) find_package(VRPN REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) -catkin_package( - INCLUDE_DIRS include - LIBRARIES vrpn_client_ros - CATKIN_DEPENDS geometry_msgs tf2_ros - DEPENDS VRPN -) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() -set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++11;-Wall") -include_directories( - include - ${catkin_INCLUDE_DIRS} - SYSTEM ${VRPN_INCLUDE_DIR} -) +include_directories(include) add_library(vrpn_client_ros - src/vrpn_client_ros.cpp -) -target_link_libraries(vrpn_client_ros - ${catkin_LIBRARIES} - ${VRPN_LIBRARIES} -) + src/vrpn_client_ros.cpp) +ament_target_dependencies(vrpn_client_ros rclcpp std_msgs geometry_msgs VRPN) -add_executable(vrpn_tracker_node - src/vrpn_tracker_node.cpp) -add_dependencies(vrpn_tracker_node vrpn_client_ros) -target_link_libraries(vrpn_tracker_node - vrpn_client_ros - ${catkin_LIBRARIES} -) +add_executable(vrpn_tracker_node src/vrpn_tracker_node.cpp) +target_link_libraries(vrpn_tracker_node vrpn_client_ros) +ament_target_dependencies(vrpn_tracker_node rclcpp std_msgs geometry_msgs VRPN) -add_executable(vrpn_client_node - src/vrpn_client_node.cpp) -add_dependencies(vrpn_client_node vrpn_client_ros) -target_link_libraries(vrpn_client_node - vrpn_client_ros - ${catkin_LIBRARIES} -) +add_executable(vrpn_client_node src/vrpn_client_node.cpp) +target_link_libraries(vrpn_client_node vrpn_client_ros) +ament_target_dependencies(vrpn_client_node rclcpp std_msgs geometry_msgs VRPN) -install(TARGETS vrpn_client_ros vrpn_tracker_node vrpn_client_node - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ ) -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}/ ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +install(TARGETS + vrpn_client_ros + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install(TARGETS + vrpn_tracker_node + vrpn_client_node + DESTINATION lib/${PROJECT_NAME} ) -if(CATKIN_ENABLE_TESTING) - find_package(roslaunch REQUIRED) - find_package(roslint REQUIRED) - roslint_cpp() - roslint_add_test() - roslaunch_add_file_check(launch) -endif() +ament_package() diff --git a/README.md b/README.md new file mode 100644 index 0000000..c0ab500 --- /dev/null +++ b/README.md @@ -0,0 +1,21 @@ +# vrpn_client_ros +This fork aims at porting the original code from the kinetic-devel branch to ROS2. + +## Requirements + +The code requires VRPN to work. Unfortunately, currently the package is only +available in melodic and older ROS releases and can be installed with: +``` + apt-get install ros-melodic-vrpn +``` + +If you want to stick to ROS2 or do not have access to melodic packages you can +clone the following repository to the src folder of your ros_overlay_ws: +``` +git clone --single-branch --branch debian/melodic/vrpn git@github.com:ros-drivers-gbp/vrpn-release.git +``` + +## What works? + +I only use pose in my project, so I did not port anything else (TF, twist, accel). Also multiple sensors per tracker are not ported. +If there is anyone who would like to use the other features and is willing to test them, I'd be happy to help. diff --git a/config/sample.params.yaml b/config/sample.params.yaml new file mode 100644 index 0000000..446dac1 --- /dev/null +++ b/config/sample.params.yaml @@ -0,0 +1,8 @@ +vrpn_client_node: + ros__parameters: + server: 192.168.0.107 + port: 3883 + frame_id: "world" + use_server_time: false + refresh_tracker_frequency: 0.2 + update_frequency: 30.0 diff --git a/include/vrpn_client_ros/vrpn_client_ros.h b/include/vrpn_client_ros/vrpn_client_ros.h index 46676b6..56977f2 100644 --- a/include/vrpn_client_ros/vrpn_client_ros.h +++ b/include/vrpn_client_ros/vrpn_client_ros.h @@ -34,11 +34,12 @@ #include "vrpn_client_ros/vrpn_client_ros.h" -#include "ros/ros.h" -#include "geometry_msgs/PoseStamped.h" -#include "geometry_msgs/TwistStamped.h" -#include "geometry_msgs/AccelStamped.h" -#include "geometry_msgs/TransformStamped.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.h" +#include "geometry_msgs/msg/accel_stamped.h" +#include "geometry_msgs/msg/transform_stamped.h" #include #include @@ -61,13 +62,13 @@ namespace vrpn_client_ros * Create and initialize VrpnTrackerRos using an existing underlying VRPN connection object. The underlying * connection object is responsible for calling the tracker's mainloop. */ - VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, ros::NodeHandle nh); + VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, rclcpp::Node::SharedPtr nh); /** * Create and initialize VrpnTrackerRos, creating a new connection to tracker_name@host. This constructor will * register timer callbacks on nh to call mainloop. */ - VrpnTrackerRos(std::string tracker_name, std::string host, ros::NodeHandle nh); + VrpnTrackerRos(std::string tracker_name, std::string host, rclcpp::Node::SharedPtr nh); ~VrpnTrackerRos(); @@ -78,19 +79,20 @@ namespace vrpn_client_ros private: TrackerRemotePtr tracker_remote_; - std::vector pose_pubs_, twist_pubs_, accel_pubs_; - ros::NodeHandle output_nh_; - bool use_server_time_, broadcast_tf_, process_sensor_id_; + rclcpp::Publisher::SharedPtr pose_pub_; + //std::vector pose_pubs_, twist_pubs_, accel_pubs_; + rclcpp::Node::SharedPtr output_nh_; + bool use_server_time_, broadcast_tf_, mainloop_executed_; std::string tracker_name; - ros::Timer mainloop_timer; + rclcpp::TimerBase::SharedPtr mainloop_timer; - geometry_msgs::PoseStamped pose_msg_; - geometry_msgs::TwistStamped twist_msg_; - geometry_msgs::AccelStamped accel_msg_; - geometry_msgs::TransformStamped transform_stamped_; + geometry_msgs::msg::PoseStamped pose_msg_; + // geometry_msgs::TwistStamped twist_msg_; + // geometry_msgs::AccelStamped accel_msg_; + // geometry_msgs::TransformStamped transform_stamped_; - void init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer); + void init(std::string tracker_name, rclcpp::Node::SharedPtr nh, bool create_mainloop_timer); static void VRPN_CALLBACK handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose); @@ -102,16 +104,15 @@ namespace vrpn_client_ros class VrpnClientRos { public: - typedef std::shared_ptr Ptr; typedef std::unordered_map TrackerMap; /** * Create and initialize VrpnClientRos object in the private_nh namespace. */ - VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh); + VrpnClientRos(rclcpp::Node::SharedPtr nh, rclcpp::Node::SharedPtr private_nh); - static std::string getHostStringFromParams(ros::NodeHandle host_nh); + static std::string getHostStringFromParams(rclcpp::Node::SharedPtr host_nh); /** * Call mainloop of underlying VRPN connection and all registered VrpnTrackerRemote objects. @@ -125,7 +126,7 @@ namespace vrpn_client_ros private: std::string host_; - ros::NodeHandle output_nh_; + rclcpp::Node::SharedPtr output_nh_; /** * Underlying VRPN connection object @@ -137,7 +138,8 @@ namespace vrpn_client_ros */ TrackerMap trackers_; - ros::Timer refresh_tracker_timer_, mainloop_timer; + + rclcpp::TimerBase::SharedPtr refresh_tracker_timer, mainloop_timer; }; } // namespace vrpn_client_ros diff --git a/launch/sample.launch b/launch/sample.launch deleted file mode 100644 index 00f8495..0000000 --- a/launch/sample.launch +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - server: $(arg server) - port: 3883 - - update_frequency: 100.0 - frame_id: world - - # Use the VRPN server's time, or the client's ROS time. - use_server_time: false - broadcast_tf: true - - # Must either specify refresh frequency > 0.0, or a list of trackers to create - refresh_tracker_frequency: 1.0 - #trackers: - #- FirstTracker - #- SecondTracker - - - - diff --git a/launch/sample.launch.py b/launch/sample.launch.py new file mode 100644 index 0000000..57fd941 --- /dev/null +++ b/launch/sample.launch.py @@ -0,0 +1,26 @@ +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory +import launch +import launch.actions +import launch.substitutions +import launch_ros.actions + + +parameters_file_path = Path( + get_package_share_directory('vrpn_client_ros'), + 'config', + 'sample.params.yaml' +) + + +def generate_launch_description(): + return launch.LaunchDescription([ + launch_ros.actions.Node( + package='vrpn_client_ros', + node_executable='vrpn_client_node', + output='screen', + emulate_tty=True, + parameters=[parameters_file_path], + ), + ]) diff --git a/package.xml b/package.xml index 4bab952..2825a1b 100644 --- a/package.xml +++ b/package.xml @@ -1,5 +1,6 @@ - + + vrpn_client_ros 0.2.2 ROS client nodes for the VRPN library, compatible with VICON, OptiTrack, and other hardware interfaces. @@ -8,16 +9,16 @@ Paul Bovbel BSD - catkin + ament_cmake geometry_msgs - roscpp tf2_ros - vrpn + VRPN - roslaunch - roslint + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/src/vrpn_client_node.cpp b/src/vrpn_client_node.cpp index 8036124..51fe304 100644 --- a/src/vrpn_client_node.cpp +++ b/src/vrpn_client_node.cpp @@ -33,10 +33,10 @@ int main(int argc, char **argv) { - ros::init(argc, argv, "vrpn_client_node"); - ros::NodeHandle nh, private_nh("~"); - vrpn_client_ros::VrpnClientRos client(nh, private_nh); - ros::spin(); + rclcpp::init(argc, argv); + auto nh = rclcpp::Node::make_shared("vrpn_client_node"); + vrpn_client_ros::VrpnClientRos client(nh, nh); + rclcpp::spin(nh); return 0; } diff --git a/src/vrpn_client_ros.cpp b/src/vrpn_client_ros.cpp index 7a03870..332e65a 100644 --- a/src/vrpn_client_ros.cpp +++ b/src/vrpn_client_ros.cpp @@ -35,17 +35,22 @@ #include "tf2/LinearMath/Matrix3x3.h" #include "tf2_ros/transform_broadcaster.h" +#include "rclcpp/logging.hpp" + #include #include #include +#include namespace { std::unordered_set name_blacklist_({"VRPN Control"}); } + namespace vrpn_client_ros { + using namespace std::chrono_literals; /** * check Ros Names as defined here: http://wiki.ros.org/Names @@ -60,7 +65,7 @@ namespace vrpn_client_ros return ! ( isalnum(c) || c == '/' || c == '_' ); } - VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, ros::NodeHandle nh) + VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, ConnectionPtr connection, rclcpp::Node::SharedPtr nh) { tracker_remote_ = std::make_shared(tracker_name.c_str(), connection.get()); @@ -81,7 +86,7 @@ namespace vrpn_client_ros init(clean_name, nh, false); } - VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, std::string host, ros::NodeHandle nh) + VrpnTrackerRos::VrpnTrackerRos(std::string tracker_name, std::string host, rclcpp::Node::SharedPtr nh) { std::string tracker_address; tracker_address = tracker_name + "@" + host; @@ -89,276 +94,282 @@ namespace vrpn_client_ros init(tracker_name, nh, true); } - void VrpnTrackerRos::init(std::string tracker_name, ros::NodeHandle nh, bool create_mainloop_timer) + void VrpnTrackerRos::init(std::string tracker_name, rclcpp::Node::SharedPtr nh, bool create_mainloop_timer) { - ROS_INFO_STREAM("Creating new tracker " << tracker_name); + RCLCPP_INFO_STREAM(nh->get_logger(), "Creating new tracker " << tracker_name); tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_pose); - tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist); - tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel); + // tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_twist); + // tracker_remote_->register_change_handler(this, &VrpnTrackerRos::handle_accel); tracker_remote_->shutup = true; - std::string error; - if (!ros::names::validate(tracker_name, error)) - { - ROS_ERROR_STREAM("Invalid tracker name " << tracker_name << ", not creating topics : " << error); - return; - } + rclcpp::expand_topic_or_service_name( + tracker_name, + nh->get_name(), + nh->get_namespace() + ); // will throw an error if invalid this->tracker_name = tracker_name; - output_nh_ = ros::NodeHandle(nh, tracker_name); + output_nh_ = nh->create_sub_node(tracker_name); std::string frame_id; - nh.param("frame_id", frame_id, "world"); - nh.param("use_server_time", use_server_time_, false); - nh.param("broadcast_tf", broadcast_tf_, false); - nh.param("process_sensor_id", process_sensor_id_, false); + nh->get_parameter("frame_id", frame_id); + nh->get_parameter("use_server_time", use_server_time_); + nh->get_parameter("broadcast_tf", broadcast_tf_); - pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id; + pose_msg_.header.frame_id = frame_id; + // pose_msg_.header.frame_id = twist_msg_.header.frame_id = accel_msg_.header.frame_id = transform_stamped_.header.frame_id = frame_id; if (create_mainloop_timer) { double update_frequency; - nh.param("update_frequency", update_frequency, 100.0); - mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), - boost::bind(&VrpnTrackerRos::mainloop, this)); + nh->get_parameter("update_frequency", update_frequency); + mainloop_timer = nh->create_wall_timer(1s/update_frequency, std::bind(&VrpnTrackerRos::mainloop, this)); } } VrpnTrackerRos::~VrpnTrackerRos() { - ROS_INFO_STREAM("Destroying tracker " << transform_stamped_.child_frame_id); + RCLCPP_INFO_STREAM(output_nh_->get_logger(), "Destroying tracker " << tracker_name); tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_pose); - tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_twist); - tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_accel); + // tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_twist); + // tracker_remote_->unregister_change_handler(this, &VrpnTrackerRos::handle_accel); } void VrpnTrackerRos::mainloop() { tracker_remote_->mainloop(); + mainloop_executed_ = false; } void VRPN_CALLBACK VrpnTrackerRos::handle_pose(void *userData, const vrpn_TRACKERCB tracker_pose) { VrpnTrackerRos *tracker = static_cast(userData); - ros::Publisher *pose_pub; - std::size_t sensor_index(0); - ros::NodeHandle nh = tracker->output_nh_; - - if (tracker->process_sensor_id_) - { - sensor_index = static_cast(tracker_pose.sensor); - nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_pose.sensor)); + if(tracker->mainloop_executed_) { + RCLCPP_WARN_ONCE( + tracker->output_nh_->get_logger(), + "VRPN update executed multiple times for single mainloop run. Try to adjust your VRPN server settings." + ); + return; } + tracker->mainloop_executed_ = true; + + rclcpp::Node::SharedPtr nh = tracker->output_nh_; - if (tracker->pose_pubs_.size() <= sensor_index) + if (!tracker->pose_pub_) { - tracker->pose_pubs_.resize(sensor_index + 1); + tracker->pose_pub_ = nh->create_publisher("pose", 1); } - pose_pub = &(tracker->pose_pubs_[sensor_index]); - if (pose_pub->getTopic().empty()) + if (tracker->use_server_time_) { - *pose_pub = nh.advertise("pose", 1); + tracker->pose_msg_.header.stamp.sec = tracker_pose.msg_time.tv_sec; + tracker->pose_msg_.header.stamp.nanosec = tracker_pose.msg_time.tv_usec * 1000; } - - if (pose_pub->getNumSubscribers() > 0) + else { - if (tracker->use_server_time_) - { - tracker->pose_msg_.header.stamp.sec = tracker_pose.msg_time.tv_sec; - tracker->pose_msg_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000; - } - else - { - tracker->pose_msg_.header.stamp = ros::Time::now(); - } - - tracker->pose_msg_.pose.position.x = tracker_pose.pos[0]; - tracker->pose_msg_.pose.position.y = tracker_pose.pos[1]; - tracker->pose_msg_.pose.position.z = tracker_pose.pos[2]; - - tracker->pose_msg_.pose.orientation.x = tracker_pose.quat[0]; - tracker->pose_msg_.pose.orientation.y = tracker_pose.quat[1]; - tracker->pose_msg_.pose.orientation.z = tracker_pose.quat[2]; - tracker->pose_msg_.pose.orientation.w = tracker_pose.quat[3]; - - pose_pub->publish(tracker->pose_msg_); + tracker->pose_msg_.header.stamp = nh->now(); } - if (tracker->broadcast_tf_) - { - static tf2_ros::TransformBroadcaster tf_broadcaster; + tracker->pose_msg_.pose.position.x = tracker_pose.pos[0]; + tracker->pose_msg_.pose.position.y = tracker_pose.pos[1]; + tracker->pose_msg_.pose.position.z = tracker_pose.pos[2]; - if (tracker->use_server_time_) - { - tracker->transform_stamped_.header.stamp.sec = tracker_pose.msg_time.tv_sec; - tracker->transform_stamped_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000; - } - else - { - tracker->transform_stamped_.header.stamp = ros::Time::now(); - } + tracker->pose_msg_.pose.orientation.x = tracker_pose.quat[0]; + tracker->pose_msg_.pose.orientation.y = tracker_pose.quat[1]; + tracker->pose_msg_.pose.orientation.z = tracker_pose.quat[2]; + tracker->pose_msg_.pose.orientation.w = tracker_pose.quat[3]; - if (tracker->process_sensor_id_) - { - tracker->transform_stamped_.child_frame_id = tracker->tracker_name + "/" + std::to_string(tracker_pose.sensor); - } - else - { - tracker->transform_stamped_.child_frame_id = tracker->tracker_name; - } - - tracker->transform_stamped_.transform.translation.x = tracker_pose.pos[0]; - tracker->transform_stamped_.transform.translation.y = tracker_pose.pos[1]; - tracker->transform_stamped_.transform.translation.z = tracker_pose.pos[2]; - - tracker->transform_stamped_.transform.rotation.x = tracker_pose.quat[0]; - tracker->transform_stamped_.transform.rotation.y = tracker_pose.quat[1]; - tracker->transform_stamped_.transform.rotation.z = tracker_pose.quat[2]; - tracker->transform_stamped_.transform.rotation.w = tracker_pose.quat[3]; - - tf_broadcaster.sendTransform(tracker->transform_stamped_); - } - } - - void VRPN_CALLBACK VrpnTrackerRos::handle_twist(void *userData, const vrpn_TRACKERVELCB tracker_twist) - { - VrpnTrackerRos *tracker = static_cast(userData); - - ros::Publisher *twist_pub; - std::size_t sensor_index(0); - ros::NodeHandle nh = tracker->output_nh_; + tracker->pose_pub_->publish(tracker->pose_msg_); - if (tracker->process_sensor_id_) - { - sensor_index = static_cast(tracker_twist.sensor); - nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_twist.sensor)); - } - - if (tracker->twist_pubs_.size() <= sensor_index) - { - tracker->twist_pubs_.resize(sensor_index + 1); - } - twist_pub = &(tracker->twist_pubs_[sensor_index]); - - if (twist_pub->getTopic().empty()) - { - *twist_pub = nh.advertise("twist", 1); - } - - if (twist_pub->getNumSubscribers() > 0) - { - if (tracker->use_server_time_) - { - tracker->twist_msg_.header.stamp.sec = tracker_twist.msg_time.tv_sec; - tracker->twist_msg_.header.stamp.nsec = tracker_twist.msg_time.tv_usec * 1000; - } - else - { - tracker->twist_msg_.header.stamp = ros::Time::now(); - } - - tracker->twist_msg_.twist.linear.x = tracker_twist.vel[0]; - tracker->twist_msg_.twist.linear.y = tracker_twist.vel[1]; - tracker->twist_msg_.twist.linear.z = tracker_twist.vel[2]; - - double roll, pitch, yaw; - tf2::Matrix3x3 rot_mat( - tf2::Quaternion(tracker_twist.vel_quat[0], tracker_twist.vel_quat[1], tracker_twist.vel_quat[2], - tracker_twist.vel_quat[3])); - rot_mat.getRPY(roll, pitch, yaw); - tracker->twist_msg_.twist.angular.x = roll; - tracker->twist_msg_.twist.angular.y = pitch; - tracker->twist_msg_.twist.angular.z = yaw; - - twist_pub->publish(tracker->twist_msg_); - } + // if (tracker->broadcast_tf_) + // { + // static tf2_ros::TransformBroadcaster tf_broadcaster; + + // if (tracker->use_server_time_) + // { + // tracker->transform_stamped_.header.stamp.sec = tracker_pose.msg_time.tv_sec; + // tracker->transform_stamped_.header.stamp.nsec = tracker_pose.msg_time.tv_usec * 1000; + // } + // else + // { + // tracker->transform_stamped_.header.stamp = ros::Time::now(); + // } + + // if (tracker->process_sensor_id_) + // { + // tracker->transform_stamped_.child_frame_id = tracker->tracker_name + "/" + std::to_string(tracker_pose.sensor); + // } + // else + // { + // tracker->transform_stamped_.child_frame_id = tracker->tracker_name; + // } + + // tracker->transform_stamped_.transform.translation.x = tracker_pose.pos[0]; + // tracker->transform_stamped_.transform.translation.y = tracker_pose.pos[1]; + // tracker->transform_stamped_.transform.translation.z = tracker_pose.pos[2]; + + // tracker->transform_stamped_.transform.rotation.x = tracker_pose.quat[0]; + // tracker->transform_stamped_.transform.rotation.y = tracker_pose.quat[1]; + // tracker->transform_stamped_.transform.rotation.z = tracker_pose.quat[2]; + // tracker->transform_stamped_.transform.rotation.w = tracker_pose.quat[3]; + + // tf_broadcaster.sendTransform(tracker->transform_stamped_); + // } } - void VRPN_CALLBACK VrpnTrackerRos::handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel) + // void VRPN_CALLBACK VrpnTrackerRos::handle_twist(void *userData, const vrpn_TRACKERVELCB tracker_twist) + // { + // VrpnTrackerRos *tracker = static_cast(userData); + + // ros::Publisher *twist_pub; + // std::size_t sensor_index(0); + // rclcpp::Node::SharedPtr nh = tracker->output_nh_; + // + // if (tracker->process_sensor_id_) + // { + // sensor_index = static_cast(tracker_twist.sensor); + // nh = rclcpp::Node::SharedPtr(tracker->output_nh_, std::to_string(tracker_twist.sensor)); + // } + // + // if (tracker->twist_pubs_.size() <= sensor_index) + // { + // tracker->twist_pubs_.resize(sensor_index + 1); + // } + // twist_pub = &(tracker->twist_pubs_[sensor_index]); + + // if (twist_pub->getTopic().empty()) + // { + // *twist_pub = nh.advertise("twist", 1); + // } + + // if (twist_pub->getNumSubscribers() > 0) + // { + // if (tracker->use_server_time_) + // { + // tracker->twist_msg_.header.stamp.sec = tracker_twist.msg_time.tv_sec; + // tracker->twist_msg_.header.stamp.nsec = tracker_twist.msg_time.tv_usec * 1000; + // } + // else + // { + // tracker->twist_msg_.header.stamp = ros::Time::now(); + // } + + // tracker->twist_msg_.twist.linear.x = tracker_twist.vel[0]; + // tracker->twist_msg_.twist.linear.y = tracker_twist.vel[1]; + // tracker->twist_msg_.twist.linear.z = tracker_twist.vel[2]; + + // double roll, pitch, yaw; + // tf2::Matrix3x3 rot_mat( + // tf2::Quaternion(tracker_twist.vel_quat[0], tracker_twist.vel_quat[1], tracker_twist.vel_quat[2], + // tracker_twist.vel_quat[3])); + // rot_mat.getRPY(roll, pitch, yaw); + + // tracker->twist_msg_.twist.angular.x = roll; + // tracker->twist_msg_.twist.angular.y = pitch; + // tracker->twist_msg_.twist.angular.z = yaw; + + // twist_pub->publish(tracker->twist_msg_); + // } + // } + + // void VRPN_CALLBACK VrpnTrackerRos::handle_accel(void *userData, const vrpn_TRACKERACCCB tracker_accel) + // { + // VrpnTrackerRos *tracker = static_cast(userData); + + // ros::Publisher *accel_pub; + // std::size_t sensor_index(0); + // rclcpp::Node::SharedPtr nh = tracker->output_nh_; + + // if (tracker->process_sensor_id_) + // { + // sensor_index = static_cast(tracker_accel.sensor); + // nh = rclcpp::Node::SharedPtr(tracker->output_nh_, std::to_string(tracker_accel.sensor)); + // } + // + // if (tracker->accel_pubs_.size() <= sensor_index) + // { + // tracker->accel_pubs_.resize(sensor_index + 1); + // } + // accel_pub = &(tracker->accel_pubs_[sensor_index]); + + // if (accel_pub->getTopic().empty()) + // { + // *accel_pub = nh.advertise("accel", 1); + // } + + // if (accel_pub->getNumSubscribers() > 0) + // { + // if (tracker->use_server_time_) + // { + // tracker->accel_msg_.header.stamp.sec = tracker_accel.msg_time.tv_sec; + // tracker->accel_msg_.header.stamp.nsec = tracker_accel.msg_time.tv_usec * 1000; + // } + // else + // { + // tracker->accel_msg_.header.stamp = ros::Time::now(); + // } + + // tracker->accel_msg_.accel.linear.x = tracker_accel.acc[0]; + // tracker->accel_msg_.accel.linear.y = tracker_accel.acc[1]; + // tracker->accel_msg_.accel.linear.z = tracker_accel.acc[2]; + + // double roll, pitch, yaw; + // tf2::Matrix3x3 rot_mat( + // tf2::Quaternion(tracker_accel.acc_quat[0], tracker_accel.acc_quat[1], tracker_accel.acc_quat[2], + // tracker_accel.acc_quat[3])); + // rot_mat.getRPY(roll, pitch, yaw); + + // tracker->accel_msg_.accel.angular.x = roll; + // tracker->accel_msg_.accel.angular.y = pitch; + // tracker->accel_msg_.accel.angular.z = yaw; + + // accel_pub->publish(tracker->accel_msg_); + // } + // } + + VrpnClientRos::VrpnClientRos(rclcpp::Node::SharedPtr nh, rclcpp::Node::SharedPtr private_nh) { - VrpnTrackerRos *tracker = static_cast(userData); - - ros::Publisher *accel_pub; - std::size_t sensor_index(0); - ros::NodeHandle nh = tracker->output_nh_; - - if (tracker->process_sensor_id_) - { - sensor_index = static_cast(tracker_accel.sensor); - nh = ros::NodeHandle(tracker->output_nh_, std::to_string(tracker_accel.sensor)); - } - - if (tracker->accel_pubs_.size() <= sensor_index) - { - tracker->accel_pubs_.resize(sensor_index + 1); - } - accel_pub = &(tracker->accel_pubs_[sensor_index]); - - if (accel_pub->getTopic().empty()) - { - *accel_pub = nh.advertise("accel", 1); - } - - if (accel_pub->getNumSubscribers() > 0) - { - if (tracker->use_server_time_) - { - tracker->accel_msg_.header.stamp.sec = tracker_accel.msg_time.tv_sec; - tracker->accel_msg_.header.stamp.nsec = tracker_accel.msg_time.tv_usec * 1000; - } - else - { - tracker->accel_msg_.header.stamp = ros::Time::now(); - } - - tracker->accel_msg_.accel.linear.x = tracker_accel.acc[0]; - tracker->accel_msg_.accel.linear.y = tracker_accel.acc[1]; - tracker->accel_msg_.accel.linear.z = tracker_accel.acc[2]; + output_nh_ = private_nh; - double roll, pitch, yaw; - tf2::Matrix3x3 rot_mat( - tf2::Quaternion(tracker_accel.acc_quat[0], tracker_accel.acc_quat[1], tracker_accel.acc_quat[2], - tracker_accel.acc_quat[3])); - rot_mat.getRPY(roll, pitch, yaw); + nh->declare_parameter("server", "localhost"); + nh->declare_parameter("port", 3883); + nh->declare_parameter("update_frequency", 100.0); + nh->declare_parameter("frame_id", "world"); + nh->declare_parameter("use_server_time", false); + //nh->declare_parameter("broadcast_tf", true); + nh->declare_parameter("refresh_tracker_frequency", 1.0); - tracker->accel_msg_.accel.angular.x = roll; - tracker->accel_msg_.accel.angular.y = pitch; - tracker->accel_msg_.accel.angular.z = yaw; + std::vector param_tracker_names; + nh->declare_parameter("trackers", param_tracker_names); - accel_pub->publish(tracker->accel_msg_); - } - } - - VrpnClientRos::VrpnClientRos(ros::NodeHandle nh, ros::NodeHandle private_nh) - { - output_nh_ = private_nh; host_ = getHostStringFromParams(private_nh); - ROS_INFO_STREAM("Connecting to VRPN server at " << host_); + RCLCPP_INFO_STREAM(output_nh_->get_logger(), "Connecting to VRPN server at " << host_); connection_ = std::shared_ptr(vrpn_get_connection_by_name(host_.c_str())); - ROS_INFO("Connection established"); + RCLCPP_INFO(output_nh_->get_logger(), "Connection established"); double update_frequency; - private_nh.param("update_frequency", update_frequency, 100.0); - mainloop_timer = nh.createTimer(ros::Duration(1 / update_frequency), boost::bind(&VrpnClientRos::mainloop, this)); + private_nh->get_parameter("update_frequency", update_frequency); + + + mainloop_timer = nh->create_wall_timer(1s/update_frequency, std::bind(&VrpnClientRos::mainloop, this)); double refresh_tracker_frequency; - private_nh.param("refresh_tracker_frequency", refresh_tracker_frequency, 0.0); + private_nh->get_parameter("refresh_tracker_frequency", refresh_tracker_frequency); if (refresh_tracker_frequency > 0.0) { - refresh_tracker_timer_ = nh.createTimer(ros::Duration(1 / refresh_tracker_frequency), - boost::bind(&VrpnClientRos::updateTrackers, this)); + refresh_tracker_timer = nh->create_wall_timer(1s/refresh_tracker_frequency, + std::bind(&VrpnClientRos::updateTrackers, this)); } std::vector param_tracker_names_; - if (private_nh.getParam("trackers", param_tracker_names_)) + if (private_nh->get_parameter("trackers", param_tracker_names_)) { for (std::vector::iterator it = param_tracker_names_.begin(); it != param_tracker_names_.end(); ++it) @@ -368,16 +379,17 @@ namespace vrpn_client_ros } } - std::string VrpnClientRos::getHostStringFromParams(ros::NodeHandle host_nh) + std::string VrpnClientRos::getHostStringFromParams(rclcpp::Node::SharedPtr host_nh) { std::stringstream host_stream; std::string server; int port; - host_nh.param("server", server, "localhost"); + + host_nh->get_parameter("server", server); host_stream << server; - if (host_nh.getParam("port", port)) + if (host_nh->get_parameter("port", port)) { host_stream << ":" << port; } @@ -389,7 +401,7 @@ namespace vrpn_client_ros connection_->mainloop(); if (!connection_->doing_okay()) { - ROS_WARN("VRPN connection is not 'doing okay'"); + RCLCPP_WARN(output_nh_->get_logger(), "VRPN connection is not 'doing okay'"); } for (TrackerMap::iterator it = trackers_.begin(); it != trackers_.end(); ++it) { @@ -404,7 +416,7 @@ namespace vrpn_client_ros { if (trackers_.count(connection_->sender_name(i)) == 0 && name_blacklist_.count(connection_->sender_name(i)) == 0) { - ROS_INFO_STREAM("Found new sender: " << connection_->sender_name(i)); + RCLCPP_INFO_STREAM(output_nh_->get_logger(), "Found new sender: " << connection_->sender_name(i)); trackers_.insert(std::make_pair(connection_->sender_name(i), std::make_shared(connection_->sender_name(i), connection_, output_nh_))); diff --git a/src/vrpn_tracker_node.cpp b/src/vrpn_tracker_node.cpp index c151a8e..8c55d67 100644 --- a/src/vrpn_tracker_node.cpp +++ b/src/vrpn_tracker_node.cpp @@ -35,18 +35,21 @@ int main(int argc, char **argv) { - ros::init(argc, argv, "vrpn_tracker_node"); - ros::NodeHandle nh, private_nh("~"); + rclcpp::init(argc, argv); + auto nh = rclcpp::Node::make_shared("vrpn_tracker_node"); + auto private_nh = rclcpp::Node::make_shared("vrpn_tracker_node_private"); - std::string tracker_name; - if (!private_nh.getParam("tracker_name", tracker_name)) + //rclcpp::Node::SharedPtr nh, private_nh("~"); + + std::string tracker_name = "Test tracker"; + if (!private_nh->get_parameter("tracker_name", tracker_name)) { - ROS_FATAL_STREAM("Must provide paramter tracker_name for node " << private_nh.getNamespace()); + RCLCPP_FATAL_STREAM(nh->get_logger(), "Must provide paramter tracker_name for node " << private_nh->get_namespace()); } std::string host = vrpn_client_ros::VrpnClientRos::getHostStringFromParams(private_nh); vrpn_client_ros::VrpnTrackerRos tracker(tracker_name, host, nh); - ros::spin(); + rclcpp::spin(nh); return 0; }