old changes

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 \

Makefile

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

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()

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
+    ])

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>

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>

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)

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();
+};

src/ros2aria/include/ros2aria/ros2aria.hpp

@@ -1,18 +1,20 @@
-#include "rclcpp/rclcpp.hpp"
 #include "Aria/Aria.h"
-
+#include "geometry_msgs/msg/transform.hpp"
-#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 "raiibot.hpp"
-#include "std_msgs/msg/bool.hpp"
+#include "rclcpp/rclcpp.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"
+#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;

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>

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)
 {

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);
 }

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
 {

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,6 +1,5 @@
-#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)
 {
@@ -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.header.frame_id = "odom"; // TODO: use correct frame_id
-    pose.child_frame_id = "child_frame_id";   // TODO: use correct child_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)

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,19 +1,8 @@
-#include <string>
+#include "ros2aria/raiibot.hpp"
-#include <memory>
-#include <iostream>
-
-#include "Aria/Aria.h"
-#include "rclcpp/rclcpp.hpp"
 
 using namespace std::chrono_literals;
 
-class RAIIBot
+RAIIBot::RAIIBot(rclcpp::Node *node, std::string port) {
-{
-public:
-    typedef std::shared_ptr<RAIIBot> SharedPtr;
-
-    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())
+    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_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();
     }
 
@@ -42,21 +29,18 @@ public:
     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;
@@ -72,66 +56,45 @@ public:
         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:
+rclcpp::Logger RAIIBot::get_logger() {
-    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()
-    {
     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);
@@ -140,4 +103,3 @@ private:
             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;
@@ -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");

src/ros2aria/src/sonar.cpp

@@ -1,4 +1,4 @@
-#include "ros2aria.hpp"
+#include "ros2aria/ros2aria.hpp"
 
 #include "sensor_msgs/point_cloud_conversion.hpp"
 

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

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)
 {