old changes

2022-07-29 11:44:09 +00:00
22 changed files with 1189 additions and 160 deletions
--- a/Dockerfile.devcontainer
+++ b/Dockerfile.devcontainer
@ -4,7 +4,7 @@ FROM athackst/ros2:foxy-dev
 #
 ENV DEBIAN_FRONTEND=noninteractive
 RUN apt-get update \
-    && apt-get -y install --no-install-recommends rsync \
+    && apt-get -y install --no-install-recommends rsync ros-foxy-xacro ros-foxy-joint-state-publisher-gui ros-foxy-urg-node\
    #
    # Clean up
    && apt-get autoremove -y \
--- a/2
+++ b/2
@ -16,7 +16,7 @@ upload:
 # 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 --net host   -it --device /dev/ttyS0 -v /home/lab1_5:/ws  -v /dev/shm:/dev/shm irth7/ros2aria-dev /bin/bash /ws/ros2aria/run.sh

 legacy:
 	ssh lab1_5@pionier6 -t -- ./run.sh
--- a/src/pionier_description/CMakeLists.txt
+++ b/src/pionier_description/CMakeLists.txt
@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 3.5)
+project(pionier_description)
+
+# 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)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+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()
+
+install(
+  DIRECTORY  launch urdf meshes
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()
--- a/src/pionier_description/launch/pionier_description_bringup.launch.py
+++ b/src/pionier_description/launch/pionier_description_bringup.launch.py
@ -0,0 +1,57 @@
+import launch
+from launch.substitutions import Command, LaunchConfiguration
+import launch_ros
+import os
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    pkg_share = launch_ros.substitutions.FindPackageShare(package='pionier_description').find('pionier_description')
+    default_model_path = os.path.join(pkg_share, 'urdf/description.urdf.xacro')
+    # default_rviz_config_path = os.path.join(pkg_share, 'rviz/urdf_config.rviz')
+
+    robot_state_publisher_node = launch_ros.actions.Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        parameters=[{'robot_description': Command(['xacro ', LaunchConfiguration('model')])}]
+    )
+    joint_state_publisher_node = launch_ros.actions.Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        condition=launch.conditions.UnlessCondition(LaunchConfiguration('gui'))
+    )
+    joint_state_publisher_gui_node = launch_ros.actions.Node(
+        package='joint_state_publisher',
+        executable='joint_state_publisher',
+        name='joint_state_publisher',
+        # condition=launch.conditions.IfCondition(LaunchConfiguration('gui'))
+    )
+    # rviz_node = launch_ros.actions.Node(
+    #     package='rviz2',
+    #     executable='rviz2',
+    #     name='rviz2',
+    #     output='screen',
+    #     arguments=['-d', LaunchConfiguration('rvizconfig')],
+    # )
+
+    urg_node = Node(
+        package='urg_node',
+        executable='urg_node_driver',
+        emulate_tty=True,
+        parameters=[{'serial_port': '/dev/ttyACM0'}],
+        output='screen'
+    )
+
+    return launch.LaunchDescription([
+        urg_node,
+        launch.actions.DeclareLaunchArgument(name='gui', default_value='True',
+                                            description='Flag to enable joint_state_publisher_gui'),
+        launch.actions.DeclareLaunchArgument(name='model', default_value=default_model_path,
+                                            description='Absolute path to robot urdf file'),
+        # launch.actions.DeclareLaunchArgument(name='rvizconfig', default_value=default_rviz_config_path,
+        #                                     description='Absolute path to rviz config file'),
+        joint_state_publisher_node,
+        joint_state_publisher_gui_node,
+        robot_state_publisher_node,
+        # rviz_node
+    ])
--- a/src/pionier_description/meshes/d435.dae
+++ b/src/pionier_description/meshes/d435.dae
--- a/src/pionier_description/package.xml
+++ b/src/pionier_description/package.xml
@ -0,0 +1,18 @@
+<?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>pionier_description</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</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>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/src/pionier_description/urdf/description.urdf.xacro
+++ b/src/pionier_description/urdf/description.urdf.xacro
@ -0,0 +1,164 @@
+<?xml version="1.0"?>
+<robot name="sam_bot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <!-- Define robot constants -->
+  <xacro:property name="base_width" value="0.381"/>
+  <xacro:property name="base_length" value="0.455"/>
+  <xacro:property name="base_height" value="0.237"/>
+
+  <xacro:property name="wheel_radius" value="0.0975"/>
+  <xacro:property name="wheel_width" value="0.05"/>
+  <xacro:property name="wheel_ygap" value="0.025"/>
+  <xacro:property name="wheel_zoff" value="0.05"/>
+  <xacro:property name="wheel_xoff" value="0.0"/>
+
+  <xacro:property name="caster_xoff" value="0.15"/>
+
+  <!-- Define some commonly used intertial properties  -->
+  <xacro:macro name="box_inertia" params="m w h d">
+    <inertial>
+      <origin xyz="0 0 0" rpy="${pi/2} 0 ${pi/2}"/>
+      <mass value="${m}"/>
+      <inertia ixx="${(m/12) * (h*h + d*d)}" ixy="0.0" ixz="0.0" iyy="${(m/12) * (w*w + d*d)}" iyz="0.0" izz="${(m/12) * (w*w + h*h)}"/>
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="cylinder_inertia" params="m r h">
+    <inertial>
+      <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+      <mass value="${m}"/>
+      <inertia ixx="${(m/12) * (3*r*r + h*h)}" ixy = "0" ixz = "0" iyy="${(m/12) * (3*r*r + h*h)}" iyz = "0" izz="${(m/2) * (r*r)}"/>
+    </inertial>
+  </xacro:macro>
+
+  <xacro:macro name="sphere_inertia" params="m r">
+    <inertial>
+      <mass value="${m}"/>
+      <inertia ixx="${(2/5) * m * (r*r)}" ixy="0.0" ixz="0.0" iyy="${(2/5) * m * (r*r)}" iyz="0.0" izz="${(2/5) * m * (r*r)}"/>
+    </inertial>
+  </xacro:macro>
+
+  <!-- Robot Base -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}"/>
+      </geometry>
+      <material name="Cyan">
+        <color rgba="0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}"/>
+      </geometry>
+    </collision>
+
+    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}"/>
+  </link>
+
+   <!-- Robot Footprint -->
+  <link name="base_footprint">
+  	<xacro:box_inertia m="0" w="0" d="0" h="0"/>
+  </link>
+
+  <joint name="base_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_footprint"/>
+    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0"/>
+  </joint>
+
+  <!-- Wheels -->
+  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
+    <link name="${prefix}_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <geometry>
+            <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+        <material name="Gray">
+          <color rgba="0.5 0.5 0.5 1.0"/>
+        </material>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0"/>
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
+        </geometry>
+      </collision>
+
+      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}"/>
+    </link>
+
+    <joint name="${prefix}_joint" type="continuous">
+      <parent link="base_link"/>
+      <child link="${prefix}_link"/>
+      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}" rpy="0 0 0"/>
+      <axis xyz="0 1 0"/>
+    </joint>
+  </xacro:macro>
+
+  <xacro:wheel prefix="drivewhl_l" x_reflect="-1" y_reflect="1" />
+  <xacro:wheel prefix="drivewhl_r" x_reflect="-1" y_reflect="-1" />
+
+  <!-- <link name="front_caster">
+    <visual>
+      <geometry>
+        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
+      </geometry>
+      <material name="Cyan">
+        <color rgba="0 1.0 1.0 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
+      </geometry>
+    </collision>
+
+    <xacro:sphere_inertia m="0.5" r="${(wheel_radius+wheel_zoff-(base_height/2))}"/>
+  </link> -->
+
+  <!-- <joint name="caster_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="front_caster"/>
+    <origin xyz="${caster_xoff} 0.0 ${-(base_height/2)}" rpy="0 0 0"/>
+  </joint> -->
+
+
+
+
+
+
+
+
+  <joint name="lidar_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="laser"/>
+    <origin xyz="0.14 0 ${base_height + 0.065}" rpy="0 0 0"/>
+  </joint>
+
+  <link name="laser">
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <box size="0.1 0.1 0.1"/>
+        </geometry>
+      </collision>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="file://$(find pionier_description)/meshes/hokuyo.dae"/>
+        </geometry>
+      </visual>
+      <inertial>
+        <mass value="1e-5" />
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
+      </inertial>
+    </link>
+</robot>
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@ -50,7 +50,8 @@ ament_target_dependencies(ros2aria nav_msgs)
 ament_target_dependencies(ros2aria tf2)
 ament_target_dependencies(ros2aria ros2aria_msgs)

-target_include_directories(ros2aria PUBLIC
+target_include_directories(ros2aria PRIVATE
+  include
  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
  $<INSTALL_INTERFACE:include>
  /usr/local/Aria/include)
--- a/src/ros2aria/include/ros2aria/raiibot.hpp
+++ b/src/ros2aria/include/ros2aria/raiibot.hpp
@ -0,0 +1,41 @@
+#pragma once
+#include <string>
+#include <memory>
+#include <iostream>
+
+#include "Aria/Aria.h"
+#include "rclcpp/rclcpp.hpp"
+
+using namespace std::chrono_literals;
+
+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();
+};
--- a/src/ros2aria/include/ros2aria/ros2aria.hpp
+++ b/src/ros2aria/include/ros2aria/ros2aria.hpp
@ -1,18 +1,20 @@
-#include "rclcpp/rclcpp.hpp"
 #include "Aria/Aria.h"
-
-#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/transform.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 "raiibot.hpp"
+#include "rclcpp/rclcpp.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"
+#include "tf2/transform_datatypes.h"
+#include "tf2/utils.h"
+#include "tf2_ros/transform_broadcaster.h"

 #define UNUSED(x) (void)(x)

@ -43,6 +45,9 @@ private:
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
    void publishPose(nav_msgs::msg::Odometry pose);

+    void sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform);
+    std::unique_ptr<tf2_ros::TransformBroadcaster> odom_tf_broadcaster_;
+
    sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
    void publishWheels(sensor_msgs::msg::JointState wheels);
@ -61,7 +66,7 @@ private:

    // 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;
+    void stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr 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;
--- a/src/ros2aria/package.xml
+++ b/src/ros2aria/package.xml
@ -17,6 +17,8 @@
  <depend>geometry_msgs</depend>
  <depend>sensor_msgs</depend>
  <depend>nav_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
  <depend>ros2aria_msgs</depend>

  <export>
--- a/src/ros2aria/src/clutch.cpp
+++ b/src/ros2aria/src/clutch.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 void Ros2Aria::clutch_callback(const std_msgs::msg::Bool::SharedPtr msg)
 {
--- a/src/ros2aria/src/cmd_vel.cpp
+++ b/src/ros2aria/src/cmd_vel.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
 {
@ -14,7 +14,8 @@ void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)

    auto r = robot->getRobot();
    r->setVel(x * 1e3);
-    if (r->hasLatVel())
+    if (r->hasLatVel()){
        r->setLatVel(y * 1e3);
+    }
    r->setRotVel(z * 180 / M_PI);
 }
--- a/src/ros2aria/src/gripper.cpp
+++ b/src/ros2aria/src/gripper.cpp
@ -1,4 +1,4 @@
-#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
 {
--- a/src/ros2aria/src/main.cpp
+++ b/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)
 {
--- a/src/ros2aria/src/pose.cpp
+++ b/src/ros2aria/src/pose.cpp
@ -1,6 +1,5 @@
-#include "ros2aria.hpp"
-#include "tf2/utils.h"
-#include "tf2/transform_datatypes.h"
+#include "ros2aria/ros2aria.hpp"
+

 inline void convert_transform_to_pose(const tf2::Transform &trans, geometry_msgs::msg::Pose &pose)
 {
@ -30,13 +29,23 @@ nav_msgs::msg::Odometry Ros2Aria::handlePose(rclcpp::Time stamp)
    pose.twist.twist.linear.y = r->getLatVel() / 1000;
    pose.twist.twist.angular.z = r->getRotVel() * M_PI / 180;

-    pose.header.frame_id = "header.frame_id"; // TODO: use correct frame_id
-    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_frame_id
+    pose.header.frame_id = "odom"; // TODO: use correct frame_id
+    pose.child_frame_id = "base_link";   // TODO: use correct child_frame_id
    pose.header.stamp = stamp;

+    sendOdomTransform(stamp, geometry_msgs::msg::Transform(rotation, position));
    return pose;
 }

+void Ros2Aria::sendOdomTransform(rclcpp::Time stamp, tf2::Transform transform){
+    geometry_msgs::msg::TransformStamped t;
+    t.header.stamp = stamp;
+    t.header.frame_id = "odom";
+    t.child_frame_id = "base_link";
+    t.transform = transform;
+    odom_tf_broadcaster_->sendTransform(t);
+}
+
 void Ros2Aria::publishPose(nav_msgs::msg::Odometry pose)
 {
    if (this->pose_pub_->get_subscription_count() == 0)
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 void Ros2Aria::publish()
 {
--- a/src/ros2aria/src/raiibot.cpp
+++ b/src/ros2aria/src/raiibot.cpp
@ -1,19 +1,8 @@
-#include <string>
-#include <memory>
-#include <iostream>
-
-#include "Aria/Aria.h"
-#include "rclcpp/rclcpp.hpp"
+#include "ros2aria/raiibot.hpp"

 using namespace std::chrono_literals;

-class RAIIBot
-{
-public:
-    typedef std::shared_ptr<RAIIBot> SharedPtr;
-
-    RAIIBot(rclcpp::Node *node, std::string port)
-    {
+RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
    this->robot = new ArRobot();
    this->gripper = new ArGripper(this->robot);
    this->node = node;
@ -29,10 +18,8 @@ public:
    // 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)");
+    if (!this->robotConn->connectRobot()) {
+        RCLCPP_ERROR(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();
    }

@ -40,23 +27,20 @@ public:
    this->robot->enableMotors();

    this->startWatchdog();
-    }
+}

-    ~RAIIBot()
-    {
+RAIIBot::~RAIIBot() {
    std::cout << std::endl
              << "RAIIBOT DESTRUCTOR RUNNING!" << std::endl;

-        if (this->robot != nullptr)
-        {
+    if (this->robot != nullptr) {
        this->robot->lock();
        std::cout << "disabling motors" << std::endl;
        this->robot->disableMotors();
        std::cout << "disabled motors" << std::endl;
        Aria::shutdown();
    }
-        if (this->robotConn != nullptr)
-        {
+    if (this->robotConn != nullptr) {
        std::cout << "disconnecting" << std::endl;
        this->robotConn->disconnectAll();
        std::cout << "disconnected" << std::endl;
@ -70,68 +54,47 @@ public:
        delete this->robotConn;
    if (this->robot != nullptr)
        delete this->robot;
-    }
+}

-    ArRobot *getRobot()
-    {
+ArRobot *RAIIBot::getRobot() {
    return this->robot;
-    }
+}

-    ArGripper *getGripper()
-    {
+ArGripper *RAIIBot::getGripper() {
    return this->gripper;
-    }
+}

-    rclcpp::Clock::SharedPtr getClock()
-    {
+rclcpp::Clock::SharedPtr RAIIBot::getClock() {
    return this->clock;
-    }
+}

-    void lock()
-    {
+void RAIIBot::lock() {
    if (this->robot != nullptr)
        this->robot->lock();
-    }
+}

-    void unlock()
-    {
+void RAIIBot::unlock() {
    if (this->robot != nullptr)
        this->robot->unlock();
-    }
+}

-    void pokeWatchdog()
-    {
+void RAIIBot::pokeWatchdog() {
    // this should probably be made thread safe, or something?
    this->lastWatchdogPoke = this->clock->now();
-    }
+}

-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()
-    {
+rclcpp::Logger RAIIBot::get_logger() {
    return this->node->get_logger();
-    }
+}

-    void startWatchdog()
-    {
+void RAIIBot::startWatchdog() {
    this->lastWatchdogPoke = clock->now();
    watchdogTimer = this->node->create_wall_timer(0.1s, std::bind(&RAIIBot::watchdog_cb, this));
-    }
+}

-    void watchdog_cb()
-    {
+void RAIIBot::watchdog_cb() {
    auto now = this->clock->now();
-        if (now - this->lastWatchdogPoke > 1s)
-        {
+    if (now - this->lastWatchdogPoke > 1s) {
        std::lock_guard<RAIIBot> lock(*this);
        auto r = this->getRobot();
        r->setVel(0);
@ -139,5 +102,4 @@ private:
        if (r->hasLatVel())
            r->setLatVel(0);
    }
-    }
-};
+}
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 using std::placeholders::_1;
 using std::placeholders::_2;
@ -19,6 +19,7 @@ Ros2Aria::Ros2Aria()
    sonar_pointcloud2_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("sonar_pointcloud2", 10);

    pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
+    odom_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(*this);

    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);

@ -46,8 +47,7 @@ Ros2Aria::~Ros2Aria()
    r->remSensorInterpTask("ROSPublishingTask");
 }

-void Ros2Aria::stop(const std::shared_ptr<std_srvs::srv::Empty::Request> request, std::shared_ptr<std_srvs::srv::Empty::Response> response) const
-{
+void Ros2Aria::stop(const std_srvs::srv::Empty::Response::SharedPtr request, std_srvs::srv::Empty::Response::SharedPtr response) const {
    UNUSED(request);
    UNUSED(response);
    RCLCPP_INFO(this->get_logger(), "stop");
--- a/src/ros2aria/src/sonar.cpp
+++ b/src/ros2aria/src/sonar.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 #include "sensor_msgs/point_cloud_conversion.hpp"

--- a/src/ros2aria/src/state.cpp
+++ b/src/ros2aria/src/state.cpp
@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"

 void Ros2Aria::publishState(rclcpp::Time stamp)
 {
@ -31,9 +31,7 @@ void Ros2Aria::publishState(rclcpp::Time stamp)
        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
+        // 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
--- a/src/ros2aria/src/wheels.cpp
+++ b/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)
 {