feat(les1): Add custom message interface

src/les_interface/CMakeLists.txt

@@ -0,0 +1,18 @@
+cmake_minimum_required(VERSION 3.8)
+project(les_interface)
+
+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)
+find_package(rosidl_default_generators REQUIRED)
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/HardwareStatus.msg"
+)
+ament_export_dependencies(rosidl_default_runtime)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+ament_package()

src/les_interface/msg/HardwareStatus.msg

@@ -0,0 +1,4 @@
+int64 version
+float64 temperature
+bool are_motors_ready
+string debug_message

src/les_interface/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>les_interface</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="wessel@todo.todo">wessel</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/les_pkg/CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_action REQUIRED)   # nodig als er een actionserver node wordt gemaakt
 find_package(std_msgs REQUIRED)        # gebruik maken von standaard messages
 find_package(geometry_msgs REQUIRED)        # gebruik maken von standaard messages
+find_package(les_interface REQUIRED)        # gebruik maken von standaard messages
 #find_package(templates_interfaces REQUIRED)   # gebruik maken van project messages
 
 
@@ -19,8 +20,8 @@ add_executable(les1_publisher src/les1/publisher.cpp)
 add_executable(les1_subscriber src/les1/subscriber.cpp)
 
 ament_target_dependencies(les1 rclcpp)
-ament_target_dependencies(les1_publisher rclcpp std_msgs geometry_msgs)
+ament_target_dependencies(les1_publisher rclcpp std_msgs geometry_msgs les_interface)
-ament_target_dependencies(les1_subscriber rclcpp std_msgs geometry_msgs)
+ament_target_dependencies(les1_subscriber rclcpp std_msgs geometry_msgs les_interface)
 
 install (
   TARGETS

src/les_pkg/package.xml

@@ -10,7 +10,8 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>rclcpp</depend>
-
+  <depend>les_interface</depend>
+  
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/les_pkg/src/les1/publisher.cpp

@@ -33,20 +33,29 @@
 #include <rclcpp/time.hpp>
 
 #include "geometry_msgs/msg/point.hpp"  // Point for x, y, z coordinates
+#include "les_interface/msg/hardware_status.hpp" // Custom HW status message
 
 class NodeLes1Publisher : public rclcpp::Node {
 
 public:
 NodeLes1Publisher()
-	: Node("node_les1_publisher")
+		: Node("node_les1_publisher")
 {
-	publisher_location_ =
+		publisher_location_ =
-	  this->create_publisher<geometry_msgs::msg::Point>("location", 10);
+			this->create_publisher<geometry_msgs::msg::Point>("location", 10);
 
-	timer_location_ = this->create_wall_timer(
+		publisher_hw_status_ =
-		std::chrono::seconds(2),
+			this->create_publisher<les_interface::msg::HardwareStatus>("hardware_status", 10);
-		std::bind(&NodeLes1Publisher::timer_location_function, this)
+
-	);
+		timer_location_ = this->create_wall_timer(
+				std::chrono::seconds(2),
+				std::bind(&NodeLes1Publisher::timer_location_function, this)
+		);
+
+		timer_hw_status_ = this->create_wall_timer(
+				std::chrono::seconds(2),
+				std::bind(&NodeLes1Publisher::timer_hw_status_function, this)
+		);
 }
 
 
@@ -58,12 +67,29 @@ void timer_location_function() {
 
 	publisher_location_->publish(coordinate);
 	RCLCPP_INFO(this->get_logger(),
-    "Published x=%.2f, y=%.2f, z=%.2f", coordinate.x, coordinate.y, coordinate.z);
+	"Published x=%.2f, y=%.2f, z=%.2f", coordinate.x, coordinate.y, coordinate.z);
+}
+
+void timer_hw_status_function() {
+	les_interface::msg::HardwareStatus hw_msg;
+	hw_msg.version = 1;
+	hw_msg.temperature = static_cast<double>(std::rand()) / RAND_MAX * 80.0;
+	hw_msg.are_motors_ready = (std::rand() % 2 == 0);
+	hw_msg.debug_message = "Status OK";
+
+	publisher_hw_status_->publish(hw_msg);
+	RCLCPP_INFO(this->get_logger(),
+		"Published HW status: version=%ld, temp=%.2f, motors_ready=%s, msg=%s",
+		hw_msg.version, hw_msg.temperature,
+		hw_msg.are_motors_ready ? "true" : "false",
+		hw_msg.debug_message.c_str());
 }
 
 private:
 	rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr publisher_location_;
-	rclcpp::TimerBase::SharedPtr timer_location_ ;
+	rclcpp::Publisher<les_interface::msg::HardwareStatus>::SharedPtr publisher_hw_status_;
+	rclcpp::TimerBase::SharedPtr timer_location_;
+	rclcpp::TimerBase::SharedPtr timer_hw_status_;
 };
 
 int main(int argc,char *argv[]) {

src/les_pkg/src/les1/subscriber.cpp

@@ -31,6 +31,7 @@
 #include "rclcpp/rclcpp.hpp"
 
 #include "geometry_msgs/msg/point.hpp"  // Point for x, y, z coordinates
+#include "les_interface/msg/hardware_status.hpp"
 
 using namespace std::placeholders;
 
@@ -45,6 +46,12 @@ NodeLes1Subscriber()
         "location", 10,
         std::bind(&NodeLes1Subscriber::sub_callback_location, this, _1)
       );
+
+    subscriber_hw_status_ =
+      this->create_subscription<les_interface::msg::HardwareStatus>(
+        "hardware_status", 10,
+        std::bind(&NodeLes1Subscriber::sub_callback_hw_status, this, _1)
+      );
 }
 
 void sub_callback_location(const geometry_msgs::msg::Point::SharedPtr msg) {
@@ -53,8 +60,17 @@ void sub_callback_location(const geometry_msgs::msg::Point::SharedPtr msg) {
     );
 }
 
+void sub_callback_hw_status(const les_interface::msg::HardwareStatus::SharedPtr msg) {
+    RCLCPP_INFO(this->get_logger(),
+      "Received HW status: version=%ld, temp=%.2f, motors_ready=%s, msg=%s",
+      msg->version, msg->temperature,
+      msg->are_motors_ready ? "true" : "false",
+      msg->debug_message.c_str());
+}
+
 private:
   rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr subscriber_location_;
+  rclcpp::Subscription<les_interface::msg::HardwareStatus>::SharedPtr subscriber_hw_status_;
 };
 
 int main(int argc,char *argv[]) {