Reviewed old package

.vscode/settings.json

@@ -61,9 +61,31 @@
         "thread": "cpp",
         "cinttypes": "cpp",
         "typeindex": "cpp",
-        "typeinfo": "cpp"
+        "typeinfo": "cpp",
+        "any": "cpp",
+        "hash_map": "cpp",
+        "hash_set": "cpp",
+        "strstream": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "forward_list": "cpp",
+        "unordered_set": "cpp",
+        "source_location": "cpp",
+        "iomanip": "cpp",
+        "numbers": "cpp",
+        "semaphore": "cpp",
+        "stop_token": "cpp",
+        "valarray": "cpp",
+        "variant": "cpp",
+        "regex": "cpp"
     },
     "cSpell.words": [
         "rclcpp"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/foxy/lib/python3.8/site-packages"
     ]
 }

Dockerfile

@@ -0,0 +1,23 @@
+ARG ROS_DISTRO=humble
+FROM ros:$ROS_DISTRO
+
+SHELL ["/bin/bash", "-c"]
+RUN apt-get update && \
+    # apt-get install -y  \
+    # && \
+    apt-get autoremove -y && \
+    apt-get clean && \
+    rm -rf /var/lib/apt/lists/*
+
+COPY ./src/AriaCoda /usr/local/Aria
+RUN cd /usr/local/Aria && make -j$(nproc)
+
+RUN echo "source /opt/ros/$ROS_DISTRO/setup.bash" >> ~/.bashrc
+COPY ros_entrypoint.sh /
+ENTRYPOINT ["../ros_entrypoint.sh"]
+RUN chmod +x /ros_entrypoint.sh
+
+WORKDIR /app
+COPY src/ros2aria /app/src/ros2aria
+COPY src/ros2aria_msgs /app/src/ros2aria_msgs
+RUN cd /app && source /opt/ros/$ROS_DISTRO/setup.bash && colcon build

Dockerfile.devcontainer

@@ -1,4 +1,4 @@
-FROM athackst/ros2:foxy-dev 
+FROM athackst/ros2:foxy-dev
 
 # ** [Optional] Uncomment this section to install additional packages. **
 #

ros_entrypoint.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+set -e
+
+# setup ros environment
+source "/opt/ros/$ROS_DISTRO/setup.bash"
+source "install/setup.bash"
+exec "$@"

src/ros2aria/CMakeLists.txt

@@ -23,6 +23,8 @@ find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
 find_package(ros2aria_msgs REQUIRED)
 # uncomment the following section in order to fill in
 # further dependencies manually.
@@ -41,13 +43,15 @@ add_executable(ros2aria
   src/wheels.cpp
   src/clutch.cpp
   src/state.cpp)
-  
+
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
 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 tf2_ros)
+ament_target_dependencies(ros2aria tf2_geometry_msgs)
 ament_target_dependencies(ros2aria ros2aria_msgs)
 
 target_include_directories(ros2aria PUBLIC

src/ros2aria/include/ros2aria/raiibot.hpp

@@ -0,0 +1,36 @@
+#include <iostream>
+#include <memory>
+#include <string>
+
+#include "Aria/Aria.h"
+#include "rclcpp/rclcpp.hpp"
+
+
+class RAIIBot {
+   public:
+    typedef std::shared_ptr<RAIIBot> SharedPtr;
+
+    RAIIBot(rclcpp::Node *node, std::string port);
+    ~RAIIBot();
+    ArRobot *getRobot();
+    ArGripper *getGripper();
+    rclcpp::Clock::SharedPtr getClock();
+    void lock();
+    void unlock();
+    void pokeWatchdog();
+
+   private:
+    ArRobot *robot = nullptr;
+    ArRobotConnector *robotConn = nullptr;
+    ArArgumentBuilder *args = nullptr;
+    ArArgumentParser *argparser = nullptr;
+    ArGripper *gripper = nullptr;
+    rclcpp::Node *node = nullptr;
+    rclcpp::TimerBase::SharedPtr watchdogTimer;
+    rclcpp::Time lastWatchdogPoke;
+    rclcpp::Clock::SharedPtr clock;
+
+    rclcpp::Logger get_logger();
+    void startWatchdog();
+    void watchdog_cb();
+};

src/ros2aria/include/ros2aria/ros2aria.hpp

@@ -1,18 +1,18 @@
-#include "rclcpp/rclcpp.hpp"
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
-#include "Aria/Aria.h"
 
-#include "sensor_msgs/msg/point_cloud.hpp"
+#include "ros2aria/raiibot.hpp"
-#include "sensor_msgs/msg/point_cloud2.hpp"
+#include <Aria/Aria.h>
-#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 "rclcpp/rclcpp.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"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "sensor_msgs/msg/point_cloud.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include "std_msgs/msg/float32.hpp"
+#include "std_msgs/msg/int8.hpp"
+#include "std_srvs/srv/empty.hpp"
 
 #define UNUSED(x) (void)(x)
 

src/ros2aria/package.xml

@@ -15,6 +15,7 @@
   <depend>rclcpp</depend>
   <depend>std_srvs</depend>
   <depend>geometry_msgs</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>nav_msgs</depend>
   <depend>ros2aria_msgs</depend>

src/ros2aria/src/clutch.cpp

@@ -1,7 +1,6 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
+void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg) {
-{
     std::lock_guard<RAIIBot> lock(*robot.get());
 
     auto r = robot->getRobot();

src/ros2aria/src/cmd_vel.cpp

@@ -1,7 +1,6 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
+void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) {
-{
     float x, y, z;
     x = msg->linear.x;
     y = msg->linear.y;

src/ros2aria/src/gripper.cpp

@@ -1,31 +1,30 @@
-#include "./ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
-void Ros2Aria::gripper_open_callback(const std_srvs::srv::Empty::Request::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const
+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
+
-{
+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
+
-{
+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
+
-{
+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());

src/ros2aria/src/main.cpp

@@ -1,7 +1,7 @@
 #include <cstdio>
 #include <memory>
 
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 int main(int argc, char **argv)
 {

src/ros2aria/src/pose.cpp

@@ -1,44 +1,32 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
-#include "tf2/utils.h"
-#include "tf2/transform_datatypes.h"
 
-inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
+nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp) {
-{
+    nav_msgs::msg::Odometry odom_msg;
-    geometry_msgs::msg::Transform msg = tf2::toMsg(trans);
-    pose.orientation = msg.rotation;
-    pose.position.x = msg.translation.x;
-    pose.position.y = msg.translation.y;
-    pose.position.z = msg.translation.z;
-}
-
-nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
-{
-    nav_msgs::msg::Odometry pose;
 
     auto r = robot->getRobot();
     ArPose p = r->getPose();
 
     tf2::Quaternion rotation;
+    auto position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
     rotation.setRPY(0, 0, p.getTh() * M_PI / 180);
 
-    tf2::Vector3 position = tf2::Vector3(p.getX() / 1000, p.getY() / 1000, 0);
+    odom_msg.pose.pose.orientation = tf2::toMsg(rotation);
+    odom_msg.pose.pose.position.x = position.getX();
+    odom_msg.pose.pose.position.y = position.getY();
+    odom_msg.pose.pose.position.z = position.getZ();
 
-    convert_transform_to_pose(tf2::Transform(rotation, position),
+    odom_msg.twist.twist.linear.x = r->getVel() / 1000;
-                              pose.pose.pose);
+    odom_msg.twist.twist.linear.y = r->getLatVel() / 1000;
+    odom_msg.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
 
-    pose.twist.twist.linear.x = r->getVel() / 1000;
+    odom_msg.header.frame_id = "header.frame_id";  // TODO: use correct frame_id
-    pose.twist.twist.linear.y = r->getLatVel() / 1000;
+    odom_msg.child_frame_id = "child_frame_id";    // TODO: use correct child_frame_id
-    pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;
+    odom_msg.header.stamp = stamp;
 
-    pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
+    return odom_msg;
-    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_frame_id
-    pose.header.stamp = stamp;
-
-    return pose;
 }
 
-void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
+void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose) {
-{
     if (this->pose_pub_->get_subscription_count() == 0)
         return;
 

src/ros2aria/src/publish.cpp

@@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 void Ros2Aria::publish()
 {

src/ros2aria/src/raiibot.cpp

@@ -1,143 +1,108 @@
-#include <string>
+#include "ros2aria/raiibot.hpp"
-#include <memory>
-#include <iostream>
 
-#include "Aria/Aria.h"
-#include "rclcpp/rclcpp.hpp"
 
-using namespace std::chrono_literals;
+RAIIBot::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();
 
-class RAIIBot
+    args = new ArArgumentBuilder();
-{
+    this->argparser = new ArArgumentParser(args);
-public:
+    argparser->loadDefaultArguments();  // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
-    typedef std::shared_ptr<RAIIBot> SharedPtr;
+    std::string port_arg = "-robotPort " + port;
-
+    args->add(port_arg.c_str());  // pass robot's serial port to Aria // TODO: use `port` variable.
-    RAIIBot(rclcpp::Node *node, std::string port)
+    // args->add("-robotLogPacketsReceived");                    // log received packets
-    {
+    // args->add("-robotLogPacketsSent");                        // log sent packets
-        this->robot = new ArRobot();
+    // args->add("-robotLogVelocitiesReceived"); // log received velocities
-        this->gripper = new ArGripper(this->robot);
+    // args->add("-robotLogMovementSent");
-        this->node = node;
+    // args->add("-robotLogMovementReceived");
-        this->clock = rclcpp::Clock::make_shared();
+    this->robotConn = new ArRobotConnector(argparser, robot);
-
+    if (!this->robotConn->connectRobot()) {
-        args = new ArArgumentBuilder();
+        // RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
-        this->argparser = new ArArgumentParser(args);
+        // rclcpp::shutdown();
-        argparser->loadDefaultArguments();  // adds any arguments given in /etc/Aria.args.  Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
-        args->add("-robotPort /dev/ttyS0"); // pass robot's serial port to Aria // TODO: use `port` variable.
-        // args->add("-robotLogPacketsReceived");                    // log received packets
-        // args->add("-robotLogPacketsSent");                        // log sent packets
-        // args->add("-robotLogVelocitiesReceived"); // log received velocities
-        // args->add("-robotLogMovementSent");
-        // args->add("-robotLogMovementReceived");
-        this->robotConn = new ArRobotConnector(argparser, robot);
-        if (!this->robotConn->connectRobot())
-        {
-
-            // RCLCPP_INFO(this->get_logger(), "RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device, or any errors reported above)");
-            // rclcpp::shutdown();
-        }
-
-        this->robot->runAsync(true);
-        this->robot->enableMotors();
-
-        this->startWatchdog();
     }
 
-    ~RAIIBot()
+    this->robot->runAsync(true);
-    {
+    this->robot->enableMotors();
-        std::cout << std::endl
-                  << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
 
-        if (this->robot != nullptr)
+    this->startWatchdog();
-        {
+}
-            this->robot->lock();
-            std::cout << "disabling motors" << std::endl;
-            this->robot->disableMotors();
-            std::cout << "disabled motors" << std::endl;
-            Aria::shutdown();
-        }
-        if (this->robotConn != nullptr)
-        {
-            std::cout << "disconnecting" << std::endl;
-            this->robotConn->disconnectAll();
-            std::cout << "disconnected" << std::endl;
-        }
 
-        if (this->args != nullptr)
+RAIIBot ::~RAIIBot() {
-            delete this->args;
+    std::cout << std::endl
-        if (this->argparser != nullptr)
+              << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;
-            delete this->argparser;
+
-        if (this->robotConn != nullptr)
+    if (this->robot != nullptr) {
-            delete this->robotConn;
+        this->robot->lock();
-        if (this->robot != nullptr)
+        std::cout << "disabling motors" << std::endl;
-            delete this->robot;
+        this->robot->disableMotors();
+        std::cout << "disabled motors" << std::endl;
+        Aria::shutdown();
+    }
+    if (this->robotConn != nullptr) {
+        std::cout << "disconnecting" << std::endl;
+        this->robotConn->disconnectAll();
+        std::cout << "disconnected" << std::endl;
     }
 
-    ArRobot *getRobot()
+    if (this->args != nullptr)
-    {
+        delete this->args;
-        return this->robot;
+    if (this->argparser != nullptr)
-    }
+        delete this->argparser;
+    if (this->robotConn != nullptr)
+        delete this->robotConn;
+    if (this->robot != nullptr)
+        delete this->robot;
+}
 
-    ArGripper *getGripper()
+ArRobot *RAIIBot::getRobot() {
-    {
+    return this->robot;
-        return this->gripper;
+}
-    }
 
-    rclcpp::Clock::SharedPtr getClock()
+ArGripper *RAIIBot::getGripper() {
-    {
+    return this->gripper;
-        return this->clock;
+}
-    }
 
-    void lock()
+rclcpp::Clock::SharedPtr RAIIBot::getClock() {
-    {
+    return this->clock;
-        if (this->robot != nullptr)
+}
-            this->robot->lock();
-    }
 
-    void unlock()
+void RAIIBot::lock() {
-    {
+    if (this->robot != nullptr)
-        if (this->robot != nullptr)
+        this->robot->lock();
-            this->robot->unlock();
+}
-    }
 
-    void pokeWatchdog()
+void RAIIBot::unlock() {
-    {
+    if (this->robot != nullptr)
-        // this should probably be made thread safe, or something?
+        this->robot->unlock();
-        this->lastWatchdogPoke = this->clock->now();
+}
-    }
 
-private:
+void RAIIBot::pokeWatchdog() {
-    ArRobot *robot = nullptr;
+    // this should probably be made thread safe, or something?
-    ArRobotConnector *robotConn = nullptr;
+    this->lastWatchdogPoke = this->clock->now();
-    ArArgumentBuilder *args = nullptr;
+}
-    ArArgumentParser *argparser = nullptr;
-    ArGripper *gripper = nullptr;
-    rclcpp::Node *node = nullptr;
-    rclcpp::TimerBase::SharedPtr watchdogTimer;
-    rclcpp::Time lastWatchdogPoke;
-    rclcpp::Clock::SharedPtr clock;
 
-    rclcpp::Logger get_logger()
+rclcpp::Logger RAIIBot::get_logger() {
-    {
+    return this->node->get_logger();
-        return this->node->get_logger();
+}
-    }
 
-    void startWatchdog()
+void RAIIBot::startWatchdog() {
-    {
+    using namespace std::chrono_literals;
-        this->lastWatchdogPoke = clock->now();
+    this->lastWatchdogPoke = clock->now();
-        watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
+    watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
-    }
+}
 
-    void watchdog_cb()
+void RAIIBot::watchdog_cb() {
-    {
+    using namespace std::chrono_literals;
-        auto now = this->clock->now();
+
-        if (now - this->lastWatchdogPoke > 1s)
+    auto now = this->clock->now();
-        {
+    if (now - this->lastWatchdogPoke > 1s) {
-            std::lock_guard<RAIIBot> lock(*this);
+        std::lock_guard<RAIIBot> lock(*this);
-            auto r = this->getRobot();
+        auto r = this->getRobot();
-            r->setVel(0);
+        r->setVel(0);
-            r->setRotVel(0);
+        r->setRotVel(0);
-            if (r->hasLatVel())
+        if (r->hasLatVel())
-                r->setLatVel(0);
+            r->setLatVel(0);
-        }
     }
-};
+}

src/ros2aria/src/ros2aria.cpp

@@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 using std::placeholders::_1;
 using std::placeholders::_2;
@@ -7,7 +7,7 @@ Ros2Aria::Ros2Aria()
     : Node("ros2aria"),
       sensorCb(this, &Ros2Aria::publish)
 {
-    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0"); // TODO: use args for port
+    this->robot = std::make_shared<RAIIBot>(this, "/dev/ttyS0");
 
     RCLCPP_INFO(this->get_logger(), "starting subscribers and services");
     cmd_vel_sub_ = this->create_subscription<geometry_msgs::msg::Twist>(

src/ros2aria/src/sonar.cpp

@@ -1,5 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
-
 #include "sensor_msgs/point_cloud_conversion.hpp"
 
 sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp)

src/ros2aria/src/state.cpp

@@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 void Ros2Aria::publishState(rclcpp::Time stamp)
 {

src/ros2aria/src/wheels.cpp

@@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
 {