turns out the only incorrect thing was my assumptions

.vscode/c_cpp_properties.json

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

Makefile

@@ -1,4 +1,4 @@
-.PHONY: build run upload legacy
+.PHONY: build run upload
 
 SOURCES := $(wildcard src/ros2aria/src/*.cpp)
 HEADERS := $(wildcard src/ros2aria/src/*.hpp)
@@ -8,15 +8,13 @@ build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
 
 build: build/ros2aria/ros2aria
 
-# .uploaded: build/ros2aria/ros2aria
-upload:
-	rsync -r  . lab1_5@pionier6:~/ros2aria
+.uploaded: build/ros2aria/ros2aria
+	rsync -r  build/ros2aria/ros2aria lab1_5@pionier6:~/ros2aria
 	touch .uploaded
 
-# upload: .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 /bin/bash /ws/ros2aria/run.sh
+	ssh lab1_5@pionier6 -t -- docker run --rm --network=host -it --device /dev/ttyS0 -v /home/lab1_5:/ws irth7/ros2aria-dev /ws/ros2aria
+
 
-legacy:
-	ssh lab1_5@pionier6 -t -- ./run.sh

node_info_ros1.txt

@@ -1,61 +0,0 @@
---------------------------------------------------------------------------------
-Node [/PIONIER6/RosAria]
-Publications: 
- * /PIONIER6/RosAria/battery_recharge_state [std_msgs/Int8]
- * /PIONIER6/RosAria/battery_state_of_charge [std_msgs/Float32]
- * /PIONIER6/RosAria/bumper_state [rosaria/BumperState]
- * /PIONIER6/RosAria/parameter_descriptions [dynamic_reconfigure/ConfigDescription]
- * /PIONIER6/RosAria/parameter_updates [dynamic_reconfigure/Config]
- * /PIONIER6/RosAria/pose [nav_msgs/Odometry]
- * /PIONIER6/RosAria/robot_info [rosaria_msgs/RobotInfoMsg]
- * /PIONIER6/RosAria/sonar [sensor_msgs/PointCloud]
- * /PIONIER6/RosAria/sonar_pointcloud2 [sensor_msgs/PointCloud2]
- * /PIONIER6/RosAria/wheels [sensor_msgs/JointState]
- * /rosout [rosgraph_msgs/Log]
- * /tf [tf2_msgs/TFMessage]
-
-Subscriptions: 
- * /PIONIER/master_stop [std_msgs/Bool]
- * /PIONIER/restrictions [rosaria_msgs/RestrictionsMsg]
- * /PIONIER6/RosAria/clutch [std_msgs/Bool]
- * /PIONIER6/RosAria/cmd_vel [unknown type]
- * /PIONIER6/RosAria/user_stop [std_msgs/Bool]
-
-Services: 
- * /PIONIER6/RosAria/get_loggers
- * /PIONIER6/RosAria/gripper_close
- * /PIONIER6/RosAria/gripper_down
- * /PIONIER6/RosAria/gripper_open
- * /PIONIER6/RosAria/gripper_up
- * /PIONIER6/RosAria/set_logger_level
- * /PIONIER6/RosAria/set_parameters
-
-
-contacting node http://10.104.16.45:35307/ ...
-Pid: 52
-Connections:
- * topic: /rosout
-    * to: /rosout
-    * direction: outbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/robot_info
-    * to: /master_plugin_lab15_19_12824_1114070233125035517
-    * direction: outbound
-    * transport: TCPROS
- * topic: /PIONIER/restrictions
-    * to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/user_stop
-    * to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER/master_stop
-    * to: /master_plugin_lab15_19_12824_1114070233125035517 (http://10.104.16.29:35537/)
-    * direction: inbound
-    * transport: TCPROS
- * topic: /PIONIER6/RosAria/clutch
-    * to: /user_plugin_lab15_19_15229_5138248156866338464 (http://10.104.16.29:43275/)
-    * direction: inbound
-    * transport: TCPROS
-

run.sh

@@ -1,6 +0,0 @@
-#!/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,7 +23,6 @@ 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)
@@ -36,11 +35,7 @@ add_executable(ros2aria
   src/cmd_vel.cpp
   src/publish.cpp
   src/sonar.cpp
-  src/pose.cpp
-  src/gripper.cpp
-  src/wheels.cpp
-  src/clutch.cpp
-  src/state.cpp)
+  src/pose.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
@@ -48,8 +43,6 @@ 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,7 +17,6 @@
   <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/clutch.cpp

@@ -1,12 +0,0 @@
-#include "ros2aria.hpp"
-
-void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
-{
-    std::lock_guard<RAIIBot> lock(*robot.get());
-
-    auto r = robot->getRobot();
-    if (msg->data)
-        r->enableMotors();
-    else
-        r->disableMotors();
-}

src/ros2aria/src/gripper.cpp

@@ -1,34 +0,0 @@
-#include "./ros2aria.hpp"
-
-void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
-    UNUSED(request);
-    UNUSED(response);
-    std::lock_guard<RAIIBot> lock(*robot.get());
-    auto g = robot->getGripper();
-    g->gripOpen();
-}
-void Ros2Aria::gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
-    UNUSED(request);
-    UNUSED(response);
-    std::lock_guard<RAIIBot> lock(*robot.get());
-    auto g = robot->getGripper();
-    g->gripClose();
-}
-void Ros2Aria::gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
-    UNUSED(request);
-    UNUSED(response);
-    std::lock_guard<RAIIBot> lock(*robot.get());
-    auto g = robot->getGripper();
-    g->liftUp();
-}
-void Ros2Aria::gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
-{
-    UNUSED(request);
-    UNUSED(response);
-    std::lock_guard<RAIIBot> lock(*robot.get());
-    auto g = robot->getGripper();
-    g->liftDown();
-}

src/ros2aria/src/publish.cpp

@@ -11,9 +11,4 @@ void Ros2Aria::publish()
 
     nav_msgs::msg::Odometry pose = handlePose(t);
     publishPose(pose);
-
-    sensor_msgs::msg::JointState wheels = handleWheels(t);
-    publishWheels(wheels);
-
-    publishState(t);
 }

src/ros2aria/src/raiibot.cpp

@@ -15,7 +15,6 @@ public:
     RAIIBot(rclcpp::Node *node, std::string port)
     {
         this->robot = new ArRobot();
-        this->gripper = new ArGripper(this->robot);
         this->node = node;
         this->clock = rclcpp::Clock::make_shared();
 
@@ -77,11 +76,6 @@ public:
         return this->robot;
     }
 
-    ArGripper *getGripper()
-    {
-        return this->gripper;
-    }
-
     rclcpp::Clock::SharedPtr getClock()
     {
         return this->clock;
@@ -110,7 +104,6 @@ private:
     ArRobotConnector *robotConn = nullptr;
     ArArgumentBuilder *args = nullptr;
     ArArgumentParser *argparser = nullptr;
-    ArGripper *gripper = nullptr;
     rclcpp::Node *node = nullptr;
     rclcpp::TimerBase::SharedPtr watchdogTimer;
     rclcpp::Time lastWatchdogPoke;

src/ros2aria/src/ros2aria.cpp

@@ -12,27 +12,13 @@ Ros2Aria::Ros2Aria()
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel", 10, std::bind(&Ros2Aria::cmd_vel_callback, this, _1));
-    clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
-        "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
+    stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
 
     sonar_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud>("sonar", 10);
     sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);
 
     pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
 
-    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
-
-    battery_recharge_state_pub_ = this->create_publisher<std_msgs::msg::Int8>("battery_recharge_state", 10);
-    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));
-    gripper_open_service_ = this->create_service<std_srvs::srv::Empty>("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2));
-    gripper_close_service_ = this->create_service<std_srvs::srv::Empty>("gripper_close", std::bind(&Ros2Aria::gripper_close_callback, this, _1, _2));
-    gripper_up_service_ = this->create_service<std_srvs::srv::Empty>("gripper_up", std::bind(&Ros2Aria::gripper_up_callback, this, _1, _2));
-    gripper_down_service_ = this->create_service<std_srvs::srv::Empty>("gripper_down", std::bind(&Ros2Aria::gripper_down_callback, this, _1, _2));
-
     // listen to sensors
     auto r = robot->getRobot();
     r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb);
@@ -48,8 +34,6 @@ Ros2Aria::~Ros2Aria()
 
 void Ros2Aria::stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
 {
-    UNUSED(request);
-    UNUSED(response);
     RCLCPP_INFO(this->get_logger(), "stop");
     rclcpp::shutdown();
 }

src/ros2aria/src/ros2aria.hpp

@@ -3,19 +3,12 @@
 
 #include "sensor_msgs/msg/point_cloud.hpp"
 #include "sensor_msgs/msg/point_cloud2.hpp"
-#include "sensor_msgs/msg/joint_state.hpp"
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav_msgs/msg/odometry.hpp"
 #include "std_srvs/srv/empty.hpp"
-#include "std_msgs/msg/bool.hpp"
-#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)
-
 class Ros2Aria : public rclcpp::Node
 {
 public:
@@ -43,35 +36,11 @@ private:
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
     void publishPose(nav_msgs::msg::Odometry pose);
 
-    sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
-    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
-    void publishWheels(sensor_msgs::msg::JointState wheels);
-
-    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
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
 
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr clutch_sub_;
-    void clutch_callback(const std_msgs::msg::Bool::SharedPtr msg);
-
     // services
     rclcpp::Service<std_srvs::srv::Empty>::SharedPtr stop_service_;
     void stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const;
-
-    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_open_service_;
-    void gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
-
-    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_close_service_;
-    void gripper_close_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
-
-    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_up_service_;
-    void gripper_up_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
-
-    rclcpp::Service<std_srvs::srv::Empty>::SharedPtr gripper_down_service_;
-    void gripper_down_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const;
 };

src/ros2aria/src/state.cpp

@@ -1,44 +0,0 @@
-#include "ros2aria.hpp"
-
-void Ros2Aria::publishState(rclcpp::Time stamp)
-{
-    auto r = this->robot->getRobot();
-
-    // TODO: original rosaria only publishes it when it changes.
-    if (this->battery_recharge_state_pub_->get_subscription_count() > 0)
-    {
-        char s = r->getChargeState();
-        std_msgs::msg::Int8 recharge_state;
-        recharge_state.data = s;
-        this->battery_recharge_state_pub_->publish(recharge_state);
-    }
-
-    // TODO: our robots don't have this, cannot test
-    if (r->haveStateOfCharge() && this->battery_state_of_charge_pub_->get_subscription_count() > 0)
-    {
-        std_msgs::msg::Float32 soc;
-        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 (true -> unlocked, false ->
-        // locked). This requires safety plugin to be implemented
-        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/src/wheels.cpp

@@ -1,32 +0,0 @@
-#include "ros2aria.hpp"
-
-sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
-{
-    sensor_msgs::msg::JointState wheels;
-
-    auto r = this->robot->getRobot();
-
-    r->requestEncoderPackets(); // TODO: check if this is synchronous or do we have a race condition or something
-    wheels.header.stamp = stamp;
-    wheels.name.resize(2);
-    wheels.position.resize(2);
-    wheels.velocity.resize(2);
-    wheels.effort.resize(0);
-
-    wheels.name[0] = "Wheel_L";
-    wheels.name[1] = "Wheel_R";
-    wheels.position[0] = r->getLeftEncoder();
-    wheels.position[1] = r->getRightEncoder();
-    wheels.velocity[0] = r->getLeftVel();
-    wheels.velocity[1] = r->getRightVel();
-
-    return wheels;
-}
-
-void Ros2Aria::publishWheels(sensor_msgs::msg::JointState wheels)
-{
-    if (this->wheels_pub_->get_subscription_count() == 0)
-        return;
-
-    this->wheels_pub_->publish(wheels);
-}

src/ros2aria_msgs/CMakeLists.txt

@@ -1,49 +0,0 @@
-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

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

src/ros2aria_msgs/msg/RobotInfoMsg.msg

@@ -1,6 +0,0 @@
-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

@@ -1,24 +0,0 @@
-<?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>