diff --git a/chassis_driver/chassis_driver/CMakeLists.txt b/chassis_driver/chassis_driver/CMakeLists.txt deleted file mode 100644 index ce119b93..00000000 --- a/chassis_driver/chassis_driver/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(chassis_driver) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -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 - -# 実行可能ノード -ament_auto_add_executable(debug_printer - src/debug_printer.cpp -) - -# 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") - -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() - -ament_auto_package() diff --git a/chassis_driver/chassis_driver/include/base/velplanner.hpp b/chassis_driver/chassis_driver/include/base/velplanner.hpp deleted file mode 100644 index 44b293e9..00000000 --- a/chassis_driver/chassis_driver/include/base/velplanner.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#pragma once - -#include -#include - -namespace velplanner{ - -struct Physics_t{ - Physics_t(){} - Physics_t(double pos, double vel, double acc):pos(pos), vel(vel), acc(acc){} - double pos = 0.0; - double vel = 0.0; - double acc = 0.0; -}; - -class VelPlanner{ -public: - VelPlanner(const Physics_t limit = Physics_t(0.0, 0.0, 0.0)): limit_(limit){} - void cycle(); - void current(const Physics_t physics); - void limit(const Physics_t limit){ limit_ = limit; } - - void vel(double vel); - void vel(double vel, double start_time); - void vel(double vel, int64_t start_time_us); - - const double pos(){ return current_.pos; } - const double vel(){ return current_.vel; } - const double acc(){ return current_.acc; } - - const Physics_t current(){ return current_; } - const bool hasAchievedTarget(){ return achieved_target; } - -private: - Physics_t limit_, first, target, current_; - - int64_t start_time = 0; - int64_t old_time = 0; - - double t1 = 0.0; - double using_acc = 0.0; - - enum class Mode{ - vel, - uniform_acceleration - } mode = Mode::uniform_acceleration; - - bool achieved_target = false; -}; - -//alias -using Limit = Physics_t; - -} diff --git a/chassis_driver/chassis_driver/include/chassis_driver/chassis_driver_node.hpp b/chassis_driver/chassis_driver/include/chassis_driver/chassis_driver_node.hpp deleted file mode 100644 index 9aa71039..00000000 --- a/chassis_driver/chassis_driver/include/chassis_driver/chassis_driver_node.hpp +++ /dev/null @@ -1,105 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include "socketcan_interface_msg/msg/socketcan_if.hpp" -#include "steered_drive_msg/msg/steered_drive.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" - -namespace chassis_driver{ - -class ChassisDriver : public rclcpp::Node { -public: - CHASSIS_DRIVER_PUBLIC - explicit ChassisDriver(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::SharedPtr _subscription_vel; - rclcpp::Subscription::SharedPtr _subscription_restart; - rclcpp::Subscription::SharedPtr _subscription_caster_orientation; - rclcpp::Subscription::SharedPtr _subscription_caster_rotation; - rclcpp::Subscription::SharedPtr _subscription_emergency; - rclcpp::Subscription::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 socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - void _subscriber_callback_caster_rotation(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - void _subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - void _subscriber_callback_bodyvel(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); - void _publisher_callback(); - void send_rpm(const double linear_vel, const double angular_vel); - static double normalize_angle(double angle); - - rclcpp::Publisher::SharedPtr publisher_can; - rclcpp::Publisher::SharedPtr publisher_odrive; - rclcpp::Publisher::SharedPtr publisher_caster_data; - rclcpp::Publisher::SharedPtr publisher_odom; - - rclcpp::Client::SharedPtr odrive_axis_client_; - - rclcpp::QoS _qos = rclcpp::QoS(10); - - - // 速度計画機 - velplanner::VelPlanner linear_planner; - const velplanner::Limit linear_limit; - const velplanner::Limit steering_limit; - - // トルク差PID - controller::PositionPid drive_pid; - - // 定数 - const int interval_ms; - const double wheel_radius; - const double tread; - const double wheelbase; - const double rotate_ratio; - const bool is_reverse_left; - const bool is_reverse_right; - const int caster_max_count; - const double caster_gear_ratio; - const double caster_wheel_radius; - const double reel_radius; - const double steering_radius; - const double preload_length; - const double preload_gain; - - // 変数 - double cmd_steering = 0.0; - double caster_orientation = 0.0; - geometry_msgs::msg::Twist current_body_vel; - // キャスター回転角関連変数 - double caster_rotation = 0.0; - int caster_rotation_lastcount = 0; - int caster_rotation_count = 0; - bool caster_rotation_initialized = false; - // オドメトリ用変数 - double caster_rotation_prev_for_odom = 0.0; - double odom_x = 0.0; - double odom_y = 0.0; - double odom_yaw = 0.0; - - // 動作モード - enum class Mode{ - cmd, - stay, - stop - } mode = Mode::stop; - -}; - -} // namespace chassis_driver diff --git a/chassis_driver/chassis_driver/include/chassis_driver/visibility_control.h b/chassis_driver/chassis_driver/include/chassis_driver/visibility_control.h deleted file mode 100644 index 1147bae1..00000000 --- a/chassis_driver/chassis_driver/include/chassis_driver/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#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_ diff --git a/chassis_driver/chassis_driver/include/debug_printer/debug_printer.hpp b/chassis_driver/chassis_driver/include/debug_printer/debug_printer.hpp deleted file mode 100644 index 844ea889..00000000 --- a/chassis_driver/chassis_driver/include/debug_printer/debug_printer.hpp +++ /dev/null @@ -1,28 +0,0 @@ -#include -#include -#include "socketcan_interface_msg/msg/socketcan_if.hpp" - -namespace debug_printer{ -class DebugPrinter : public rclcpp::Node{ -public: - explicit DebugPrinter(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - explicit DebugPrinter(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - rclcpp::Subscription::SharedPtr _subscription_rpm_rx; - rclcpp::Subscription::SharedPtr _subscription_can_tx; - rclcpp::Subscription::SharedPtr _subscription_potentio; - - void _subscriber_callback_rpm_rx(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - void _subscriber_callback_can_tx(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - void _subscriber_callback_potentio(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg); - - rclcpp::Publisher::SharedPtr publisher_left_rpm_rx; - rclcpp::Publisher::SharedPtr publisher_right_rpm_rx; - rclcpp::Publisher::SharedPtr publisher_left_rpm_tx; - rclcpp::Publisher::SharedPtr publisher_right_rpm_tx; - rclcpp::Publisher::SharedPtr publisher_potentio; - - rclcpp::QoS _qos = rclcpp::QoS(10); -}; -} diff --git a/chassis_driver/chassis_driver/package.xml b/chassis_driver/chassis_driver/package.xml deleted file mode 100644 index 637a436a..00000000 --- a/chassis_driver/chassis_driver/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - chassis_driver - 0.0.0 - TODO: Package description - cmos - TODO: License declaration - - ament_cmake_ros - - rclcpp - std_msgs - geometry_msgs - nav_msgs - socketcan_interface_msg - steered_drive_msg - utilities - odrive_can - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/chassis_driver/chassis_driver/src/chassis_driver_node.cpp b/chassis_driver/chassis_driver/src/chassis_driver_node.cpp deleted file mode 100644 index 7ab6d198..00000000 --- a/chassis_driver/chassis_driver/src/chassis_driver_node.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#include "chassis_driver/chassis_driver_node.hpp" - -#include "utilities/data_utils.hpp" -#include "utilities/utils.hpp" - -#include -#include - -using namespace utils; - -namespace chassis_driver{ - -ChassisDriver::ChassisDriver(const rclcpp::NodeOptions& options) : ChassisDriver("", 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()), -caster_max_count(get_parameter("caster.max_count").as_int()), -caster_gear_ratio(get_parameter("caster.gear_ratio").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( - "cmd_vel", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_vel, this, std::placeholders::_1) - ); - _subscription_restart = this->create_subscription( - "restart", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1) - ); - _subscription_caster_orientation = this->create_subscription( - "can_rx_012", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_caster_orientation, this, std::placeholders::_1) - ); - _subscription_caster_rotation = this->create_subscription( - "can_rx_013", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_caster_rotation, this, std::placeholders::_1) - ); - _subscription_emergency = this->create_subscription( - "can_rx_712", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_emergency, this, std::placeholders::_1) - ); - _subscription_bodyvel = this->create_subscription( - "vectornav/velocity_body", - _qos, - std::bind(&ChassisDriver::_subscriber_callback_bodyvel, this, std::placeholders::_1) - ); - publisher_can = this->create_publisher("can_tx", _qos); - publisher_odrive = this->create_publisher("/odrive_axis0/control_message", _qos); - publisher_caster_data = this->create_publisher("caster_data", _qos); - publisher_odom = this->create_publisher("caster_odom", _qos); - - // ODriveのAxis Stateサービスクライアント作成 - odrive_axis_client_ = this->create_client("/odrive_axis0/request_axis_state"); - - _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(), "Chassis Driver Node has been started. max vel: %.2f m/s, steering angle: %.1f deg", - linear_limit.vel, rtod(steering_limit.pos)); -} - -void ChassisDriver::_subscriber_callback_vel(const steered_drive_msg::msg::SteeredDrive::SharedPtr msg){ - if(mode == Mode::stop) return; - mode = Mode::cmd; - - 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 ChassisDriver::_publisher_callback(){ -/*従動輪オドメトリ計算*/ - if(caster_rotation_initialized){ - const double delta_rotation = caster_rotation - caster_rotation_prev_for_odom; - caster_rotation_prev_for_odom = caster_rotation; - - const double delta_travel = delta_rotation * caster_wheel_radius; - const double delta_theta = delta_travel * std::sin(caster_orientation) / wheelbase; - - const double delta_center = delta_travel * std::cos(caster_orientation); - - const double heading_mid = odom_yaw + delta_theta * 0.5; - odom_x += delta_center * std::cos(heading_mid); - odom_y += delta_center * std::sin(heading_mid); - odom_yaw = normalize_angle(odom_yaw + delta_theta); - - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = this->now(); - odom_msg.header.frame_id = "base_link"; - odom_msg.pose.pose.position.x = odom_x; - odom_msg.pose.pose.position.y = odom_y; - odom_msg.pose.pose.position.z = 0.0; - - geometry_msgs::msg::Quaternion orientation_msg; - orientation_msg.x = 0.0; - orientation_msg.y = 0.0; - orientation_msg.z = std::sin(odom_yaw * 0.5); - orientation_msg.w = std::cos(odom_yaw * 0.5); - odom_msg.pose.pose.orientation = orientation_msg; - - publisher_odom->publish(odom_msg); - } - -/*速度計画*/ - // 速度計画機の参照 - linear_planner.cycle(); - const double linear_vel = linear_planner.vel(); - // 停止指令時 - if(mode == Mode::stop || mode == Mode::stay){ - this->send_rpm(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; - // RCLCPP_INFO(this->get_logger(), "DEL:%.2f POS:%.2f ENC:%.2f", rtod(delta), rtod(motor_pos), rtod(caster_orientation)); - - // ODriveにトルク指令を送信 - auto msg_odrive_control = std::make_shared(); - 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); - - // （テスト用）従動輪{目標舵角，実測舵角，回転位置｝を出版 - std_msgs::msg::Float64MultiArray caster_data_msg; - caster_data_msg.data = {delta, caster_orientation, caster_rotation}; - publisher_caster_data->publish(caster_data_msg); - -/*駆動輪制御*/ - // 直進時にはプリロードがかかるため，トルク差は0にする - const double angular_command = (straight_flag ? 0 : 1) * drive_pid.cycle(caster_orientation, delta) * body_vel_squared; - // RCLCPP_INFO(this->get_logger(), "ANG_CMD: %.2f", rtod(angular_command)); - send_rpm(linear_vel, angular_command); -} - -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); - - // ODriveのAxis Stateをクローズドループに設定（axis_requested_state: 8） - auto request = std::make_shared(); - request->axis_requested_state = 8; - odrive_axis_client_->async_send_request(request); - - caster_rotation_initialized = false; - caster_rotation_count = 0; - - caster_rotation_prev_for_odom = 0.0; - odom_x = 0.0; - odom_y = 0.0; - odom_yaw = 0.0; - - RCLCPP_INFO(this->get_logger(), "再起動"); -} - -void ChassisDriver::_subscriber_callback_caster_orientation(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - const int count = static_cast(bytes_to_int16(_candata)); - caster_orientation = count / static_cast(caster_max_count) * 2.0 * d_pi; - // RCLCPP_INFO(this->get_logger(), "CAS_ORI:%f CNT:%d", rtod(caster_orientation), count); -} -void ChassisDriver::_subscriber_callback_caster_rotation(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - const int count = static_cast(bytes_to_int16(_candata)); - if(!caster_rotation_initialized){ - caster_rotation_lastcount = count; - caster_rotation_initialized = true; - } - const int count_gap = count - caster_rotation_lastcount; - caster_rotation_lastcount = count; - - caster_rotation_count += count_gap; - if(count_gap > caster_max_count / 2) caster_rotation_count -= caster_max_count; - else if(count_gap < -caster_max_count / 2) caster_rotation_count += caster_max_count; - - // 取付位置からマイナスをかける - caster_rotation = -caster_rotation_count / static_cast(caster_max_count) * 2.0 * d_pi * caster_gear_ratio; - // RCLCPP_INFO(this->get_logger(), "CAS_ROT:%f CNT:%d", rtod(caster_rotation), count); - // RCLCPP_INFO(this->get_logger(), "ROT CRR:%f CNT:%d", rtod(current_rotation), count); -} -void ChassisDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - if(_candata[6] and mode!=Mode::stop){ - mode = Mode::stop; - RCLCPP_INFO(this->get_logger(), "緊急停止!"); - } -} -void ChassisDriver::_subscriber_callback_bodyvel(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg){ - current_body_vel = msg->twist.twist; - // RCLCPP_INFO(this->get_logger(), "VEL:%.2f", current_body_vel.linear.x); -} - -void ChassisDriver::send_rpm(const double linear_vel, const double 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; - const double right_rpm = (is_reverse_right ? -1 : 1) * (right_vel*30.0 / d_pi) * rotate_ratio; - - // RCLCPP_INFO(this->get_logger(), "right:%f left:%f", right_rpm, left_rpm); - // 出版 - auto msg_can = std::make_shared(); - msg_can->canid = 0x210; - msg_can->candlc = 8; - - uint8_t _candata[8]; - int32_to_bytes(_candata, static_cast(right_rpm)); - int32_to_bytes(_candata+4, static_cast(left_rpm)); - - for(int i=0; icandlc; i++) msg_can->candata[i]=_candata[i]; - publisher_can->publish(*msg_can); - -} - -double ChassisDriver::normalize_angle(double angle){ - return std::atan2(std::sin(angle), std::cos(angle)); -} - - -} // namespace chassis_driver diff --git a/chassis_driver/chassis_driver/src/debug_printer.cpp b/chassis_driver/chassis_driver/src/debug_printer.cpp deleted file mode 100644 index 81fe243a..00000000 --- a/chassis_driver/chassis_driver/src/debug_printer.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "debug_printer/debug_printer.hpp" -#include "utilities/data_utils.hpp" - -namespace debug_printer{ -DebugPrinter::DebugPrinter(const rclcpp::NodeOptions &options) : DebugPrinter("", options) {} - -DebugPrinter::DebugPrinter(const std::string &name_space, const rclcpp::NodeOptions &options) -: rclcpp::Node("debug_printer_node", name_space, options){ - - _subscription_rpm_rx = this->create_subscription( - "can_rx_711", - _qos, - std::bind(&DebugPrinter::_subscriber_callback_rpm_rx, this, std::placeholders::_1) - ); - _subscription_potentio = this->create_subscription( - "can_rx_11", - _qos, - std::bind(&DebugPrinter::_subscriber_callback_potentio, this, std::placeholders::_1) - ); - _subscription_can_tx = this->create_subscription( - "can_tx", - _qos, - std::bind(&DebugPrinter::_subscriber_callback_can_tx, this, std::placeholders::_1) - ); - - publisher_left_rpm_rx = this->create_publisher("left_rpm_rx", _qos); - publisher_right_rpm_rx = this->create_publisher("right_rpm_rx", _qos); - publisher_left_rpm_tx = this->create_publisher("left_rpm_tx", _qos); - publisher_right_rpm_tx = this->create_publisher("right_rpm_tx", _qos); - publisher_potentio = this->create_publisher("potentio", _qos); - - - - RCLCPP_INFO(this->get_logger(), "ロボテックモータドライバーに関するデータを整形してトピックに流します。"); -} - -void DebugPrinter::_subscriber_callback_rpm_rx(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - auto msg_tx = std::make_shared(); - - const int left = msg_tx->data = static_cast(bytes_to_int32(_candata)); - publisher_left_rpm_rx->publish(*msg_tx); - - const int right = msg_tx->data = static_cast(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); -} -void DebugPrinter::_subscriber_callback_can_tx(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - if(msg->canid == 0x210 && msg->candlc == 8){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - auto msg_tx = std::make_shared(); - - const int left = msg_tx->data = static_cast(bytes_to_int32(_candata)); - publisher_left_rpm_tx->publish(*msg_tx); - - const int right = msg_tx->data = static_cast(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); - } -} - -void DebugPrinter::_subscriber_callback_potentio(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ - uint8_t _candata[8]; - for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; - - auto msg_tx = std::make_shared(); - const int value = msg_tx->data = static_cast(bytes_to_int16(_candata)); - publisher_potentio->publish(*msg_tx); - RCLCPP_DEBUG(this->get_logger(), "POTENTIO:%d", value); -} - - -} // namespace - -int main(int argc, char * argv[]){ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/chassis_driver/chassis_driver/src/velplanner.cpp b/chassis_driver/chassis_driver/src/velplanner.cpp deleted file mode 100644 index 1557c628..00000000 --- a/chassis_driver/chassis_driver/src/velplanner.cpp +++ /dev/null @@ -1,69 +0,0 @@ -#include "base/velplanner.hpp" -#include "utilities/utils.hpp" - -using namespace utils; - -namespace velplanner{ - -rclcpp::Clock system_clock(RCL_ROS_TIME); -int64_t micros(){ - return system_clock.now().nanoseconds()*1e-3; -} - -//VelPlanner -void VelPlanner::cycle(){ - if (mode == Mode::vel){ - const double time = (micros() - start_time) / 1000000.0; - if (time < t1){ - old_time = micros(); - current_.acc = using_acc; - current_.vel = using_acc * time + first.vel; - current_.pos = using_acc / 2.0 * time * time + first.vel * time + first.pos; - } - else{ - mode = Mode::uniform_acceleration; - achieved_target = true; - current_ = target; - cycle(); - } - } - else if(mode == Mode::uniform_acceleration){ - const double dt = (micros() - old_time) / 1000000.0; - old_time = micros(); - current_.pos += current_.acc / 2.0 * dt * dt + current_.vel * dt; - current_.vel += current_.acc * dt; - } - else{ - current_.vel = 0.0; - current_.acc = 0.0; - } -} - -void VelPlanner::current(const Physics_t physics){ - current_ = physics; - mode = Mode::uniform_acceleration; - old_time = micros(); -} - -void VelPlanner::vel(double vel){ - this->vel(vel, micros()); -} - -void VelPlanner::vel(double vel, double start_time){ - this->vel(vel, (int64_t)(start_time * 1000000)); -} - -void VelPlanner::vel(double vel, int64_t start_time_us){ - start_time = start_time_us; - achieved_target = false; - mode = Mode::vel; - first = current_; - target.acc = 0.0; - target.vel = constrain(vel, -limit_.vel, limit_.vel); - const double diff_vel = target.vel - first.vel; - using_acc = (diff_vel >= 0) ? limit_.acc : -limit_.acc; - t1 = diff_vel / using_acc; - target.pos = using_acc * t1 * t1 / 2.0 + first.vel * t1 + first.pos; -} - -} diff --git a/chassis_driver/steered_drive_msg/CMakeLists.txt b/chassis_driver/steered_drive_msg/CMakeLists.txt deleted file mode 100644 index 33891dd7..00000000 --- a/chassis_driver/steered_drive_msg/CMakeLists.txt +++ /dev/null @@ -1,38 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(steered_drive_msg) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(rosidl_default_generators REQUIRED) - -set(msg_files - "msg/SteeredDrive.msg" -) - -# remove stale python package directory so symlink creation succeeds on rebuilds -set(_python_pkg_path "${CMAKE_CURRENT_BINARY_DIR}/ament_cmake_python/${PROJECT_NAME}/${PROJECT_NAME}") -if(EXISTS "${_python_pkg_path}" AND IS_DIRECTORY "${_python_pkg_path}") - file(REMOVE_RECURSE "${_python_pkg_path}") -endif() - -rosidl_generate_interfaces(${PROJECT_NAME} - ${msg_files} -) - -ament_export_dependencies(rosidl_default_runtime) - -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_package() diff --git a/chassis_driver/steered_drive_msg/msg/SteeredDrive.msg b/chassis_driver/steered_drive_msg/msg/SteeredDrive.msg deleted file mode 100644 index 7be11f9c..00000000 --- a/chassis_driver/steered_drive_msg/msg/SteeredDrive.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 steering_angle -float64 velocity diff --git a/chassis_driver/steered_drive_msg/package.xml b/chassis_driver/steered_drive_msg/package.xml deleted file mode 100644 index 8a714397..00000000 --- a/chassis_driver/steered_drive_msg/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - steered_drive_msg - 0.0.0 - Message definitions for steered drive control. - cmos - TODO: License declaration - - ament_cmake - - rosidl_default_generators - rosidl_default_generators - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/odrive_can b/odrive_can deleted file mode 160000 index 2c689e63..00000000 --- a/odrive_can +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 2c689e63c34c195d13169b807ba6ec7435fd0266 diff --git a/socketcan_interface b/socketcan_interface deleted file mode 160000 index b324118d..00000000 --- a/socketcan_interface +++ /dev/null @@ -1 +0,0 @@ -Subproject commit b324118d1afa1c25de86245f9035c4a40cd3cdde