Get rid of undeclared parameters as per ROS 2.0 guidelines, update th…

…e yaml parameter file descriptions accordingly

param/xsens_mti_node.yaml

@@ -2,20 +2,27 @@
     ros__parameters:
 
         ## Device settings, provide one of the following:
-        ##  - device_id
-        ##  - port + baudrate information
-        ##  - nothing: the driver scans for devices and selects the first found.
-        # device_id: '077007EF' # uppercase hex string
-        # port: '/dev/ttyUSB0'
-        # baudrate: 115200 # non necessary for some devices
+        ##  - scan_for devices = 
+        ##          true:   the driver ignores port and baudrate settings, scans for devices on all ports and selects the first found
+        ##          false: = Requires correct port and baudrate both to be specified
+        scan_for_devices: true
+        port: "" # port name, e.g. '/dev/ttyUSB0'
+        baudrate: 115200 # non necessary for some devices
 
+        ## Connect only to specific device_id:
+        ##  device_id = '077007EF' (uppercase hex string), returns with error if the device with ID is not found on the ports
+        ##  device_id = "" Null string disables this check
+        device_id: "" # Null String ("") by default, else uppercase hex string, e.g. '077007EF'
+
+        # Enable logging. Requires name of the file to be specified.
+        enable_logging: false
         ## Log file (optional), placed in ~/.ros/ otherwise use absolute path
-        # log_file: log.mtb
+        log_file: log.mtb
 
         publisher_queue_size: 5
 
         # TF transform frame_id (default: imu_link), you may want to change it if you use multiple devices
-        #frame_id: "imu_link"
+        frame_id: "imu_link"
 
         # Message publishers
         pub_imu: true
@@ -38,7 +45,8 @@
         ## Sensor standard deviation [x,y,z] (optional)
         ## This value is used to override the covariance matrix in sensor_msgs/Imu and
         ## sensor_msgs/MagneticField messages.
-        # linear_acceleration_stddev: [0, 0, 0] # [m/s^2]
-        # angular_velocity_stddev: [0, 0, 0] # [rad/s]
-        # orientation_stddev: [0, 0, 0] # [rad]
-        # magnetic_field_stddev: [0, 0, 0] # [Tesla]
+        ## Important: Make sure the values provided are in decimal points, or else might give error in execution
+        linear_acceleration_stddev: [0.0, 0.0, 0.0] # [m/s^2]
+        angular_velocity_stddev: [0.0, 0.0, 0.0] # [rad/s]
+        orientation_stddev: [0.0, 0.0, 0.0] # [rad]
+        magnetic_field_stddev: [0.0, 0.0, 0.0] # [Tesla]

src/main.cpp

@@ -51,8 +51,6 @@ int main(int argc, char *argv[])
 	rclcpp::init(argc, argv);
 	rclcpp::executors::SingleThreadedExecutor exec;
 	rclcpp::NodeOptions nodeOptions;
-	nodeOptions.allow_undeclared_parameters(true);
-	nodeOptions.automatically_declare_parameters_from_overrides(true);
 
 	auto xdaInterface = std::make_shared<XdaInterface>("xsens_driver", nodeOptions);
 	exec.add_node(xdaInterface);
@@ -70,7 +68,8 @@ int main(int argc, char *argv[])
 		exec.spin_some();
 	}
 
-	xdaInterface->close();
+	xdaInterface.reset();
+
 	rclcpp::shutdown();
 
 	return 0;

src/messagepublishers/imupublisher.h

@@ -38,10 +38,11 @@
 #define IMUPUBLISHER_H
 
 #include "packetcallback.h"
+#include "publisherhelperfunctions.h"
 #include <sensor_msgs/msg/imu.hpp>
 
 
-struct ImuPublisher : public PacketCallback
+struct ImuPublisher : public PacketCallback, PublisherHelperFunctions
 {
     rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr pub;
     double orientation_variance[3];
@@ -52,24 +53,19 @@ struct ImuPublisher : public PacketCallback
     ImuPublisher(rclcpp::Node &node)
         : node_handle(node)
     {
-	// TODO: Port this for allow_undeclared_parameters
-        std::vector<double> variance;
-        // node.declare_parameter("orientation_stddev", variance);
-        // node.declare_parameter("angular_velocity_stddev", variance);
-        // node.declare_parameter("linear_acceleration_stddev", variance);
-        // node.declare_parameter("orientation_stddev", orientation_variance);
-        // node.declare_parameter("angular_velocity_stddev", angular_velocity_variance);
-        // node.declare_parameter("linear_acceleration_stddev", linear_acceleration_variance);
+        std::vector<double> variance = {0, 0, 0};
+        node.declare_parameter("orientation_stddev", variance);
+        node.declare_parameter("angular_velocity_stddev", variance);
+        node.declare_parameter("linear_acceleration_stddev", variance);
 
         int pub_queue_size = 5;
         node.get_parameter("publisher_queue_size", pub_queue_size);
         pub = node.create_publisher<sensor_msgs::msg::Imu>("/imu/data", pub_queue_size);
 
         // REP 145: Conventions for IMU Sensor Drivers (http://www.ros.org/reps/rep-0145.html)
-	// TODO: Port this for allow_undeclared_parameters
-        variance_from_stddev_param("orientation_stddev", orientation_variance);
-        variance_from_stddev_param("angular_velocity_stddev", angular_velocity_variance);
-        variance_from_stddev_param("linear_acceleration_stddev", linear_acceleration_variance);
+        variance_from_stddev_param("orientation_stddev", orientation_variance, node);
+        variance_from_stddev_param("angular_velocity_stddev", angular_velocity_variance, node);
+        variance_from_stddev_param("linear_acceleration_stddev", linear_acceleration_variance, node);
     }
 
     void operator()(const XsDataPacket &packet, rclcpp::Time timestamp)
@@ -157,28 +153,6 @@ struct ImuPublisher : public PacketCallback
             pub->publish(msg);
         }
     }
-
-    void variance_from_stddev_param(std::string param, double *variance_out)
-    {
-    	// TODO: Port this for allow_undeclared_parameters
-        std::vector<double> stddev;
-        if (node_handle.get_parameter(param, stddev))
-        {
-            if (stddev.size() == 3)
-            {
-                auto squared = [](double x) { return x * x; };
-                std::transform(stddev.begin(), stddev.end(), variance_out, squared);
-            }
-            else
-            {
-                RCLCPP_WARN(node_handle.get_logger(), "Wrong size of param: %s, must be of size 3", param.c_str());
-            }
-        }
-        else
-        {
-            memset(variance_out, 0, 3 * sizeof(double));
-        }
-    }
 };
 
 #endif

src/messagepublishers/publisherhelperfunctions.h

@@ -0,0 +1,51 @@
+// Copyright 2021 BlueSpace.ai, Inc.
+//
+// Source code modified from the source provided by Xsens Technologies
+
+
+#ifndef PUBLISHER_HELPER_FUNCTION_H
+#define PUBLISHER_HELPER_FUNCTION_H
+
+#include <string>
+#include <vector>
+#include <rclcpp/rclcpp.hpp>
+
+class PublisherHelperFunctions
+{
+public:
+    PublisherHelperFunctions(/* args */);
+    ~PublisherHelperFunctions();
+
+    void variance_from_stddev_param(std::string param, double *variance_out, rclcpp::Node& node_handle)
+    {
+        std::vector<double> stddev;
+        if (node_handle.get_parameter(param, stddev))
+        {
+            if (stddev.size() == 3)
+            {
+                auto squared = [](double x) { return x * x; };
+                std::transform(stddev.begin(), stddev.end(), variance_out, squared);
+            }
+            else
+            {
+                RCLCPP_WARN(node_handle.get_logger(), "Wrong size of param: %s, must be of size 3", param.c_str());
+            }
+        }
+        else
+        {
+            memset(variance_out, 0, 3 * sizeof(double));
+        }
+    }
+
+};
+
+PublisherHelperFunctions::PublisherHelperFunctions(/* args */)
+{
+}
+
+PublisherHelperFunctions::~PublisherHelperFunctions()
+{
+}
+
+#endif
+

src/xdainterface.cpp

@@ -58,14 +58,12 @@
 #include "messagepublishers/positionllapublisher.h"
 #include "messagepublishers/velocitypublisher.h"
 
-// #define XS_DEFAULT_BAUDRATE (115200)
-
 XdaInterface::XdaInterface(const std::string &node_name, const rclcpp::NodeOptions &options)
 	: Node(node_name, options)
 	, m_device(nullptr)
 	, m_xdaCallback(*this)
 {
-	// declareCommonParameters();
+	declareCommonParameters();
 	RCLCPP_INFO(get_logger(), "Creating XsControl object...");
 	m_control = XsControl::construct();
 	assert(m_control != 0);
@@ -164,31 +162,31 @@ void XdaInterface::registerPublishers()
 
 bool XdaInterface::connectDevice()
 {
-	// Read baudrate parameter if set
+	XsPortInfo mtPort;
 	XsBaudRate baudrate = XBR_Invalid;
-	if (has_parameter("baudrate"))
-	{
+	bool checkDeviceID = false;
+	std::string deviceId = "";
+
+	// Check if scanning is enabled
+	bool scan_for_devices = false;
+	get_parameter("scan_for_devices", scan_for_devices);
+
+	if (!scan_for_devices){
+		// Read baudrate parameter
 		int baudrateParam = 0;
 		get_parameter("baudrate", baudrateParam);
 		RCLCPP_INFO(get_logger(), "Found baudrate parameter: %d", baudrateParam);
 		baudrate = XsBaud::numericToRate(baudrateParam);
-	}
-	// Read device ID parameter
-	bool checkDeviceID = false;
-	std::string deviceId;
-	if (has_parameter("device_id"))
-	{
-		get_parameter("device_id", deviceId);
-		checkDeviceID = true;
-		RCLCPP_INFO(get_logger(), "Found device ID parameter: %s.",deviceId.c_str());
 
-	}
-	// Read port parameter if set
-	XsPortInfo mtPort;
+		// Read device ID parameter if set
+		get_parameter("device_id", deviceId);
+		if (deviceId != "")
+		{
+			checkDeviceID = true;
+			RCLCPP_INFO(get_logger(), "Found device ID parameter: %s.", deviceId.c_str());
+		}
 
-	// TODO: Port this for allow_undeclared_parameters
-	if (has_parameter("port"))
-	{
+		// Read port parameter
 		std::string portName;
 		get_parameter("port", portName);
 		RCLCPP_INFO(get_logger(), "Found port name parameter: %s", portName.c_str());
@@ -259,18 +257,23 @@ bool XdaInterface::prepare()
 	if (!m_device->gotoMeasurement())
 		return handleError("Could not put device into measurement mode");
 
-	// TODO: Port this for allow_undeclared_parameters
-	std::string logFile;
-	if (get_parameter("log_file", logFile))
+	bool enable_logging = false;
+	get_parameter("enable_logging", enable_logging);
+
+	if (enable_logging)
 	{
-		if (m_device->createLogFile(logFile) != XRV_OK)
-			return handleError("Failed to create a log file! (" + logFile + ")");
-		else
-			RCLCPP_INFO(get_logger(), "Created a log file: %s", logFile.c_str());
-
-		RCLCPP_INFO(get_logger(), "Recording to %s ...", logFile.c_str());
-		if (!m_device->startRecording())
-			return handleError("Could not start recording");
+		std::string logFile;
+		if (get_parameter("log_file", logFile))
+		{
+			if (m_device->createLogFile(logFile) != XRV_OK)
+				return handleError("Failed to create a log file! (" + logFile + ")");
+			else
+				RCLCPP_INFO(get_logger(), "Created a log file: %s", logFile.c_str());
+
+			RCLCPP_INFO(get_logger(), "Recording to %s ...", logFile.c_str());
+			if (!m_device->startRecording())
+				return handleError("Could not start recording");
+		}
 	}
 
 	return true;
@@ -324,4 +327,12 @@ void XdaInterface::declareCommonParameters()
 	declare_parameter("pub_transform", should_publish);
 	declare_parameter("pub_positionLLA", should_publish);
 	declare_parameter("pub_velocity", should_publish);
+
+	declare_parameter("scan_for_devices", true);
+	declare_parameter("device_id", "");
+	declare_parameter("port", "");
+	declare_parameter("baudrate", XsBaud::rateToNumeric(XBR_Invalid));
+
+	declare_parameter("enable_logging", false);
+	declare_parameter("log_file", "log.mtb");
 }