From 92d96f94a6ef422e30b764b8c1218038003c42e8 Mon Sep 17 00:00:00 2001
From: Wojciech Kwolek <me@irth.pl>
Date: Thu, 20 May 2021 12:01:49 +0000
Subject: [PATCH] implement wheels

---
 Makefile                      |  5 +++--
 src/ros2aria/CMakeLists.txt   |  3 ++-
 src/ros2aria/src/publish.cpp  |  3 +++
 src/ros2aria/src/ros2aria.cpp |  2 ++
 src/ros2aria/src/ros2aria.hpp |  5 +++++
 src/ros2aria/src/wheels.cpp   | 32 ++++++++++++++++++++++++++++++++
 6 files changed, 47 insertions(+), 3 deletions(-)
 create mode 100644 src/ros2aria/src/wheels.cpp

diff --git a/Makefile b/Makefile
index 3db77ce..e767f9f 100644
--- a/Makefile
+++ b/Makefile
@@ -1,4 +1,4 @@
-.PHONY: build run upload
+.PHONY: build run upload legacy
 
 SOURCES := $(wildcard src/ros2aria/src/*.cpp)
 HEADERS := $(wildcard src/ros2aria/src/*.hpp)
@@ -17,4 +17,5 @@ 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 /ws/ros2aria
 
-
+legacy:
+	ssh lab1_5@pionier6 -t -- ./run.sh
\ No newline at end of file
diff --git a/src/ros2aria/CMakeLists.txt b/src/ros2aria/CMakeLists.txt
index 0c9dbcd..9aeebaa 100644
--- a/src/ros2aria/CMakeLists.txt
+++ b/src/ros2aria/CMakeLists.txt
@@ -36,7 +36,8 @@ add_executable(ros2aria
   src/publish.cpp
   src/sonar.cpp
   src/pose.cpp
-  src/gripper.cpp)
+  src/gripper.cpp
+  src/wheels.cpp)
   
 ament_target_dependencies(ros2aria rclcpp)
 ament_target_dependencies(ros2aria std_srvs)
diff --git a/src/ros2aria/src/publish.cpp b/src/ros2aria/src/publish.cpp
index 2cda932..3b8353d 100644
--- a/src/ros2aria/src/publish.cpp
+++ b/src/ros2aria/src/publish.cpp
@@ -11,4 +11,7 @@ void Ros2Aria::publish()
 
     nav_msgs::msg::Odometry pose = handlePose(t);
     publishPose(pose);
+
+    sensor_msgs::msg::JointState wheels = handleWheels(t);
+    publishWheels(wheels);
 }
\ No newline at end of file
diff --git a/src/ros2aria/src/ros2aria.cpp b/src/ros2aria/src/ros2aria.cpp
index 334203f..d692237 100644
--- a/src/ros2aria/src/ros2aria.cpp
+++ b/src/ros2aria/src/ros2aria.cpp
@@ -18,6 +18,8 @@ Ros2Aria::Ros2Aria()
 
     pose_pub_ = this->create_publisher<nav_msgs::msg::Odometry>("pose", 1000);
 
+    wheels_pub_ = this->create_publisher<sensor_msgs::msg::JointState>("wheels", 1000);
+
     // services
     stop_service_ = this->create_service<std_srvs::srv::Empty>("stop", std::bind(&Ros2Aria::stop, this, _1, _2));
     gripper_open_service_ = this->create_service<std_srvs::srv::Empty>("gripper_open", std::bind(&Ros2Aria::gripper_open_callback, this, _1, _2));
diff --git a/src/ros2aria/src/ros2aria.hpp b/src/ros2aria/src/ros2aria.hpp
index fb15bfa..3d9783a 100644
--- a/src/ros2aria/src/ros2aria.hpp
+++ b/src/ros2aria/src/ros2aria.hpp
@@ -3,6 +3,7 @@
 
 #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"
@@ -38,6 +39,10 @@ private:
     rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr pose_pub_;
     void publishPose(nav_msgs::msg::Odometry pose);
 
+    sensor_msgs::msg::JointState handleWheels(rclcpp::Time stamp);
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr wheels_pub_;
+    void publishWheels(sensor_msgs::msg::JointState wheels);
+
     // subscribers
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
     void cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg);
diff --git a/src/ros2aria/src/wheels.cpp b/src/ros2aria/src/wheels.cpp
new file mode 100644
index 0000000..5292786
--- /dev/null
+++ b/src/ros2aria/src/wheels.cpp
@@ -0,0 +1,32 @@
+#include "ros2aria.hpp"
+
+sensor_msgs::msg::JointState Ros2Aria::handleWheels(rclcpp::Time stamp)
+{
+    sensor_msgs::msg::JointState wheels;
+
+    auto r = this->robot->getRobot();
+
+    r->requestEncoderPackets(); // TODO: check if this is synchronous or do we have a race condition or something
+    wheels.header.stamp = stamp;
+    wheels.name.resize(2);
+    wheels.position.resize(2);
+    wheels.velocity.resize(2);
+    wheels.effort.resize(0);
+
+    wheels.name[0] = "Wheel_L";
+    wheels.name[1] = "Wheel_R";
+    wheels.position[0] = r->getLeftEncoder();
+    wheels.position[1] = r->getRightEncoder();
+    wheels.velocity[0] = r->getLeftVel();
+    wheels.velocity[1] = r->getRightVel();
+
+    return wheels;
+}
+
+void Ros2Aria::publishWheels(sensor_msgs::msg::JointState wheels)
+{
+    if (this->wheels_pub_->get_subscription_count() == 0)
+        return;
+
+    this->wheels_pub_->publish(wheels);
+}
\ No newline at end of file