switch from custom message to a PoseStamped

.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

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).

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})
 

vicon_receiver/include/vicon_receiver/publisher.hpp

@@ -1,8 +1,9 @@
 #ifndef PUBLISHER_HPP
 #define PUBLISHER_HPP
+
 #include <unistd.h>
 #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<vicon_receiver::msg::Position>::SharedPtr position_publisher_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
 
 public:
     bool is_ready = false;
@@ -33,4 +34,4 @@ class Publisher
     void publish(PositionStruct p);
 };
 
-#endif
+#endif

vicon_receiver/package.xml

@@ -6,16 +6,8 @@
   <description>ROS2 package to retrieve data from a Vicon system</description>
   <maintainer email="info@opt4smart.eu">OPT4SMART group</maintainer>
   <license>GNU General Public License v3</license>
-
-  <member_of_group>rosidl_interface_packages</member_of_group>
 
-  <buildtool_depend>rosidl_default_generators</buildtool_depend>
-
-  <exec_depend>rosidl_default_runtime</exec_depend>
-
-  <build_depend>rclcpp</build_depend>
-
-  <exec_depend>rclcpp</exec_depend>
+  <depend>rclcpp</depend>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

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<vicon_receiver::msg::Position>(topic_name, 10);
+    pose_publisher_ = node->create_publisher<geometry_msgs::msg::PoseStamped>(topic_name, 10);
     is_ready = true;
 }
 
 void Publisher::publish(PositionStruct p)
 {
-    auto msg = std::make_shared<vicon_receiver::msg::Position>();
-    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);
 }