publish robot state

.vscode/c_cpp_properties.json

@@ -8,6 +8,7 @@
       "includePath": [
         "/opt/ros/foxy/include/**",
         "/workspaces/ros2aria/src/ros2aria/include/**",
+        "/workspaces/ros2aria/install/ros2aria_msgs/include/**",
         "/usr/include/**",
         "/usr/local/Aria/include"
       ],

Makefile

@@ -9,13 +9,13 @@ build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
 build: build/ros2aria/ros2aria
 
 .uploaded: build/ros2aria/ros2aria
-	rsync -r  build/ros2aria/ros2aria lab1_5@pionier6:~/ros2aria
+	rsync -r  . lab1_5@pionier6:~/ros2aria
 	touch .uploaded
 
 upload: .uploaded
 
 run: upload
-	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /ws/ros2aria
+	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh
 
 legacy:
-	ssh lab1_5@pionier6 -t -- ./run.sh
+	ssh lab1_5@pionier6 -t -- ./run.sh

run.sh

@@ -0,0 +1,6 @@
+#!/bin/bash
+export LD_LIBRARY_PATH=/usr/local/Aria/lib
+source /opt/ros/foxy/setup.bash
+source /ws/ros2aria/install/setup.bash
+ros2 run ros2aria ros2aria
+

src/ros2aria/CMakeLists.txt

@@ -23,6 +23,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(ros2aria_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
@@ -47,6 +48,8 @@ ament_target_dependencies(ros2aria geometry_msgs)
 ament_target_dependencies(ros2aria sensor_msgs)
 ament_target_dependencies(ros2aria nav_msgs)
 ament_target_dependencies(ros2aria tf2)
+ament_target_dependencies(ros2aria ros2aria_msgs)
+
 target_include_directories(ros2aria PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

src/ros2aria/package.xml

@@ -17,6 +17,7 @@
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
+  <depend>ros2aria_msgs</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

src/ros2aria/src/ros2aria.cpp

@@ -24,6 +24,7 @@ Ros2Aria::Ros2Aria()
 
     battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10); // TODO: original rosaria latches this - what does that mean and how does it relate to ros2?
     battery_state_of_charge_pub_ = this->create_publisher<std_msgs::msg::Float32>("battery_state_of_charge", 10);
+    robot_info_pub_ = this->create_publisher<ros2aria_msgs::msg::RobotInfoMsg>("robot_info", 10);
 
     // services
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));

src/ros2aria/src/ros2aria.hpp

@@ -11,6 +11,7 @@
 #include "std_msgs/msg/int8.hpp"
 #include "std_msgs/msg/float32.hpp"
 
+#include "ros2aria_msgs/msg/robot_info_msg.hpp"
 #include "./raiibot.cpp"
 
 #define UNUSED(x) (void)(x)
@@ -48,6 +49,7 @@ private:
 
     rclcpp::Publisher<std_msgs::msg::Int8>::SharedPtr battery_recharge_state_pub_;
     rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr battery_state_of_charge_pub_;
+    rclcpp::Publisher<ros2aria_msgs::msg::RobotInfoMsg>::SharedPtr robot_info_pub_;
     void publishState(rclcpp::Time stamp);
 
     // subscribers

src/ros2aria/src/state.cpp

@@ -20,4 +20,23 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
         soc.data = r->getStateOfCharge() / 100.0;
         this->battery_state_of_charge_pub_->publish(soc);
     }
+
+    if (this->robot_info_pub_->get_subscription_count() > 0)
+    {
+        ros2aria_msgs::msg::RobotInfoMsg robot_info_msg;
+
+        // TODO: allow setting the robot_id
+        robot_info_msg.robot_id.data = 6;
+        robot_info_msg.battery_voltage.data = r->getRealBatteryVoltageNow();
+        robot_info_msg.twist.linear.x = r->getVel() / 1000;
+        robot_info_msg.twist.linear.y = r->getLatVel() / 1000.0;
+        robot_info_msg.twist.angular.z = r->getRotVel() * M_PI / 180;
+        // TODO: actually keep track of robot state
+        robot_info_msg.state.data = true;
+        robot_info_msg.clutch.data = r->areMotorsEnabled();
+        // TODO: actually look for obstacles
+        robot_info_msg.obstacle_detected.data = false;
+
+        this->robot_info_pub_->publish(robot_info_msg);
+    }
 }

src/ros2aria_msgs/CMakeLists.txt

@@ -0,0 +1,49 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2aria_msgs)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+	set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+	set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+rosidl_generate_interfaces(ros2aria_msgs
+	"msg/RestrictionsMsg.msg"
+	"msg/RobotInfoMsg.msg"
+	DEPENDENCIES std_msgs geometry_msgs
+	)
+
+
+
+if(BUILD_TESTING)
+	find_package(ament_lint_auto REQUIRED)
+	# the following line skips the linter which checks for copyrights
+	# uncomment the line when a copyright and license is not present in all source files
+	#set(ament_cmake_copyright_FOUND TRUE)
+	# the following line skips cpplint (only works in a git repo)
+	# uncomment the line when this package is not in a git repo
+	#set(ament_cmake_cpplint_FOUND TRUE)
+	ament_lint_auto_find_test_dependencies()
+endif()
+
+
+
+ament_export_dependencies(rosidl_default_runtime)
+ament_package()

src/ros2aria_msgs/msg/RestrictionsMsg.msg

@@ -0,0 +1,3 @@
+std_msgs/Float64 distance
+std_msgs/Float64 linear_velocity
+std_msgs/Float64 angular_velocity

src/ros2aria_msgs/msg/RobotInfoMsg.msg

@@ -0,0 +1,6 @@
+std_msgs/UInt8 robot_id
+std_msgs/Float64 battery_voltage
+geometry_msgs/Twist twist
+std_msgs/Bool state
+std_msgs/Bool clutch
+std_msgs/Bool obstacle_detected

src/ros2aria_msgs/package.xml

@@ -0,0 +1,24 @@
+<?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>ros2aria_msgs</name>
+	<version>0.0.0</version>
+	<description>TODO: Package description</description>
+	<maintainer email="me@irth.pl">wojciech</maintainer>
+	<license>TODO: License declaration</license>
+
+	<buildtool_depend>ament_cmake</buildtool_depend>
+
+	<test_depend>ament_lint_auto</test_depend>
+	<test_depend>ament_lint_common</test_depend>
+
+	<build_depend>rosidl_default_generators</build_depend>
+
+	<exec_depend>rosidl_default_runtime</exec_depend>
+
+	<member_of_group>rosidl_interface_packages</member_of_group>
+
+	<export>
+		<build_type>ament_cmake</build_type>
+	</export>
+</package>