From 4f2a9046df87c3f60adf97a82a83144666f8c538 Mon Sep 17 00:00:00 2001 From: Wojciech Kwolek Date: Wed, 21 Apr 2021 08:53:34 +0000 Subject: [PATCH] read sonar data; split Ros2Aria into multiple files --- .vscode/settings.json | 5 +- src/ros2aria/CMakeLists.txt | 11 +++- src/ros2aria/package.xml | 1 + src/ros2aria/src/cmd_vel.cpp | 20 +++++++ src/ros2aria/src/main.cpp | 18 +++++++ src/ros2aria/src/publish.cpp | 8 +++ src/ros2aria/src/raiibot.cpp | 5 ++ src/ros2aria/src/ros2aria.cpp | 99 +++++------------------------------ src/ros2aria/src/ros2aria.hpp | 35 +++++++++++++ src/ros2aria/src/sonar.cpp | 44 ++++++++++++++++ 10 files changed, 159 insertions(+), 87 deletions(-) create mode 100644 src/ros2aria/src/cmd_vel.cpp create mode 100644 src/ros2aria/src/main.cpp create mode 100644 src/ros2aria/src/publish.cpp create mode 100644 src/ros2aria/src/ros2aria.hpp create mode 100644 src/ros2aria/src/sonar.cpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 120d6fc..4831153 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -62,5 +62,8 @@ "cinttypes": "cpp", "typeindex": "cpp", "typeinfo": "cpp" - } + }, + "cSpell.words": [ + "rclcpp" + ] } \ No newline at end of file diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt index 3d859a6..7afc7ac 100644 --- a/src/ros2aria/CMakeLists.txt +++ b/src/ros2aria/CMakeLists.txt @@ -20,15 +20,24 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_srvs REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) -add_executable(ros2aria src/ros2aria.cpp src/raiibot.cpp) +add_executable(ros2aria + src/main.cpp + src/ros2aria.cpp + src/raiibot.cpp + src/cmd_vel.cpp + src/publish.cpp + src/sonar.cpp) + ament_target_dependencies(ros2aria rclcpp) ament_target_dependencies(ros2aria std_srvs) ament_target_dependencies(ros2aria geometry_msgs) +ament_target_dependencies(ros2aria sensor_msgs) target_include_directories(ros2aria PUBLIC $ $ diff --git a/src/ros2aria/package.xml b/src/ros2aria/package.xml index ac2ad23..2aa283e 100644 --- a/src/ros2aria/package.xml +++ b/src/ros2aria/package.xml @@ -15,6 +15,7 @@ rclcpp std_srvs geometry_msgs + sensor_msgs ament_cmake diff --git a/src/ros2aria/src/cmd_vel.cpp b/src/ros2aria/src/cmd_vel.cpp new file mode 100644 index 0000000..0f829e5 --- /dev/null +++ b/src/ros2aria/src/cmd_vel.cpp @@ -0,0 +1,20 @@ +#include "ros2aria.hpp" + +void Ros2Aria::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) +{ + float x, y, z; + x = msg->linear.x; + y = msg->linear.y; + z = msg->angular.z; + RCLCPP_INFO(this->get_logger(), "cmd_vel: x:%f y:%f z:%f", x, y, z); + + std::lock_guard lock(*robot.get()); + + this->robot->pokeWatchdog(); + + auto r = robot->getRobot(); + r->setVel(x * 1e3); + if (r->hasLatVel()) + r->setLatVel(y * 1e3); + r->setRotVel(z * 180 / M_PI); +} diff --git a/src/ros2aria/src/main.cpp b/src/ros2aria/src/main.cpp new file mode 100644 index 0000000..1947004 --- /dev/null +++ b/src/ros2aria/src/main.cpp @@ -0,0 +1,18 @@ +#include +#include + +#include "ros2aria.hpp" + +int main(int argc, char **argv) +{ + (void)argc; + (void)argv; + + printf("hello world ros2aria package\n"); + + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp new file mode 100644 index 0000000..8d13e53 --- /dev/null +++ b/src/ros2aria/src/publish.cpp @@ -0,0 +1,8 @@ +#include "ros2aria.hpp" + +void Ros2Aria::publish() +{ + // RCLCPP_INFO(this->get_logger(), "publish"); + rclcpp::Time t = robot->getClock()->now(); + sensor_msgs::msg::PointCloud sonarData = handleSonar(t); +} \ No newline at end of file diff --git a/src/ros2aria/src/raiibot.cpp b/src/ros2aria/src/raiibot.cpp index faa68f6..00b8307 100644 --- a/src/ros2aria/src/raiibot.cpp +++ b/src/ros2aria/src/raiibot.cpp @@ -73,6 +73,11 @@ public: return this->robot; } + rclcpp::Clock::SharedPtr getClock() + { + return this->clock; + } + void lock() { if (this->robot != nullptr) diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp index 24ef17c..e8da64c 100644 --- a/src/ros2aria/src/ros2aria.cpp +++ b/src/ros2aria/src/ros2aria.cpp @@ -1,27 +1,13 @@ -#include -#include - -#include "Aria/Aria.h" - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_srvs/srv/empty.hpp" -#include "geometry_msgs/msg/twist.hpp" - -#include "./raiibot.cpp" +#include "ros2aria.hpp" using std::placeholders::_1; using std::placeholders::_2; -using namespace std::chrono_literals; -class Ros2Aria : public rclcpp::Node +Ros2Aria::Ros2Aria() + : Node("ros2aria"), + sensorCb(this, &Ros2Aria::publish) { -public: - Ros2Aria() - : Node("ros2aria"), - sensorCb(this, &Ros2Aria::publish) - { - this->robot = std::make_shared(this, "/dev/ttyS0"); + this->robot = std::make_shared(this, "/dev/ttyS0"); // TODO: use args for port RCLCPP_INFO(this->get_logger(), "starting subscribers and services"); cmd_vel_sub_ = this->create_subscription( @@ -31,75 +17,18 @@ public: // listen to sensors auto r = robot->getRobot(); r->addSensorInterpTask("ROSPublishingTask", 100, &this->sensorCb); - } - ~Ros2Aria() - { + sonar_frame_id = "pionier6"; // TODO: use args +} + +Ros2Aria::~Ros2Aria() +{ auto r = robot->getRobot(); r->remSensorInterpTask("ROSPublishingTask"); - } +} - void publish() - { - RCLCPP_INFO(this->get_logger(), "publish"); - } - -private: - RAIIBot::SharedPtr robot; - - ArFunctorC sensorCb; - - void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg) - { - float x, y, z; - x = msg->linear.x; - y = msg->linear.y; - z = msg->angular.z; - RCLCPP_INFO(this->get_logger(), "cmd_vel: x:%f y:%f z:%f", x, y, z); - - std::lock_guard lock(*robot.get()); - - this->robot->pokeWatchdog(); - - auto r = robot->getRobot(); - r->setVel(x * 1e3); - if (r->hasLatVel()) - r->setLatVel(y * 1e3); - r->setRotVel(z * 180 / M_PI); - } - - rclcpp::Subscription::SharedPtr cmd_vel_sub_; - - void stop(const std::shared_ptr request, std::shared_ptr response) const - { +void Ros2Aria::stop(const std::shared_ptr request, std::shared_ptr response) const +{ RCLCPP_INFO(this->get_logger(), "stop"); rclcpp::shutdown(); - } - rclcpp::Service::SharedPtr stop_service_; -}; - -int main(int argc, char **argv) -{ - (void)argc; - (void)argv; - - printf("hello world ros2aria package\n"); - // printf("Enabled motors\n"); - // robot->setVel(2); - // robot->move(500); - - // sleep(10); - - // robot->move(-500); - // sleep(2); - // conn->disconnectAll(); - - // return 0; - - // return 0; - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - - return 0; -} +} \ No newline at end of file diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp new file mode 100644 index 0000000..3ed55a3 --- /dev/null +++ b/src/ros2aria/src/ros2aria.hpp @@ -0,0 +1,35 @@ +#include "rclcpp/rclcpp.hpp" +#include "Aria/Aria.h" + +#include "sensor_msgs/msg/point_cloud.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "std_srvs/srv/empty.hpp" + +#include "./raiibot.cpp" + +class Ros2Aria : public rclcpp::Node +{ +public: + Ros2Aria(); + ~Ros2Aria(); + +private: + // robot + RAIIBot::SharedPtr robot; + ArFunctorC sensorCb; + + // config + std::string sonar_frame_id; + + // publishers + void publish(); + sensor_msgs::msg::PointCloud handleSonar(rclcpp::Time stamp); + + // subscribers + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg); + + // services + rclcpp::Service::SharedPtr stop_service_; + void stop(const std::shared_ptr request, std::shared_ptr response) const; +}; \ No newline at end of file diff --git a/src/ros2aria/src/sonar.cpp b/src/ros2aria/src/sonar.cpp new file mode 100644 index 0000000..01cf463 --- /dev/null +++ b/src/ros2aria/src/sonar.cpp @@ -0,0 +1,44 @@ +#include "ros2aria.hpp" + +sensor_msgs::msg::PointCloud Ros2Aria::handleSonar(rclcpp::Time stamp) +{ + + sensor_msgs::msg::PointCloud cloud; //sonar readings. + cloud.header.stamp = stamp; + // sonar sensors relative to base_link + cloud.header.frame_id = this->sonar_frame_id; + + auto r = robot->getRobot(); + + for (int i = 0; i < r->getNumSonar(); i++) + { + ArSensorReading *reading = NULL; + reading = r->getSonarReading(i); + if (!reading) + { + RCLCPP_INFO(this->get_logger(), "did not receive a sonar reading."); + continue; + } + + // getRange() will return an integer between 0 and 5000 (5m) + std::cout << reading->getRange() << " "; + + // local (x,y). Appears to be from the centre of the robot, since values may + // exceed 5000. This is good, since it means we only need 1 transform. + // x & y seem to be swapped though, i.e. if the robot is driving north + // x is north/south and y is east/west. + // + //ArPose sensor = reading->getSensorPosition(); //position of sensor. + // sonar_debug_info << "(" << reading->getLocalX() + // << ", " << reading->getLocalY() + // << ") from (" << sensor.getX() << ", " + // << sensor.getY() << ") ;; " ; + //add sonar readings (robot-local coordinate frame) to cloud + geometry_msgs::msg::Point32 p; + p.x = reading->getLocalX() / 1000.0; + p.y = reading->getLocalY() / 1000.0; + p.z = 0.0; + cloud.points.push_back(p); + } + return cloud; +}