従動輪のプリロード可変機構の再現

sim_chassis_driver/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(sim_chassis_driver)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(sim_chassis_driver_node SHARED
+  src/sim_chassis_driver.cpp)
+target_compile_features(sim_chassis_driver_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+
+ament_target_dependencies(sim_chassis_driver_node
+  rclcpp
+  std_msgs
+  geometry_msgs
+  steered_drive_msg
+  utilities
+  chassis_driver
+)
+
+target_compile_definitions(sim_chassis_driver_node PRIVATE "SIM_CHASSIS_DRIVER_BUILDING_LIBRARY")
+
+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()
+
+set(USE_SCOPED_HEADER_INSTALL_DIR TRUE)
+ament_auto_package()

sim_chassis_driver/include/sim_chassis_driver/sim_chassis_driver.hpp

@@ -0,0 +1,71 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/float64.hpp>
+#include <std_msgs/msg/float64_multi_array.hpp>
+#include "steered_drive_msg/msg/steered_drive.hpp"
+#include "base/velplanner.hpp"
+#include "utilities/position_pid.hpp"
+#include "sim_chassis_driver/visibility_control.h"
+
+namespace sim_chassis_driver {
+
+class SimChassisDriver : public rclcpp::Node {
+public:
+  SIM_CHASSIS_DRIVER_PUBLIC
+  explicit SimChassisDriver(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+  SIM_CHASSIS_DRIVER_PUBLIC
+  explicit SimChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+
+private:
+  rclcpp::Subscription<steered_drive_msg::msg::SteeredDrive>::SharedPtr subscription_vel_;
+  rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr subscription_restart_;
+  rclcpp::Subscription<std_msgs::msg::Float64>::SharedPtr subscription_caster_orientation_;
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr subscription_bodyvel_;
+  rclcpp::TimerBase::SharedPtr pub_timer_;
+
+  void _subscriber_callback_vel(const steered_drive_msg::msg::SteeredDrive::SharedPtr msg);
+  void _subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg);
+  void _subscriber_callback_caster_orientation(const std_msgs::msg::Float64::SharedPtr msg);
+  void _subscriber_callback_bodyvel(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg);
+  void _publisher_callback();
+  void send_sim_command(const double linear_vel, const double angular_vel);
+  static double normalize_angle(double angle);
+
+  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr publisher_diff_drive_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_reel_position_;
+  rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr publisher_caster_data_;
+
+  rclcpp::QoS qos_ = rclcpp::QoS(10);
+
+  const int interval_ms;
+  const double wheel_radius;
+  const double tread;
+  const double wheelbase;
+  const double caster_wheel_radius;
+  const double reel_radius;
+  const double steering_radius;
+  const double preload_length;
+  const double preload_gain;
+
+  velplanner::VelPlanner linear_planner;
+  const velplanner::Limit linear_limit;
+  const velplanner::Limit steering_limit;
+  controller::PositionPid drive_pid;
+
+  double cmd_steering = 0.0;
+  double caster_orientation = 0.0;
+  geometry_msgs::msg::Twist current_body_vel;
+
+  enum class Mode {
+    cmd,
+    stay,
+    stop
+  } mode = Mode::stop;
+};
+
+}  // namespace sim_chassis_driver

sim_chassis_driver/include/sim_chassis_driver/visibility_control.h

@@ -0,0 +1,32 @@
+#ifndef SIM_CHASSIS_DRIVER__VISIBILITY_CONTROL_H_
+#define SIM_CHASSIS_DRIVER__VISIBILITY_CONTROL_H_
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define SIM_CHASSIS_DRIVER_EXPORT __attribute__ ((dllexport))
+    #define SIM_CHASSIS_DRIVER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define SIM_CHASSIS_DRIVER_EXPORT __declspec(dllexport)
+    #define SIM_CHASSIS_DRIVER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef SIM_CHASSIS_DRIVER_BUILDING_LIBRARY
+    #define SIM_CHASSIS_DRIVER_PUBLIC SIM_CHASSIS_DRIVER_EXPORT
+  #else
+    #define SIM_CHASSIS_DRIVER_PUBLIC SIM_CHASSIS_DRIVER_IMPORT
+  #endif
+  #define SIM_CHASSIS_DRIVER_PUBLIC_TYPE SIM_CHASSIS_DRIVER_PUBLIC
+  #define SIM_CHASSIS_DRIVER_LOCAL
+#else
+  #define SIM_CHASSIS_DRIVER_EXPORT __attribute__ ((visibility("default")))
+  #define SIM_CHASSIS_DRIVER_IMPORT
+  #if __GNUC__ >= 4
+    #define SIM_CHASSIS_DRIVER_PUBLIC __attribute__ ((visibility("default")))
+    #define SIM_CHASSIS_DRIVER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define SIM_CHASSIS_DRIVER_PUBLIC
+    #define SIM_CHASSIS_DRIVER_LOCAL
+  #endif
+  #define SIM_CHASSIS_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif  // SIM_CHASSIS_DRIVER__VISIBILITY_CONTROL_H_

sim_chassis_driver/package.xml

@@ -0,0 +1,28 @@
+<?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>sim_chassis_driver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="a.w.g.d0201@icloud.com">ken222d</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>socketcan_interface_msg</depend>
+  <depend>steered_drive_msg</depend>
+  <depend>utilities</depend>
+  <depend>chassis_driver</depend>
+  <!-- <depend>odrive_can</depend> -->
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

sim_chassis_driver/src/sim_chassis_driver.cpp

@@ -0,0 +1,159 @@
+#include "sim_chassis_driver/sim_chassis_driver.hpp"
+
+#include "utilities/data_utils.hpp"
+#include "utilities/utils.hpp"
+
+#include <cmath>
+#include <float.h>
+
+using namespace utils;
+
+namespace sim_chassis_driver {
+
+SimChassisDriver::SimChassisDriver(const rclcpp::NodeOptions& options)
+: SimChassisDriver("", options) {}
+
+SimChassisDriver::SimChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options)
+: rclcpp::Node("sim_chassis_driver_node", name_space, options),
+  interval_ms(get_parameter("interval_ms").as_int()),
+  wheel_radius(get_parameter("wheel_radius").as_double()),
+  tread(get_parameter("tread").as_double()),
+  wheelbase(get_parameter("wheelbase").as_double()),
+  caster_wheel_radius(this->get_parameter("caster.wheel_radius").as_double()),
+  reel_radius(this->get_parameter("caster.reel_radius").as_double()),
+  steering_radius(this->get_parameter("caster.steering_radius").as_double()),
+  preload_length(this->get_parameter("caster.preload_length").as_double()),
+  preload_gain(this->get_parameter("caster.preload_gain").as_double()),
+  linear_limit(DBL_MAX,
+               get_parameter("linear_max.vel").as_double(),
+               get_parameter("linear_max.acc").as_double()),
+  steering_limit(dtor(get_parameter("steering_max.pos").as_double()),
+                 DBL_MAX, DBL_MAX),
+  drive_pid(get_parameter("interval_ms").as_int())
+{
+  subscription_vel_ = this->create_subscription<steered_drive_msg::msg::SteeredDrive>(
+      "cmd_vel",
+      qos_,
+      std::bind(&SimChassisDriver::_subscriber_callback_vel, this, std::placeholders::_1));
+
+  subscription_restart_ = this->create_subscription<std_msgs::msg::Empty>(
+      "restart",
+      qos_,
+      std::bind(&SimChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1));
+
+  subscription_caster_orientation_ = this->create_subscription<std_msgs::msg::Float64>(
+      "caster_orientation",
+      qos_,
+      std::bind(&SimChassisDriver::_subscriber_callback_caster_orientation, this, std::placeholders::_1));
+
+  subscription_bodyvel_ = this->create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+      "vectornav/velocity_body",
+      qos_,
+      std::bind(&SimChassisDriver::_subscriber_callback_bodyvel, this, std::placeholders::_1));
+
+  publisher_diff_drive_ = this->create_publisher<geometry_msgs::msg::Twist>("cmd_vel_twists", qos_);
+  publisher_reel_position_ = this->create_publisher<std_msgs::msg::Float64>("caster_reel_position_cmd", qos_);
+  publisher_caster_data_ = this->create_publisher<std_msgs::msg::Float64MultiArray>("caster_data", qos_);
+
+  pub_timer_ = this->create_wall_timer(
+      std::chrono::milliseconds(interval_ms),
+      [this]() { _publisher_callback(); });
+
+  linear_planner.limit(linear_limit);
+  drive_pid.gain(
+      get_parameter("drive_pid.p_gain").as_double(),
+      get_parameter("drive_pid.i_gain").as_double(),
+      get_parameter("drive_pid.d_gain").as_double());
+
+  RCLCPP_INFO(this->get_logger(),
+              "Sim Chassis Driver Node Started. max vel: %.2f m/s, steering angle: %.1f deg",
+              linear_limit.vel, rtod(steering_limit.pos));
+}
+
+void SimChassisDriver::_subscriber_callback_vel(const steered_drive_msg::msg::SteeredDrive::SharedPtr msg)
+{
+
+  if (mode == Mode::stop) {
+    return;
+  }
+  mode = Mode::cmd;
+  std::cout << "SimChassisDriver: Received velocity command." << std::endl;
+
+  const double linear_vel = constrain(msg->velocity, -linear_limit.vel, linear_limit.vel);
+  cmd_steering = constrain(msg->steering_angle, -steering_limit.pos, steering_limit.pos);
+  linear_planner.vel(linear_vel);
+}
+
+void SimChassisDriver::_publisher_callback()
+{
+  linear_planner.cycle();
+  const double linear_vel = linear_planner.vel();
+
+  if (mode == Mode::stop || mode == Mode::stay) {
+    std::cout << "SimChassisDriver: stop or stay mode, sending zero command." << std::endl;
+    send_sim_command(0.0, 0.0);
+    return;
+  }
+
+  double delta = 0.0;
+  bool driving_flag = false;
+  if (std::abs(linear_vel) > 0.1) {
+    delta = cmd_steering;
+    driving_flag = true;
+  }
+  const double body_vel_squared = current_body_vel.linear.x * current_body_vel.linear.x;
+
+  double motor_pos = 0.0;
+  bool straight_flag = false;
+  double winding_length = std::abs(steering_radius * std::sin(caster_orientation)) * preload_gain * body_vel_squared;
+  if (std::abs(delta) < dtor(1.0) && driving_flag) {
+    straight_flag = true;
+    winding_length = preload_length;
+  }
+  winding_length = constrain(winding_length, 0.0, preload_length);
+  motor_pos = winding_length / reel_radius;
+
+  std_msgs::msg::Float64 reel_pos_msg;
+  reel_pos_msg.data = motor_pos;
+  publisher_reel_position_->publish(reel_pos_msg);
+
+  std_msgs::msg::Float64MultiArray caster_data_msg;
+  caster_data_msg.data = {delta, caster_orientation, winding_length};
+  publisher_caster_data_->publish(caster_data_msg);
+
+  const double angular_command = (straight_flag ? 0.0 : 1.0) * drive_pid.cycle(caster_orientation, delta) * body_vel_squared;
+  send_sim_command(linear_vel, angular_command);
+}
+
+void SimChassisDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr)
+{
+  mode = Mode::stay;
+  velplanner::Physics_t physics_zero(0.0, 0.0, 0.0);
+  linear_planner.current(physics_zero);
+  RCLCPP_INFO(this->get_logger(), "Sim Chassis Driver restarted.");
+}
+
+void SimChassisDriver::_subscriber_callback_caster_orientation(const std_msgs::msg::Float64::SharedPtr msg)
+{
+  caster_orientation = msg->data;
+}
+
+void SimChassisDriver::_subscriber_callback_bodyvel(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg)
+{
+  current_body_vel = msg->twist.twist;
+}
+
+void SimChassisDriver::send_sim_command(const double linear_vel, const double angular_vel)
+{
+  geometry_msgs::msg::Twist twist_msg;
+  twist_msg.linear.x = linear_vel;
+  twist_msg.angular.z = angular_vel;
+  publisher_diff_drive_->publish(twist_msg);
+}
+
+double SimChassisDriver::normalize_angle(double angle)
+{
+  return std::atan2(std::sin(angle), std::cos(angle));
+}
+
+}  // namespace sim_chassis_driver

sim_executor/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.8)
+project(sim_executor)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic -fPIC)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(sim_exec
+        src/sim.cpp
+        )
+target_compile_features(sim_exec PUBLIC c_std_99 cxx_std_17)
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE launch config)