update comments, close #2

closes #10

.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

@@ -1,4 +1,4 @@
-.PHONY: build run upload
+.PHONY: build run upload legacy
 
 SOURCES := $(wildcard src/ros2aria/src/*.cpp)
 HEADERS := $(wildcard src/ros2aria/src/*.hpp)
@@ -8,13 +8,15 @@ build/ros2aria/ros2aria: $(SOURCES) $(HEADERS)
 
 build: build/ros2aria/ros2aria
 
-.uploaded: build/ros2aria/ros2aria
+# .uploaded: build/ros2aria/ros2aria
-	rsync -r  build/ros2aria/ros2aria lab1_5@pionier6:~/ros2aria
+upload:
+	rsync -r  . 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 /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

node_info_ros1.txt

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

@@ -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)
@@ -35,7 +36,11 @@ add_executable(ros2aria
   src/cmd_vel.cpp
   src/publish.cpp
   src/sonar.cpp
-  src/pose.cpp)
+  src/pose.cpp
+  src/gripper.cpp
+  src/wheels.cpp
+  src/clutch.cpp
+  src/state.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
@@ -43,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/clutch.cpp

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

@@ -0,0 +1,34 @@
+#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,4 +11,9 @@ 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,6 +15,7 @@ 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();
 
@@ -76,6 +77,11 @@ public:
         return this->robot;
     }
 
+    ArGripper *getGripper()
+    {
+        return this->gripper;
+    }
+
     rclcpp::Clock::SharedPtr getClock()
     {
         return this->clock;
@@ -104,6 +110,7 @@ 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,13 +12,27 @@ 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));
-    stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
+    clutch_sub_ = this->create_subscription<std_msgs::msg::Bool>(
+        "clutch", 10, std::bind(&Ros2Aria::clutch_callback, this, _1));
 
     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);
@@ -34,6 +48,8 @@ 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,12 +3,19 @@
 
 #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:
@@ -36,11 +43,35 @@ 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

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

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

@@ -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>