From 842f75366569b34e02f4d1cbf5072fcda9804487 Mon Sep 17 00:00:00 2001 From: Wojciech Kwolek Date: Fri, 21 May 2021 11:46:31 +0000 Subject: [PATCH] publish robot state --- .vscode/c_cpp_properties.json | 1 + Makefile | 6 +-- run.sh | 6 +++ src/ros2aria/CMakeLists.txt | 3 ++ src/ros2aria/package.xml | 1 + src/ros2aria/src/ros2aria.cpp | 1 + src/ros2aria/src/ros2aria.hpp | 2 + src/ros2aria/src/state.cpp | 19 +++++++++ src/ros2aria_msgs/CMakeLists.txt | 49 +++++++++++++++++++++++ src/ros2aria_msgs/msg/RestrictionsMsg.msg | 3 ++ src/ros2aria_msgs/msg/RobotInfoMsg.msg | 6 +++ src/ros2aria_msgs/package.xml | 24 +++++++++++ 12 files changed, 118 insertions(+), 3 deletions(-) create mode 100755 run.sh create mode 100644 src/ros2aria_msgs/CMakeLists.txt create mode 100644 src/ros2aria_msgs/msg/RestrictionsMsg.msg create mode 100644 src/ros2aria_msgs/msg/RobotInfoMsg.msg create mode 100644 src/ros2aria_msgs/package.xml diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 13765aa..48976f7 100644 --- a/.vscode/c_cpp_properties.json +++ b/.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" ], diff --git a/Makefile b/Makefile index e767f9f..24dc356 100644 --- a/Makefile +++ b/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 \ No newline at end of file + ssh lab1_5@pionier6 -t -- ./run.sh diff --git a/run.sh b/run.sh new file mode 100755 index 0000000..a9917bf --- /dev/null +++ b/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 + diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt index 57ec68f..43cd619 100644 --- a/src/ros2aria/CMakeLists.txt +++ b/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( 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 $ $ diff --git a/src/ros2aria/package.xml b/src/ros2aria/package.xml index fd71d3a..8828e9f 100644 --- a/src/ros2aria/package.xml +++ b/src/ros2aria/package.xml @@ -17,6 +17,7 @@ geometry_msgs sensor_msgs nav_msgs + ros2aria_msgs ament_cmake diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 26ed9dd..32b6768 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/src/ros2aria/src/ros2aria.cpp @@ -24,6 +24,7 @@ Ros2Aria::Ros2Aria() battery_recharge_state_pub_ = this->create_publisher("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("battery_state_of_charge", 10); + robot_info_pub_ = this->create_publisher("robot_info", 10); // services stop_service_ = this->create_service("stop", std::bind(&Ros2Aria::stop, this, _1, _2)); diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp index 80efaa1..69b0927 100644 --- a/src/ros2aria/src/ros2aria.hpp +++ b/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::SharedPtr battery_recharge_state_pub_; rclcpp::Publisher::SharedPtr battery_state_of_charge_pub_; + rclcpp::Publisher::SharedPtr robot_info_pub_; void publishState(rclcpp::Time stamp); // subscribers diff --git a/src/ros2aria/src/state.cpp b/src/ros2aria/src/state.cpp index 9001fa5..7525067 100644 --- a/src/ros2aria/src/state.cpp +++ b/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); + } } \ No newline at end of file diff --git a/src/ros2aria_msgs/CMakeLists.txt b/src/ros2aria_msgs/CMakeLists.txt new file mode 100644 index 0000000..f4a0683 --- /dev/null +++ b/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( 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() diff --git a/src/ros2aria_msgs/msg/RestrictionsMsg.msg b/src/ros2aria_msgs/msg/RestrictionsMsg.msg new file mode 100644 index 0000000..3530527 --- /dev/null +++ b/src/ros2aria_msgs/msg/RestrictionsMsg.msg @@ -0,0 +1,3 @@ +std_msgs/Float64 distance +std_msgs/Float64 linear_velocity +std_msgs/Float64 angular_velocity diff --git a/src/ros2aria_msgs/msg/RobotInfoMsg.msg b/src/ros2aria_msgs/msg/RobotInfoMsg.msg new file mode 100644 index 0000000..25e200e --- /dev/null +++ b/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 diff --git a/src/ros2aria_msgs/package.xml b/src/ros2aria_msgs/package.xml new file mode 100644 index 0000000..5967ecd --- /dev/null +++ b/src/ros2aria_msgs/package.xml @@ -0,0 +1,24 @@ + + + + ros2aria_msgs + 0.0.0 + TODO: Package description + wojciech + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rosidl_default_generators + + rosidl_default_runtime + + rosidl_interface_packages + + + ament_cmake + +