diff --git a/.gitignore b/.gitignore index 35d74bb..378eac2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,51 +1 @@ -devel/ -logs/ -build/ -bin/ -lib/ -msg_gen/ -srv_gen/ -msg/*Action.msg -msg/*ActionFeedback.msg -msg/*ActionGoal.msg -msg/*ActionResult.msg -msg/*Feedback.msg -msg/*Goal.msg -msg/*Result.msg -msg/_*.py -build_isolated/ -devel_isolated/ - -# Generated by dynamic reconfigure -*.cfgc -/cfg/cpp/ -/cfg/*.py - -# Ignore generated docs -*.dox -*.wikidoc - -# eclipse stuff -.project -.cproject - -# qcreator stuff -CMakeLists.txt.user - -srv/_*.py -*.pcd -*.pyc -qtcreator-* -*.user - -/planning/cfg -/planning/docs -/planning/src - -*~ - -# Emacs -.#* - -# Catkin custom files -CATKIN_IGNORE +build diff --git a/README.md b/README.md index 4969cf5..9e8d0d2 100644 --- a/README.md +++ b/README.md @@ -59,9 +59,9 @@ Exit the program with CTRL+C. ### Information on ROS 2 topics and messages -The **ros2-vicon-receiver** package creates a topic for each segment in each subject with the pattern `namespace/subject_name/segment_name`. Information is published on the topics as soon as new data is available from the vicon client (typically at the vicon client frequency). The message type [Position](vicon_receiver/msg/Position.msg) is used. +The **ros2-vicon-receiver** package creates a topic for each segment in each subject with the pattern `namespace/subject_name/segment_name`. Information is published on the topics as soon as new data is available from the vicon client (typically at the vicon client frequency). The message type [PoseStamped](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseStamped.html) is used. -Example: suppose your namespace is the default `vicon` and you have two subjects (`subject_1` and `subject_2`) with two segments each (`segment_1` and `segment_2`). Then **ros2-vicon-receiver** will publish [Position](vicon_receiver/msg/Position.msg) messages on the following topics: +Example: suppose your namespace is the default `vicon` and you have two subjects (`subject_1` and `subject_2`) with two segments each (`segment_1` and `segment_2`). Then **ros2-vicon-receiver** will publish [PoseStamped](https://docs.ros2.org/latest/api/geometry_msgs/msg/PoseStamped.html) messages on the following topics: ``` vicon/subject_1/segment_1 vicon/subject_1/segment_2 @@ -72,8 +72,9 @@ vicon/subject_2/segment_2 ## Constributors **ros2-vicon-receiver** is developed by [Andrea Camisa](https://www.unibo.it/sitoweb/a.camisa), -[Andrea Testa](https://www.unibo.it/sitoweb/a.testa) and -[Giuseppe Notarstefano](https://www.unibo.it/sitoweb/giuseppe.notarstefano) +[Andrea Testa](https://www.unibo.it/sitoweb/a.testa), +[Giuseppe Notarstefano](https://www.unibo.it/sitoweb/giuseppe.notarstefano), +[Michael Mugnai](https://maik93.github.io/) ## Acknowledgements This result is part of a project that has received funding from the European Research Council (ERC) under the European Union's Horizon 2020 research and innovation programme (grant agreement No 638992 - OPT4SMART). diff --git a/vicon_receiver/CMakeLists.txt b/vicon_receiver/CMakeLists.txt index bd1fccb..c295a51 100644 --- a/vicon_receiver/CMakeLists.txt +++ b/vicon_receiver/CMakeLists.txt @@ -6,9 +6,10 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) + set(CMAKE_CXX_STANDARD_REQUIRED ON) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -22,29 +23,13 @@ include_directories( # find dependencies find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Boost REQUIRED COMPONENTS thread) -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Position.msg" -) - -ament_export_dependencies(rosidl_default_runtime) - add_executable(vicon_client src/vicon_receiver/communicator.cpp src/vicon_receiver/publisher.cpp) target_link_libraries(vicon_client ViconDataStreamSDK_CPP ${Boost_LIBRARIES}) -ament_target_dependencies(vicon_client "rclcpp") - -get_default_rmw_implementation(rmw_implementation) -find_package("${rmw_implementation}" REQUIRED) -get_rmw_typesupport(typesupport_impls "${rmw_implementation}" LANGUAGE "cpp") - -foreach(typesupport_impl ${typesupport_impls}) - rosidl_target_interfaces(vicon_client - ${PROJECT_NAME} ${typesupport_impl} - ) -endforeach() +ament_target_dependencies(vicon_client rclcpp geometry_msgs) install(TARGETS vicon_client DESTINATION lib/${PROJECT_NAME}) diff --git a/vicon_receiver/include/vicon_receiver/publisher.hpp b/vicon_receiver/include/vicon_receiver/publisher.hpp index ee28f6d..0cb1a27 100644 --- a/vicon_receiver/include/vicon_receiver/publisher.hpp +++ b/vicon_receiver/include/vicon_receiver/publisher.hpp @@ -1,8 +1,9 @@ #ifndef PUBLISHER_HPP #define PUBLISHER_HPP + #include #include "rclcpp/rclcpp.hpp" -#include "vicon_receiver/msg/position.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" // Struct used to hold segment data to transmit to the Publisher class. struct PositionStruct @@ -21,7 +22,7 @@ struct PositionStruct class Publisher { private: - rclcpp::Publisher::SharedPtr position_publisher_; + rclcpp::Publisher::SharedPtr pose_publisher_; public: bool is_ready = false; @@ -33,4 +34,4 @@ class Publisher void publish(PositionStruct p); }; -#endif \ No newline at end of file +#endif diff --git a/vicon_receiver/msg/Position.msg b/vicon_receiver/msg/Position.msg deleted file mode 100644 index 0afc677..0000000 --- a/vicon_receiver/msg/Position.msg +++ /dev/null @@ -1,11 +0,0 @@ -float32 x_trans -float32 y_trans -float32 z_trans -float32 x_rot #first element of the quaternion -float32 y_rot #second element of the quaternion -float32 z_rot #third element of the quaternion -float32 w #forth element of the quaternion -string segment_name #name of a specific component of the object -string subject_name #name of the entire object -int32 frame_number #unit of time for each capture -string translation_type #Local or Global diff --git a/vicon_receiver/package.xml b/vicon_receiver/package.xml index 6f4b753..b0c32aa 100644 --- a/vicon_receiver/package.xml +++ b/vicon_receiver/package.xml @@ -6,16 +6,8 @@ ROS2 package to retrieve data from a Vicon system OPT4SMART group GNU General Public License v3 - - rosidl_interface_packages - rosidl_default_generators - - rosidl_default_runtime - - rclcpp - - rclcpp + rclcpp ament_cmake diff --git a/vicon_receiver/src/vicon_receiver/publisher.cpp b/vicon_receiver/src/vicon_receiver/publisher.cpp index b50111c..a9791f7 100644 --- a/vicon_receiver/src/vicon_receiver/publisher.cpp +++ b/vicon_receiver/src/vicon_receiver/publisher.cpp @@ -2,23 +2,21 @@ Publisher::Publisher(std::string topic_name, rclcpp::Node* node) { - position_publisher_ = node->create_publisher(topic_name, 10); + pose_publisher_ = node->create_publisher(topic_name, 10); is_ready = true; } void Publisher::publish(PositionStruct p) { - auto msg = std::make_shared(); - msg->x_trans = p.translation[0]; - msg->y_trans = p.translation[1]; - msg->z_trans = p.translation[2]; - msg->x_rot = p.rotation[0]; - msg->y_rot = p.rotation[1]; - msg->z_rot = p.rotation[2]; - msg->w = p.rotation[3]; - msg->subject_name = p.subject_name; - msg->segment_name = p.segment_name; - msg->frame_number = p.frame_number; - msg->translation_type = p.translation_type; - position_publisher_->publish(*msg); + auto msg = geometry_msgs::msg::PoseStamped(); + msg.header.stamp = rclcpp::Clock().now(); + msg.header.frame_id = "vicon"; + msg.pose.position.x = p.translation[0]; + msg.pose.position.y = p.translation[1]; + msg.pose.position.z = p.translation[2]; + msg.pose.orientation.x = p.rotation[0]; + msg.pose.orientation.y = p.rotation[1]; + msg.pose.orientation.z = p.rotation[2]; + msg.pose.orientation.w = p.rotation[3]; + pose_publisher_->publish(msg); }