ROS2 project setup

diff --git a/Makefile b/Makefile
new file mode 100644 (file)
index 0000000..81ed6d5
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,11 @@
+.PHONY = build
+
+SHELL := /bin/bash
+
+run:
+       colcon build
+       source install/setup.bash && ros2 run pioneer3DX_controller Pioneer3DX_Controller
+
+debug:
+       colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo
+       source install/setup.bash && ros2 run --prefix 'gdb -ex start --args' pioneer3DX_controller Pioneer3DX_Controller

diff --git a/src/pioneer3DX_controller/CMakeLists.txt b/src/pioneer3DX_controller/CMakeLists.txt
new file mode 100644 (file)
index 0000000..afa82c8
--- /dev/null
+++ b/src/pioneer3DX_controller/CMakeLists.txt
@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(pioneer3DX_controller)
+
+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(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+
+add_executable(Pioneer3DX_Controller src/main.cpp src/p3dx_node.cpp)
+ament_target_dependencies(Pioneer3DX_Controller rclcpp nav_msgs)
+install(TARGETS Pioneer3DX_Controller DESTINATION lib/${PROJECT_NAME})
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

diff --git a/src/pioneer3DX_controller/package.xml b/src/pioneer3DX_controller/package.xml
new file mode 100644 (file)
index 0000000..ab2ebe4
--- /dev/null
+++ b/src/pioneer3DX_controller/package.xml
@@ -0,0 +1,20 @@
+<?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>pioneer3DX_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="leo@todo.todo">leo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

diff --git a/src/pioneer3DX_controller/src/main.cpp b/src/pioneer3DX_controller/src/main.cpp
new file mode 100644 (file)
index 0000000..9e4284d
--- /dev/null
+++ b/src/pioneer3DX_controller/src/main.cpp
@@ -0,0 +1,41 @@
+#include <cstdint>
+#include <cstdio>
+
+#include "main.hpp"
+#include "p3dx_node.hpp"
+
+int32_t main(int32_t argc, const char *argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<P3DX_Controller_Node>());
+  rclcpp::shutdown();
+}
+
+[[nodiscard]] int32_t serial_open(const char *portname)
+{
+  int32_t fd = open(portname, O_RDWR | O_NOCTTY);
+  assert(fd >= 0);
+
+  struct termios tty = {};
+  tcgetattr(fd, &tty);
+
+  cfsetispeed(&tty, B115200);
+  cfsetospeed(&tty, B115200);
+
+  tty.c_cflag &= (tcflag_t)~PARENB;
+  tty.c_cflag &= (tcflag_t)~CSTOPB;
+  tty.c_cflag &= (tcflag_t)~CRTSCTS;
+  tty.c_cflag |= CS8 | CLOCAL | CREAD;
+
+  tty.c_lflag = 0;
+  tty.c_iflag = 0;
+  tty.c_oflag = 0;
+
+  tty.c_cc[VMIN]  = 1;
+  tty.c_cc[VTIME] = 0;
+
+  tcflush(fd, TCIOFLUSH);
+  tcsetattr(fd, TCSANOW, &tty);
+
+  return fd;
+}

diff --git a/src/pioneer3DX_controller/src/main.hpp b/src/pioneer3DX_controller/src/main.hpp
new file mode 100644 (file)
index 0000000..fba410c
--- /dev/null
+++ b/src/pioneer3DX_controller/src/main.hpp
@@ -0,0 +1,9 @@
+#ifndef MAIN_HPP
+#define MAIN_HPP
+
+#include <termios.h>
+#include <fcntl.h>
+
+[[nodiscard]] int32_t serial_open(const char *portname);
+
+#endif

diff --git a/src/pioneer3DX_controller/src/p3dx_node.cpp b/src/pioneer3DX_controller/src/p3dx_node.cpp
new file mode 100644 (file)
index 0000000..661d4a0
--- /dev/null
+++ b/src/pioneer3DX_controller/src/p3dx_node.cpp
@@ -0,0 +1,46 @@
+#include "p3dx_node.hpp"
+
+using namespace std::chrono_literals;
+
+P3DX_Controller_Node::P3DX_Controller_Node() :
+  rclcpp::Node("P3DX_Controller_Node"), serial_fd(serial_open("/dev/ttyACM0")),
+  publisher_odometry(this->create_publisher<nav_msgs::msg::Odometry>("odometry", 10)),
+  subscriber_command_velocity(this->create_subscription<geometry_msgs::msg::Twist>("p3dx_command_velocity", 10,
+                                                                                   std::bind(&P3DX_Controller_Node::callback_subscribe_command_velocity,
+                                                                                             this, std::placeholders::_1))),
+  publisher_odometry_timer(this->create_wall_timer(50ms, std::bind(&P3DX_Controller_Node::callback_publish_odometry, this)))
+{
+  this->declare_parameter("frame_id_odometry", std::string("p3dx_controller_odometry"));
+  assert(this->serial_fd != -1);
+}
+
+void P3DX_Controller_Node::callback_publish_odometry(void)
+{
+  nav_msgs::msg::Odometry output;
+  output.header.frame_id = this->get_parameter("frame_id_odometry").as_string();
+  // output.header.stamp = ;            // TODO(lb): generate some kind of current timestamp
+
+  // output.pose.pose.position.x = ;    // TODO(lb): fill with pioneer data
+  // output.pose.pose.position.y = ;    // TODO(lb): fill with pioneer data
+  // output.pose.pose.orientation.x = ; // TODO(lb): fill with pioneer data
+  // output.pose.pose.orientation.y = ; // TODO(lb): fill with pioneer data
+
+  // output.twist.twist.linear.x = ;  // TODO(lb): fill with pioneer data
+  // output.twist.twist.linear.y = ;  // TODO(lb): fill with pioneer data
+  // output.twist.twist.angular.x = ; // TODO(lb): fill with pioneer data
+  // output.twist.twist.angular.y = ; // TODO(lb): fill with pioneer data
+
+  assert(output.pose.pose.position.z == 0);
+  assert(output.pose.pose.orientation.z == 0);
+  assert(output.pose.pose.orientation.w == 1);
+  assert(output.twist.twist.linear.z == 0);
+  assert(output.twist.twist.angular.z == 0);
+
+  this->publisher_odometry->publish(output);
+  RCLCPP_INFO(this->get_logger(), "Hello, World!");
+}
+
+void P3DX_Controller_Node::callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd)
+{
+  (void)cmd;
+}

diff --git a/src/pioneer3DX_controller/src/p3dx_node.hpp b/src/pioneer3DX_controller/src/p3dx_node.hpp
new file mode 100644 (file)
index 0000000..164a193
--- /dev/null
+++ b/src/pioneer3DX_controller/src/p3dx_node.hpp
@@ -0,0 +1,21 @@
+#ifndef P3DX_NODE_HPP
+#define P3DX_NODE_HPP
+
+#include <rclcpp/rclcpp.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+
+#include "main.hpp"
+
+struct P3DX_Controller_Node: public rclcpp::Node {
+  const int32_t serial_fd;
+  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr publisher_odometry;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr subscriber_command_velocity;
+  rclcpp::TimerBase::SharedPtr publisher_odometry_timer;
+
+  P3DX_Controller_Node(void);
+
+  void callback_publish_odometry(void);
+  void callback_subscribe_command_velocity(const geometry_msgs::msg::Twist::SharedPtr cmd);
+};
+
+#endif