implement wheels

closes #5

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

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)

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

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

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

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