Improve drivetrain calculations

messages/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED)
 # Generate messages
 rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ControllerInput.msg"
+  "msg/WheelStates.msg"
 )
 
 ament_export_dependencies(rosidl_default_runtime)

messages/msg/WheelStates.msg

@@ -0,0 +1,2 @@
+float64[4] speeds
+float64[4] angles

slugbot_package/CMakeLists.txt

@@ -56,6 +56,7 @@ ament_target_dependencies(
   rclcpp
   webots_ros2_driver
   std_msgs
+  messages
 )
 install(TARGETS
   ${PROJECT_NAME}
@@ -77,6 +78,16 @@ install(TARGETS
   slugbot_driver_node
   DESTINATION lib/${PROJECT_NAME}
 )
+
+add_executable(drivetrain_node src/Drivetrain.cpp)
+ament_target_dependencies(drivetrain_node
+  rclcpp
+  std_msgs
+)
+install(TARGETS
+  drivetrain_node
+  DESTINATION lib/${PROJECT_NAME}
+)
 # Install additional directories.
 install(DIRECTORY
   launch
@@ -85,10 +96,34 @@ install(DIRECTORY
   protos
   DESTINATION share/${PROJECT_NAME}/
 )
+# Geometry utilities library from repository utils
+add_library(slugbot_geometry STATIC
+  ${CMAKE_SOURCE_DIR}/../utils/math/Translation2d.cpp
+  ${CMAKE_SOURCE_DIR}/../utils/math/Rotation2d.cpp
+  ${CMAKE_SOURCE_DIR}/../utils/math/Pose2d.cpp
+)
+target_include_directories(slugbot_geometry PUBLIC
+  ${CMAKE_SOURCE_DIR}/../utils
+)
+target_compile_features(slugbot_geometry PUBLIC cxx_std_14)
+install(TARGETS
+  slugbot_geometry
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+ament_export_libraries(slugbot_geometry)
+ament_export_include_directories(${CMAKE_SOURCE_DIR}/../utils)
 ament_export_include_directories(
   include
 )
 ament_export_libraries(
   ${PROJECT_NAME}
 )
-ament_package()
+ament_package()
+
+# Link geometry library into executables that may use it
+target_link_libraries(slugbot_driver_node slugbot_geometry)
+target_link_libraries(${PROJECT_NAME} slugbot_geometry)
+target_link_libraries(obstacle_avoider slugbot_geometry)
+target_link_libraries(drivetrain_node slugbot_geometry)

slugbot_package/include/slugbot_package/Drivetrain.hpp

@@ -0,0 +1,34 @@
+#pragma once
+
+#include <unordered_map>
+#include <string>
+
+#include "rclcpp/macros.hpp"
+
+#include "std_msgs/msg/float64.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+class Drivetrain : public rclcpp::Node {
+public:
+  Drivetrain();
+  void update();
+
+private:
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr front_left_wheel_subscription;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr front_right_wheel_subscription;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr back_left_wheel_subscription;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr back_right_wheel_subscription;
+
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr left_turn_angle_subscription;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr right_turn_angle_subscription;
+
+  double front_left_wheel_speed_msg = 0.0;
+  double front_right_wheel_speed_msg = 0.0;
+  double back_left_wheel_speed_msg = 0.0;
+  double back_right_wheel_speed_msg = 0.0;
+
+  double left_turn_angle_msg = 0.0;
+  double right_turn_angle_msg = 0.0;
+  rclcpp::TimerBase::SharedPtr timer;
+};

slugbot_package/include/slugbot_package/Simulation.hpp

@@ -1,13 +1,17 @@
 #ifndef SIMULATION_HPP
 #define SIMULATION_HPP
 
+#include <unordered_map>
+#include <string>
+
 #include "rclcpp/macros.hpp"
 #include "webots_ros2_driver/PluginInterface.hpp"
 #include "webots_ros2_driver/WebotsNode.hpp"
 
-#include "std_msgs/msg/float64.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "messages/msg/wheel_states.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include <webots/robot.h>
 
 namespace slugbot_driver {
 class Simulation : public webots_ros2_driver::PluginInterface {
@@ -17,28 +21,23 @@ class Simulation : public webots_ros2_driver::PluginInterface {
             std::unordered_map<std::string, std::string> &parameters) override;
 
 private:
+  rclcpp::Subscription<messages::msg::WheelStates>::SharedPtr wheel_states_subscription;
 
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr left_wheel_subscription;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr right_wheel_subscription;
-  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr turn_angle_subscription;
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr keyboard_publisher;
-  double left_wheel_speed_msg;
-  double right_wheel_speed_msg;
-  double turn_angle_msg;
-  std::string keys_pressed;
 
-  WbDeviceTag right_motors[3];
-  WbDeviceTag *right_side;
+  messages::msg::WheelStates wheel_states_msg;
 
-  WbDeviceTag left_motors[3];
-  WbDeviceTag *left_side;
+  std::string keys_pressed;
 
+  WbDeviceTag motors[4];
   WbDeviceTag turn_motors[4];
 
-  // distance sensors for debugging
+  // distance sensors for debugging (optional)
   WbDeviceTag ds_left;
   WbDeviceTag ds_right;
+
   int time_step_ms = 0;
 };
 } // namespace slugbot_driver
+
 #endif

slugbot_package/include/slugbot_package/SlugbotDriver.hpp

@@ -1,31 +1,31 @@
 #ifndef SLUGBOT_DRIVER_HPP
 #define SLUGBOT_DRIVER_HPP
 
-#include "std_msgs/msg/float64.hpp"
+#include <string>
+#include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
-#include "geometry_msgs/msg/twist.hpp"
 #include "messages/msg/controller_input.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "sensor_msgs/msg/range.hpp"
-
-#include "rclcpp/rclcpp.hpp"
+#include "messages/msg/wheel_states.hpp"
 
 class SlugbotDriver : public rclcpp::Node {
 public:
   explicit SlugbotDriver();
   void update();
 
 private:
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr left_wheel_publisher;
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr right_wheel_publisher;
-  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr turn_angle_publisher;
+  void optimize_wheel_states(messages::msg::WheelStates& wheels);
+
+  messages::msg::WheelStates current_wheel_states;
+
+  rclcpp::Publisher<messages::msg::WheelStates>::SharedPtr wheel_states_publisher;
+
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr keyboard_subscription;
-  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_subscription_avoid;
   rclcpp::Subscription<messages::msg::ControllerInput>::SharedPtr controller_subscription;
-  geometry_msgs::msg::Twist cmd_vel_msg_avoid;
   std::string keys_pressed;
   messages::msg::ControllerInput controller_input;
+
   rclcpp::TimerBase::SharedPtr timer;
   bool recieved_input = false;
 };
+
 #endif

slugbot_package/launch/robot_launch.py

@@ -0,0 +1,29 @@
+import os
+import shutil
+import launch
+from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory, get_package_prefix
+
+def generate_launch_description():
+    obstacle_avoider = Node(
+        package='slugbot_package',
+        executable='obstacle_avoider',
+    )
+
+    slugbot_node = Node(
+        package='slugbot_package',
+        executable='slugbot_driver_node',
+    )
+
+    drivetrain = Node(
+        package='slugbot_package',
+        executable='drivetrain_node',
+    )
+
+    return LaunchDescription([
+        slugbot_node,
+        obstacle_avoider,
+        drivetrain
+    ])

slugbot_package/launch/simulation_launch.py

@@ -1,5 +1,3 @@
-# TODO: Add a separate launch file for the real robot
-
 import os
 import shutil
 import launch

slugbot_package/package.xml

@@ -16,6 +16,8 @@
   <depend>webots_ros2_driver</depend>
   <depend>pluginlib</depend>
   <depend>sensor_msgs</depend>
+  <build_depend>ament_index_cpp</build_depend>
+  <exec_depend>ament_index_cpp</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>