Feat/vonav

.gitignore

@@ -2,6 +2,7 @@
 *.log
 *.urdf
 *.pyc
+*.pt
 
 #ディレクトリで指定
 bin/

.gitmodules

@@ -4,3 +4,9 @@
 [submodule "vectornav"]
 	path = vectornav
 	url = https://github.com/open-rdc/vectornav
+[submodule "socketcan_interface"]
+	path = socketcan_interface
+	url = https://github.com/hcmos/ros2-socketcan_interface
+[submodule "utilities"]
+	path = utilities
+	url = https://github.com/hcmos/ros2-utilities

chassis_driver/CMakeLists.txt

@@ -6,61 +6,23 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
 endif()
 
 # find dependencies
-find_package(ament_cmake REQUIRED)
-find_package(ament_cmake_ros REQUIRED)
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(geometry_msgs REQUIRED)
-find_package(socketcan_interface_msg REQUIRED)
-find_package(utilities REQUIRED)
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
 
-add_library(chassis_driver_node src/chassis_driver_node.cpp)
+ament_auto_add_library(chassis_driver_node
+  src/chassis_driver_node.cpp
+  src/velplanner.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(chassis_driver_node
-  rclcpp
-  std_msgs
-  geometry_msgs
-  socketcan_interface_msg
-  utilities
-)
 
 # 実行可能ノード
-add_executable(debug_printer
+ament_auto_add_executable(debug_printer
   src/debug_printer.cpp
 )
-target_include_directories(debug_printer PUBLIC
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include>
-)
-ament_target_dependencies(debug_printer
-  rclcpp
-  socketcan_interface_msg
-  utilities
-)
 
 # Causes the visibility macros to use dllexport rather than dllimport,
 # which is appropriate when building the dll but not consuming it.
 target_compile_definitions(chassis_driver_node PRIVATE "CHASSIS_DRIVER_BUILDING_LIBRARY")
 
-install(
-  DIRECTORY include/
-  DESTINATION include
-)
-install(
-  TARGETS chassis_driver_node
-  EXPORT export_${PROJECT_NAME}
-  ARCHIVE DESTINATION lib
-  LIBRARY DESTINATION lib
-  RUNTIME DESTINATION bin
-)
-install(
-  TARGETS debug_printer
-  DESTINATION lib/${PROJECT_NAME}
-)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -73,22 +35,4 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_export_include_directories(
-  include
-)
-ament_export_libraries(
-  chassis_driver_node
-)
-ament_export_targets(
-  export_${PROJECT_NAME}
-)
-
-ament_export_dependencies(
-  rclcpp
-  std_msgs
-  geometry_msgs
-  socketcan_interface_msg
-  utilities
-)
-
-ament_package()
+ament_auto_package()

chassis_driver/include/chassis_driver/chassis_driver_node.hpp

@@ -6,7 +6,10 @@
 #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 "base/velplanner.hpp"
+#include "utilities/position_pid.hpp"
+#include "odrive_can/msg/control_message.hpp"
+#include "odrive_can/srv/axis_state.hpp"
 
 #include "chassis_driver/visibility_control.h"
 
@@ -24,21 +27,23 @@ class ChassisDriver : public rclcpp::Node {
     rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr _subscription_vel;
     rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr _subscription_stop;
     rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr _subscription_restart;
-    rclcpp::Subscription<socketcan_interface_msg::msg::SocketcanIF>::SharedPtr _subscription_rpm;
+    rclcpp::Subscription<socketcan_interface_msg::msg::SocketcanIF>::SharedPtr _subscription_caster;
     rclcpp::Subscription<socketcan_interface_msg::msg::SocketcanIF>::SharedPtr _subscription_emergency;
     rclcpp::TimerBase::SharedPtr _pub_timer;
 
     void _subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg);
     void _subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg);
     void _subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg);
-    void _subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg);
+    void _subscriber_callback_caster(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg);
     void _subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg);
     void _publisher_callback();
     void send_rpm(const double linear_vel, const double angular_vel);
 
     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::Publisher<odrive_can::msg::ControlMessage>::SharedPtr publisher_odrive;
+
+    rclcpp::Client<odrive_can::srv::AxisState>::SharedPtr odrive_axis_client_;
 
     rclcpp::QoS _qos = rclcpp::QoS(10);
 
@@ -49,6 +54,9 @@ class ChassisDriver : public rclcpp::Node {
     velplanner::VelPlanner angular_planner;
     const velplanner::Limit angular_limit;
 
+    // 従動輪のPID
+    controller::PositionPid caster_pid;
+
     // 定数
     const int interval_ms;
     const double wheel_radius;
@@ -57,6 +65,12 @@ class ChassisDriver : public rclcpp::Node {
     const double rotate_ratio;
     const bool is_reverse_left;
     const bool is_reverse_right;
+    const int caster_max_count;
+    const double caster_max_angle;
+    const double motor_max_torque;
+
+    // 変数
+    double caster_orientation = 0.0;
 
     // 動作モード
     enum class Mode{

chassis_driver/package.xml

@@ -4,7 +4,7 @@
   <name>chassis_driver</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
-  <maintainer email="c1000702@planet.kanazawa-it.ac.jp">kazuma</maintainer>
+  <maintainer email="hogehoge@gmail.com">cmos</maintainer>
   <license>TODO: License declaration</license>
 
   <buildtool_depend>ament_cmake_ros</buildtool_depend>
@@ -14,6 +14,7 @@
   <depend>geometry_msgs</depend>
   <depend>socketcan_interface_msg</depend>
   <depend>utilities</depend>
+  <depend>odrive_can</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

chassis_driver/src/chassis_driver_node.cpp

@@ -2,6 +2,7 @@
 
 #include "utilities/data_utils.hpp"
 #include "utilities/utils.hpp"
+
 #include <float.h>
 #include <cmath>
 
@@ -20,14 +21,18 @@ 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()),
+caster_max_count(get_parameter("caster_max_count").as_int()),
+caster_max_angle(dtor(get_parameter("caster_max_angle").as_double())),
+motor_max_torque(get_parameter("motor_max_torque").as_double()),
+caster_pid(get_parameter("interval_ms").as_int()),
 
 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())
+dtor(get_parameter("angular_max.vel").as_double()),
+dtor(get_parameter("angular_max.acc").as_double()))
 {
     _subscription_vel = this->create_subscription<geometry_msgs::msg::Twist>(
         "cmd_vel",
@@ -44,19 +49,22 @@ get_parameter("angular_max.acc").as_double())
         _qos,
         std::bind(&ChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1)
     );
-    _subscription_rpm = this->create_subscription<socketcan_interface_msg::msg::SocketcanIF>(
-        "can_rx_711",
+    _subscription_caster = this->create_subscription<socketcan_interface_msg::msg::SocketcanIF>(
+        "can_rx_012",
         _qos,
-        std::bind(&ChassisDriver::_subscriber_callback_rpm, this, std::placeholders::_1)
+        std::bind(&ChassisDriver::_subscriber_callback_caster, this, std::placeholders::_1)
     );
     _subscription_emergency = this->create_subscription<socketcan_interface_msg::msg::SocketcanIF>(
         "can_rx_712",
         _qos,
         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);
+    publisher_odrive = this->create_publisher<odrive_can::msg::ControlMessage>("/odrive_axis0/control_message", _qos);
+
+    // ODriveのAxis Stateサービスクライアント作成
+    odrive_axis_client_ = this->create_client<odrive_can::srv::AxisState>("/odrive_axis0/request_axis_state");
 
     _pub_timer = this->create_wall_timer(
         std::chrono::milliseconds(interval_ms),
@@ -65,6 +73,15 @@ get_parameter("angular_max.acc").as_double())
 
     linear_planner.limit(linear_limit);
     angular_planner.limit(angular_limit);
+    caster_pid.gain(get_parameter("p_gain").as_double(), get_parameter("i_gain").as_double(), get_parameter("d_gain").as_double());
+
+    // ODriveのAxis Stateをクローズドループに設定（axis_requested_state: 8）
+    auto request = std::make_shared<odrive_can::srv::AxisState::Request>();
+    request->axis_requested_state = 8;
+    auto future = odrive_axis_client_->async_send_request(request);
+
+    RCLCPP_INFO(this->get_logger(), "Chassis Driver Node has been started. max vel: %.2f m/s, max angular vel: %.2f deg/s",
+        linear_limit.vel, rtod(angular_limit.vel));
 }
 
 void ChassisDriver::_subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg){
@@ -86,28 +103,49 @@ void ChassisDriver::_publisher_callback(){
         return;
     }
 
+    // 速度計画機の参照
     const double linear_vel = linear_planner.vel();
     const double angular_vel = angular_planner.vel();
+
+    // 従動輪目標角度の生成
+    double delta = 0.0;
+    if(linear_vel == 0.0){
+        delta = 0.0;
+    }
+    else{
+        delta = std::asin((wheelbase*angular_vel) / linear_vel);
+        if(std::isnan(delta)) delta = 0.0;
+        delta = constrain(delta, -caster_max_angle, caster_max_angle);
+    }
+
+    // const double motor_torque = -1.0* constrain(caster_pid.cycle(caster_orientation, delta), -motor_max_torque, motor_max_torque);
+    double motor_pos = 0.0;
+    if(delta > dtor(1.0)){
+        motor_pos = -1.0 * (delta + dtor(5.0));
+    }
+    else if(delta < dtor(-1.0)){
+        motor_pos = -1.0 * (delta - dtor(5.0));
+    }
+    // RCLCPP_INFO(this->get_logger(), "DEL:%.2f POS:%.2f ENC:%.2f", rtod(delta), rtod(motor_pos), rtod(caster_orientation));
+
     send_rpm(linear_vel, angular_vel);
 
+    // ODriveにトルク指令を送信
+    auto msg_odrive_control = std::make_shared<odrive_can::msg::ControlMessage>();
+    msg_odrive_control->control_mode = 3;
+    msg_odrive_control->input_mode = 1;
+    msg_odrive_control->input_pos = motor_pos;
+    msg_odrive_control->input_vel = 0.0;
+    msg_odrive_control->input_torque = 0.0;
+    publisher_odrive->publish(*msg_odrive_control);
+
     // デバッグ用にロボットの目標速度指令値を出版
     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(linear_vel == 0.0){
-        msg_steer->data = 0.0;
-    }
-    else{
-        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 ChassisDriver::_subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg){
@@ -120,10 +158,17 @@ void ChassisDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::Sha
     velplanner::Physics_t physics_zero(0.0, 0.0, 0.0);
     linear_planner.current(physics_zero);
     angular_planner.current(physics_zero);
+    caster_pid.reset();
     RCLCPP_INFO(this->get_logger(), "再稼働");
 }
 
-void ChassisDriver::_subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
+void ChassisDriver::_subscriber_callback_caster(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];
+
+    const int count = static_cast<int>(bytes_to_int16(_candata));
+    caster_orientation = count / static_cast<double>(caster_max_count) * 2.0 * d_pi;
+    // RCLCPP_INFO(this->get_logger(), "CAS:%f CNT:%d", rtod(caster_orientation), count);
 }
 void ChassisDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){
     uint8_t _candata[8];
@@ -136,9 +181,8 @@ void ChassisDriver::_subscriber_callback_emergency(const socketcan_interface_msg
 }
 
 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 = (-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);
 
@@ -153,8 +197,8 @@ void ChassisDriver::send_rpm(const double linear_vel, const double angular_vel){
     msg_can->candlc = 8;
 
     uint8_t _candata[8];
-    int_to_bytes(_candata, static_cast<int>(right_rpm));
-    int_to_bytes(_candata+4, static_cast<int>(left_rpm));
+    int32_to_bytes(_candata, static_cast<int32_t>(right_rpm));
+    int32_to_bytes(_candata+4, static_cast<int32_t>(left_rpm));
 
     for(int i=0; i<msg_can->candlc; i++) msg_can->candata[i]=_candata[i];
     publisher_can->publish(*msg_can);

chassis_driver/src/debug_printer.cpp

@@ -40,10 +40,10 @@ void DebugPrinter::_subscriber_callback_rpm_rx(const socketcan_interface_msg::ms
 
     auto msg_tx = std::make_shared<std_msgs::msg::Int64>();
 
-    const int left = msg_tx->data = static_cast<int64_t>(bytes_to_int(_candata));
+    const int left = msg_tx->data = static_cast<int>(bytes_to_int32(_candata));
     publisher_left_rpm_rx->publish(*msg_tx);
 
-    const int right = msg_tx->data = static_cast<int64_t>(bytes_to_int(_candata+4));
+    const int right = msg_tx->data = static_cast<int>(bytes_to_int32(_candata+4));
     publisher_right_rpm_rx->publish(*msg_tx);
 
     RCLCPP_DEBUG(this->get_logger(), "RPM RX   L:%d  R:%d", left, right);
@@ -55,10 +55,10 @@ void DebugPrinter::_subscriber_callback_can_tx(const socketcan_interface_msg::ms
 
         auto msg_tx = std::make_shared<std_msgs::msg::Int64>();
 
-        const int left = msg_tx->data = static_cast<int64_t>(bytes_to_int(_candata));
+        const int left = msg_tx->data = static_cast<int>(bytes_to_int32(_candata));
         publisher_left_rpm_tx->publish(*msg_tx);
 
-        const int right = msg_tx->data = static_cast<int64_t>(bytes_to_int(_candata+4));
+        const int right = msg_tx->data = static_cast<int>(bytes_to_int32(_candata+4));
         publisher_right_rpm_tx->publish(*msg_tx);
 
         RCLCPP_DEBUG(this->get_logger(), "RPM TX   L:%d  R:%d", left, right);
@@ -70,7 +70,7 @@ void DebugPrinter::_subscriber_callback_potentio(const socketcan_interface_msg::
     for(int i=0; i<msg->candlc; i++) _candata[i] = msg->candata[i];
 
     auto msg_tx = std::make_shared<std_msgs::msg::Int64>();
-    const int value = msg_tx->data = static_cast<int64_t>(bytes_to_short(_candata));
+    const int value = msg_tx->data = static_cast<int>(bytes_to_int16(_candata));
     publisher_potentio->publish(*msg_tx);
     RCLCPP_DEBUG(this->get_logger(), "POTENTIO:%d", value);
 }