add Ros2 items so I can merge new fp changes part 2

fixposition_odometry_converter_ros2/CMakeLists.txt

@@ -0,0 +1,52 @@
+# GENERAL ==============================================================================================================
+cmake_minimum_required(VERSION 3.10.2)
+project(fixposition_odometry_converter_ros2)
+set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Wall -Wextra -Wpedantic -Wno-unused-parameter")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3")
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+# DEPENDENCIES =========================================================================================================
+set(PACKAGE_INCLUDE_DEPENDS
+  rclcpp
+  rclcpp_components
+  nav_msgs
+  geometry_msgs
+  fixposition_driver_ros2
+)
+
+find_package(ament_cmake REQUIRED)
+
+foreach(Dependency IN ITEMS ${PACKAGE_INCLUDE_DEPENDS})
+  find_package(${Dependency} REQUIRED)
+endforeach()
+
+include_directories(
+  include
+)
+
+# BUILD EXECUTABLE =====================================================================================================
+add_library(odom_converter SHARED 
+  src/params.cpp
+  src/odom_converter_node.cpp)
+ament_target_dependencies(odom_converter ${PACKAGE_INCLUDE_DEPENDS})
+rclcpp_components_register_nodes(odom_converter "fixposition::OdomConverterNode")
+rclcpp_components_register_node(odom_converter
+  PLUGIN "fixposition::OdomConverterNode"
+  EXECUTABLE odom_converter_node
+)
+
+# INSTALL ==============================================================================================================
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+
+install(
+  TARGETS odom_converter
+  LIBRARY DESTINATION lib
+)
+
+install(DIRECTORY include
+  DESTINATION .
+)
+
+ament_package()

fixposition_odometry_converter_ros2/README.md

@@ -0,0 +1,22 @@
+
+# Fixposition Odometry Converter
+
+## Description
+
+An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. The x component of the velocity in the input messages is then extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the VRTK2. If the param `use_angular` is selected, the z component of the angular velocity (yaw rate) of the input messages is also extracted and placed in the speed vector.
+
+_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one or two values, i.e. the total vehicle speed or the total vehicle speed and yaw rate. It also assumes that the x axis of the odometry output and the VRTK2 axis are aligned. For situations where all 4 inputs are desired, a custom converter is necessary._
+
+## Input Parameters
+
+The `odom_converter.yaml` file exposes the necessary parameters for the correct operation of the node. The parameter that may cause the most doubt is the `multiplicative_factor`. This should be chosen such that the inputed float velocity value is transformed into milimeters per second, e.g. 1000 for an input that is expressed in meters per second.
+
+## Launch
+
+After the configuration is set, to launch the node simply run:
+
+   `ros2 launch fixposition_odometry_converter_ros2 odom_converter.launch`
+
+# License
+
+This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details

fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp

@@ -0,0 +1,89 @@
+/**
+ *  @file
+ *  @brief Declaration of OdomConverterNode class
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ *
+ * Port to ROS 2 by Husarion
+ * \endverbatim
+ *
+ */
+
+#ifndef __ODOM_CONVERTER_NODE_HPP__
+#define __ODOM_CONVERTER_NODE_HPP__
+
+/* ROS */
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_with_covariance.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+/* PACKAGE */
+#include <fixposition_driver_ros2/msg/speed.hpp>
+#include <fixposition_odometry_converter_ros2/params.hpp>
+
+namespace fixposition {
+
+class OdomConverterNode : public rclcpp::Node {
+   public:
+    /**
+     * @brief Construct a new OdomConverterNode object
+     *
+     * @param[in] options node options
+     */
+    OdomConverterNode(const rclcpp::NodeOptions& options);
+
+    /**
+     * @brief Destroy the OdomConverterNode object
+     *
+     */
+    ~OdomConverterNode() = default;
+
+    /**
+     * @brief Subscribes to the correct topic depending on the parameters
+     *
+     */
+    void Subscribe();
+
+    /**
+     * @brief Converts and publishes the speed to an integer value in [mm/s]
+     *
+     * @param speed
+     * @param angular
+     * @param use_angular default false
+     */
+    void ConvertAndPublish(const double speed, const double angular, bool use_angular = false);
+
+   private:
+    /**
+     * @brief Converts a message of type TwistWithCovariance to the fixposition_driver Speed message and publishes it
+     *
+     * @param[in] msg
+     */
+    void TwistWithCovCallback(const geometry_msgs::msg::TwistWithCovariance::SharedPtr msg);
+
+    /**
+     * @brief Converts a message of type Odometry to the fixposition_driver Speed message and publishes it
+     *
+     * @param[in] msg
+     */
+    void OdometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
+
+    /**
+     * @brief Converts a message of type Twist to the fixposition_driver Speed message and publishes it
+     *
+     * @param[in] msg
+     */
+    void TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
+
+    OdomInputParams params_;
+    rclcpp::SubscriptionBase::SharedPtr ws_sub_;
+    rclcpp::Publisher<fixposition_driver_ros2::msg::Speed>::SharedPtr ws_pub_;
+};
+}  // namespace fixposition
+#endif  //__ODOM_CONVERTER_NODE_HPP__

fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp

@@ -0,0 +1,50 @@
+/**
+ *  @file
+ *  @brief Parameters for the odometry converter
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ * 
+ * Port to ROS 2 by Husarion
+ * \endverbatim
+ *
+ */
+
+#ifndef __ODOM_CONVERTER_PARAMS_HPP__
+#define __ODOM_CONVERTER_PARAMS_HPP__
+
+/* SYSTEM / STL */
+#include <string>
+
+/* ROS */
+#include <rclcpp/rclcpp.hpp>
+
+namespace fixposition {
+
+enum class VelTopicType : int8_t { Twist = 0, TwistWithCov = 1, Odometry = 2 };
+
+struct OdomInputParams {
+    VelTopicType topic_type;
+    std::string input_topic;
+    std::string fixposition_speed_topic;
+    int multiplicative_factor;
+    bool use_angular;
+    /**
+     * @brief Load all parameters from ROS 2
+     *
+     * @param[in] param_itf parameter interface
+     * @param[in] logging_itf logging interface
+     * @return true success
+     * @return false fail
+     */
+    bool LoadFromRos(const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr& param_itf,
+                     const rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr& logging_itf);
+};
+
+}  // namespace fixposition
+
+#endif

fixposition_odometry_converter_ros2/launch/odom_converter.launch

@@ -0,0 +1,5 @@
+<launch>
+  <node name="odom_converter_node" pkg="fixposition_odometry_converter_ros2" exec="odom_converter_node" output="screen">
+    <param from="$(find-pkg-share fixposition_odometry_converter_ros2)/launch/odom_converter.yaml" />
+  </node>
+</launch>

fixposition_odometry_converter_ros2/launch/odom_converter.yaml

@@ -0,0 +1,7 @@
+/**:
+  ros__parameters:
+    topic_type: "Odometry" # Twist, TwistWithCov or Odometry
+    input_topic: "/robot/odom" # Original topic name
+    fixposition_speed_topic: "/fixposition/speed" # Fixposition topic name, to be subscribed by the fixposition_driver
+    multiplicative_factor: 1000 # To convert from the original unit of measurement to mm/s
+    use_angular: false # If the angular velocity of the speed message is filled

fixposition_odometry_converter_ros2/package.xml

@@ -0,0 +1,19 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>fixposition_odometry_converter_ros2</name>
+  <version>1.0.0</version>
+  <description>Conversion from Odometry messages to Fixposition's Speed input for ROS 2</description>
+  <maintainer email="[email protected]">Fixposition AG</maintainer>
+  <license>MIT</license>
+  <author email="[email protected]">Maciej Stępień</author>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>fixposition_driver_ros2</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

fixposition_odometry_converter_ros2/src/odom_converter_node.cpp

@@ -0,0 +1,83 @@
+/**
+ *  @file
+ *  @brief Implementation of OdomConverterNode class
+ *
+ * \verbatim
+ *  ___    ___
+ *  \  \  /  /
+ *   \  \/  /   Fixposition AG
+ *   /  /\  \   All right reserved.
+ *  /__/  \__\
+ *
+ * Port to ROS 2 by Husarion
+ * \endverbatim
+ *
+ */
+
+/* PACKAGE */
+#include <fixposition_odometry_converter_ros2/odom_converter_node.hpp>
+
+namespace fixposition {
+
+OdomConverterNode::OdomConverterNode(const rclcpp::NodeOptions& options) : Node("odom_converter", options) {
+    // read parameters
+    if (!params_.LoadFromRos(this->get_node_parameters_interface(), this->get_node_logging_interface())) {
+        RCLCPP_ERROR(this->get_logger(), "Parameter Loading failed, shutting down...");
+        rclcpp::shutdown();
+    }
+
+    // initialize the subscriber
+    Subscribe();
+
+    // initialize the publisher
+    ws_pub_ = this->create_publisher<fixposition_driver_ros2::msg::Speed>(params_.fixposition_speed_topic, 10);
+}
+
+void OdomConverterNode::Subscribe() {
+    switch (params_.topic_type) {
+        case VelTopicType::Twist:
+            ws_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
+                params_.input_topic, 10, std::bind(&OdomConverterNode::TwistCallback, this, std::placeholders::_1));
+            break;
+        case VelTopicType::TwistWithCov:
+            ws_sub_ = this->create_subscription<geometry_msgs::msg::TwistWithCovariance>(
+                params_.input_topic, 10,
+                std::bind(&OdomConverterNode::TwistWithCovCallback, this, std::placeholders::_1));
+            break;
+        case VelTopicType::Odometry:
+            ws_sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+                params_.input_topic, 10, std::bind(&OdomConverterNode::OdometryCallback, this, std::placeholders::_1));
+            break;
+    }
+}
+
+void OdomConverterNode::ConvertAndPublish(const double speed, const double angular, bool use_angular) {
+    if (ws_pub_->get_subscription_count() > 0) {
+        const int int_speed = round(speed * params_.multiplicative_factor);
+        const int angular_speed = round(angular * params_.multiplicative_factor);
+        fixposition_driver_ros2::msg::Speed msg;
+        msg.speeds.push_back(int_speed);
+        if (params_.use_angular) {
+            msg.speeds.push_back(angular_speed);
+        }
+        ws_pub_->publish(msg);
+    }
+}
+
+void OdomConverterNode::TwistWithCovCallback(const geometry_msgs::msg::TwistWithCovariance::SharedPtr msg) {
+    ConvertAndPublish(msg->twist.linear.x, msg->twist.angular.z, params_.use_angular);
+}
+
+void OdomConverterNode::OdometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+    ConvertAndPublish(msg->twist.twist.linear.x, msg->twist.twist.angular.z, params_.use_angular);
+}
+
+void OdomConverterNode::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) {
+    ConvertAndPublish(msg->linear.x, msg->angular.z, params_.use_angular);
+}
+
+}  // namespace fixposition
+
+// Register the component with class_loader
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(fixposition::OdomConverterNode)