Real and simulator

roboteq_driver/CMakeLists.txt → chassis_driver/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(roboteq_driver)
+project(chassis_driver)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -14,12 +14,12 @@ find_package(geometry_msgs REQUIRED)
 find_package(socketcan_interface_msg REQUIRED)
 find_package(utilities REQUIRED)
 
-add_library(roboteq_driver_node src/roboteq_driver_node.cpp)
-target_compile_features(roboteq_driver_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
-target_include_directories(roboteq_driver_node PUBLIC
+add_library(chassis_driver_node src/chassis_driver_node.cpp)
+target_compile_features(chassis_driver_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(chassis_driver_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-ament_target_dependencies(roboteq_driver_node
+ament_target_dependencies(chassis_driver_node
   rclcpp
   std_msgs
   geometry_msgs
@@ -29,7 +29,7 @@ ament_target_dependencies(roboteq_driver_node
 
 # 実行可能ノード
 add_executable(debug_printer
-  src/debug_printer
+  src/debug_printer.cpp
 )
 target_include_directories(debug_printer PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -43,14 +43,14 @@ ament_target_dependencies(debug_printer
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
-target_compile_definitions(roboteq_driver_node PRIVATE "ROBOTEQ_DRIVER_BUILDING_LIBRARY")
+target_compile_definitions(chassis_driver_node PRIVATE "CHASSIS_DRIVER_BUILDING_LIBRARY")
 
 install(
   DIRECTORY include/
   DESTINATION include
 )
 install(
-  TARGETS roboteq_driver_node
+  TARGETS chassis_driver_node
   EXPORT export_${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
@@ -77,7 +77,7 @@ ament_export_include_directories(
   include
 )
 ament_export_libraries(
-  roboteq_driver_node
+  chassis_driver_node
 )
 ament_export_targets(
   export_${PROJECT_NAME}

roboteq_driver/include/roboteq_driver/roboteq_driver_node.hpp → chassis_driver/include/chassis_driver/chassis_driver_node.hpp

@@ -2,21 +2,23 @@
 
 #include <rclcpp/rclcpp.hpp>
 #include <geometry_msgs/msg/twist.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/float64.hpp>
 #include "socketcan_interface_msg/msg/socketcan_if.hpp"
+#include "utilities/velplanner.hpp"
 
-#include "roboteq_driver/visibility_control.h"
+#include "chassis_driver/visibility_control.h"
 
-namespace roboteq_driver{
+namespace chassis_driver{
 
-class RoboteqDriver : public rclcpp::Node {
+class ChassisDriver : public rclcpp::Node {
 public:
-    ROBOTEQ_DRIVER_PUBLIC
-    explicit RoboteqDriver(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+    CHASSIS_DRIVER_PUBLIC
+    explicit ChassisDriver(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
 
-    ROBOTEQ_DRIVER_PUBLIC
-    explicit RoboteqDriver(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+    CHASSIS_DRIVER_PUBLIC
+    explicit ChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
 
 private:
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _subscription_vel;
@@ -36,9 +38,17 @@ class RoboteqDriver : public rclcpp::Node {
 
     rclcpp::Publisher<socketcan_interface_msg::msg::SocketcanIF>::SharedPtr publisher_can;
     rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr publisher_steer;
+    rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr publisher_ref_vel;
 
     rclcpp::QoS _qos = rclcpp::QoS(10);
 
+
+    // 速度計画機
+    velplanner::VelPlanner linear_planner;
+    const velplanner::Limit linear_limit;
+    velplanner::VelPlanner angular_planner;
+    const velplanner::Limit angular_limit;
+
     // 定数
     const int interval_ms;
     const double wheel_radius;
@@ -48,9 +58,6 @@ class RoboteqDriver : public rclcpp::Node {
     const bool is_reverse_left;
     const bool is_reverse_right;
 
-    // 制御
-    std::shared_ptr<geometry_msgs::msg::Twist> vel;
-
     // 動作モード
     enum class Mode{
         cmd,
@@ -60,4 +67,4 @@ class RoboteqDriver : public rclcpp::Node {
 
 };
 
-}  // namespace roboteq_driver
+}  // namespace chassis_driver

chassis_driver/include/chassis_driver/visibility_control.h

@@ -0,0 +1,35 @@
+#ifndef CHASSIS_DRIVER__VISIBILITY_CONTROL_H_
+#define CHASSIS_DRIVER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define CHASSIS_DRIVER_EXPORT __attribute__ ((dllexport))
+    #define CHASSIS_DRIVER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define CHASSIS_DRIVER_EXPORT __declspec(dllexport)
+    #define CHASSIS_DRIVER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef CHASSIS_DRIVER_BUILDING_LIBRARY
+    #define CHASSIS_DRIVER_PUBLIC CHASSIS_DRIVER_EXPORT
+  #else
+    #define CHASSIS_DRIVER_PUBLIC CHASSIS_DRIVER_IMPORT
+  #endif
+  #define CHASSIS_DRIVER_PUBLIC_TYPE CHASSIS_DRIVER_PUBLIC
+  #define CHASSIS_DRIVER_LOCAL
+#else
+  #define CHASSIS_DRIVER_EXPORT __attribute__ ((visibility("default")))
+  #define CHASSIS_DRIVER_IMPORT
+  #if __GNUC__ >= 4
+    #define CHASSIS_DRIVER_PUBLIC __attribute__ ((visibility("default")))
+    #define CHASSIS_DRIVER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define CHASSIS_DRIVER_PUBLIC
+    #define CHASSIS_DRIVER_LOCAL
+  #endif
+  #define CHASSIS_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif  // CHASSIS_DRIVER__VISIBILITY_CONTROL_H_

roboteq_driver/package.xml → chassis_driver/package.xml

@@ -1,7 +1,7 @@
 <?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>roboteq_driver</name>
+  <name>chassis_driver</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="[email protected]">kazuma</maintainer>

roboteq_driver/src/roboteq_driver_node.cpp → chassis_driver/src/chassis_driver_node.cpp

@@ -1,4 +1,4 @@
-#include "roboteq_driver/roboteq_driver_node.hpp"
+#include "chassis_driver/chassis_driver_node.hpp"
 
 #include "utilities/data_utils.hpp"
 #include "utilities/utils.hpp"
@@ -7,93 +7,125 @@
 
 using namespace utils;
 
-namespace roboteq_driver{
+namespace chassis_driver{
 
-RoboteqDriver::RoboteqDriver(const rclcpp::NodeOptions& options) : RoboteqDriver("", options) {}
+ChassisDriver::ChassisDriver(const rclcpp::NodeOptions& options) : ChassisDriver("", options) {}
 
-RoboteqDriver::RoboteqDriver(const std::string& name_space, const rclcpp::NodeOptions& options)
-: rclcpp::Node("roboteq_driver_node", name_space, options),
+ChassisDriver::ChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options)
+: rclcpp::Node("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()),
 rotate_ratio(1.0 / get_parameter("reduction_ratio").as_double()),
 is_reverse_left(get_parameter("reverse_left_flag").as_bool()),
-is_reverse_right(get_parameter("reverse_right_flag").as_bool())
+is_reverse_right(get_parameter("reverse_right_flag").as_bool()),
+
+linear_limit(DBL_MAX,
+get_parameter("linear_max.vel").as_double(),
+get_parameter("linear_max.acc").as_double()),
+
+angular_limit(DBL_MAX,
+get_parameter("angular_max.vel").as_double(),
+get_parameter("angular_max.acc").as_double())
 {
     _subscription_vel = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel",
         _qos,
-        std::bind(&RoboteqDriver::_subscriber_callback_vel, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_vel, this, std::placeholders::_1)
     );
     _subscription_stop = this->create_subscription<std_msgs::msg::Empty>(
         "stop",
         _qos,
-        std::bind(&RoboteqDriver::_subscriber_callback_stop, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_stop, this, std::placeholders::_1)
     );
     _subscription_restart = this->create_subscription<std_msgs::msg::Empty>(
         "restart",
         _qos,
-        std::bind(&RoboteqDriver::_subscriber_callback_restart, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1)
     );
     _subscription_rpm = this->create_subscription<socketcan_interface_msg::msg::SocketcanIF>(
         "can_rx_711",
         _qos,
-        std::bind(&RoboteqDriver::_subscriber_callback_rpm, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_rpm, this, std::placeholders::_1)
     );
     _subscription_emergency = this->create_subscription<socketcan_interface_msg::msg::SocketcanIF>(
         "can_rx_712",
         _qos,
-        std::bind(&RoboteqDriver::_subscriber_callback_emergency, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_emergency, this, std::placeholders::_1)
     );
     publisher_can = this->create_publisher<socketcan_interface_msg::msg::SocketcanIF>("can_tx", _qos);
     publisher_steer = this->create_publisher<std_msgs::msg::Float64>("cybergear/pos", _qos);
+    publisher_ref_vel = this->create_publisher<geometry_msgs::msg::TwistStamped>("ref_vel", _qos);
 
     _pub_timer = this->create_wall_timer(
         std::chrono::milliseconds(interval_ms),
         [this] { _publisher_callback(); }
     );
+
+    linear_planner.limit(linear_limit);
+    angular_planner.limit(angular_limit);
 }
 
-void RoboteqDriver::_subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg){
     if(mode == Mode::stop) return;
     mode = Mode::cmd;
 
-    vel = msg;
+    const double linear_vel = constrain(msg->linear.x, -linear_limit.vel, linear_limit.vel);
+    const double angular_vel = constrain(msg->angular.z, -angular_limit.vel, angular_limit.vel);
+    linear_planner.vel(linear_vel);
+    angular_planner.vel(angular_vel);
 }
 
-void RoboteqDriver::_publisher_callback(){
+void ChassisDriver::_publisher_callback(){
+    linear_planner.cycle();
+    angular_planner.cycle();
+
     if(mode == Mode::stop || mode == Mode::stay){
         this->send_rpm(0.0, 0.0);
         return;
     }
-    if(vel == nullptr) return;
-    send_rpm(vel->linear.x, vel->angular.z);
+
+    const double linear_vel = linear_planner.vel();
+    const double angular_vel = angular_planner.vel();
+    send_rpm(linear_vel, angular_vel);
+
+    // デバッグ用にロボットの目標速度指令値を出版
+    auto msg_ref_vel = std::make_shared<geometry_msgs::msg::TwistStamped>();
+    msg_ref_vel->header.stamp = this->now();
+    msg_ref_vel->header.frame_id = "map";
+    msg_ref_vel->twist.linear.x = linear_vel;
+    msg_ref_vel->twist.angular.z = angular_vel;
+    publisher_ref_vel->publish(*msg_ref_vel);
 
     // 従動輪角度の送信
     auto msg_steer = std::make_shared<std_msgs::msg::Float64>();
-    if(vel->linear.x == 0.0){
+    if(linear_vel == 0.0){
         msg_steer->data = 0.0;
     }
     else{
-        msg_steer->data = std::asin((wheelbase*vel->angular.z) / vel->linear.x);
+        msg_steer->data = std::asin((wheelbase*angular_vel) / linear_vel);
         if(std::isnan(msg_steer->data)) msg_steer->data = 0.0;
     }
     publisher_steer->publish(*msg_steer);
 }
 
-void RoboteqDriver::_subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg){
     mode = Mode::stop;
     RCLCPP_INFO(this->get_logger(), "停止");
 }
-void RoboteqDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg){
     mode = Mode::stay;
+
+    velplanner::Physics_t physics_zero(0.0, 0.0, 0.0);
+    linear_planner.current(physics_zero);
+    angular_planner.current(physics_zero);
     RCLCPP_INFO(this->get_logger(), "再稼働");
 }
 
-void RoboteqDriver::_subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
 }
-void RoboteqDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
     uint8_t _candata[8];
     for(int i=0; i<msg->candlc; i++) _candata[i] = msg->candata[i];
 
@@ -103,10 +135,12 @@ void RoboteqDriver::_subscriber_callback_emergency(const socketcan_interface_msg
     }
 }
 
-void RoboteqDriver::send_rpm(const double linear_vel, const double angular_vel){
+void ChassisDriver::send_rpm(const double linear_vel, const double angular_vel){
     // 駆動輪の目標角速度
-    const double left_vel = (1.0 / wheel_radius) * linear_vel + (tread / wheel_radius*2.0) * angular_vel;
-    const double right_vel = (1.0 / wheel_radius) * linear_vel - (tread / wheel_radius*2.0) * angular_vel;
+    // const double left_vel = (1.0 / wheel_radius) * linear_vel + (tread / wheel_radius*2.0) * angular_vel;
+    // const double right_vel = (1.0 / wheel_radius) * linear_vel - (tread / wheel_radius*2.0) * angular_vel;
+    const double left_vel = (-tread*angular_vel + 2.0*linear_vel) / (2.0*wheel_radius);
+    const double right_vel = (tread*angular_vel + 2.0*linear_vel) / (2.0*wheel_radius);
 
     // rad/s -> rpm  &  回転方向制御
     const double left_rpm = (is_reverse_left ? -1 : 1) * (left_vel*30.0 / d_pi) * rotate_ratio;
@@ -127,4 +161,4 @@ void RoboteqDriver::send_rpm(const double linear_vel, const double angular_vel){
 }
 
 
-}  // namespace roboteq_driver
+}  // namespace chassis_driver