diff --git a/sim_chassis_driver/CMakeLists.txt b/sim_chassis_driver/CMakeLists.txt new file mode 100644 index 00000000..ca11a595 --- /dev/null +++ b/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() diff --git a/sim_chassis_driver/include/sim_chassis_driver/sim_chassis_driver.hpp b/sim_chassis_driver/include/sim_chassis_driver/sim_chassis_driver.hpp new file mode 100644 index 00000000..cd56403c --- /dev/null +++ b/sim_chassis_driver/include/sim_chassis_driver/sim_chassis_driver.hpp @@ -0,0 +1,71 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#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::SharedPtr subscription_vel_; + rclcpp::Subscription::SharedPtr subscription_restart_; + rclcpp::Subscription::SharedPtr subscription_caster_orientation_; + 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 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::SharedPtr publisher_diff_drive_; + rclcpp::Publisher::SharedPtr publisher_reel_position_; + rclcpp::Publisher::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 diff --git a/sim_chassis_driver/include/sim_chassis_driver/visibility_control.h b/sim_chassis_driver/include/sim_chassis_driver/visibility_control.h new file mode 100644 index 00000000..6f3a788e --- /dev/null +++ b/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_ diff --git a/sim_chassis_driver/package.xml b/sim_chassis_driver/package.xml new file mode 100644 index 00000000..4865cf70 --- /dev/null +++ b/sim_chassis_driver/package.xml @@ -0,0 +1,28 @@ + + + + sim_chassis_driver + 0.0.0 + TODO: Package description + ken222d + TODO: License declaration + + ament_cmake_auto + + rclcpp + std_msgs + geometry_msgs + nav_msgs + socketcan_interface_msg + steered_drive_msg + utilities + chassis_driver + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sim_chassis_driver/src/sim_chassis_driver.cpp b/sim_chassis_driver/src/sim_chassis_driver.cpp new file mode 100644 index 00000000..3d7ef3d4 --- /dev/null +++ b/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 +#include + +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( + "cmd_vel", + qos_, + std::bind(&SimChassisDriver::_subscriber_callback_vel, this, std::placeholders::_1)); + + subscription_restart_ = this->create_subscription( + "restart", + qos_, + std::bind(&SimChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1)); + + subscription_caster_orientation_ = this->create_subscription( + "caster_orientation", + qos_, + std::bind(&SimChassisDriver::_subscriber_callback_caster_orientation, this, std::placeholders::_1)); + + subscription_bodyvel_ = this->create_subscription( + "vectornav/velocity_body", + qos_, + std::bind(&SimChassisDriver::_subscriber_callback_bodyvel, this, std::placeholders::_1)); + + publisher_diff_drive_ = this->create_publisher("cmd_vel_twists", qos_); + publisher_reel_position_ = this->create_publisher("caster_reel_position_cmd", qos_); + publisher_caster_data_ = this->create_publisher("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 \ No newline at end of file diff --git a/sim_executor/CMakeLists.txt b/sim_executor/CMakeLists.txt new file mode 100644 index 00000000..e7f8b654 --- /dev/null +++ b/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) diff --git a/sim_executor/config/sim_params.yaml b/sim_executor/config/sim_params.yaml new file mode 100644 index 00000000..03997ed2 --- /dev/null +++ b/sim_executor/config/sim_params.yaml @@ -0,0 +1,251 @@ +launch: #起動パラメータ + ros__parameters: + joy: false + vectornav : false + odrive : false + socketcan : false + # zed : false + sim : true + +/**: #ワイルドカード(ここのパラメータは全ノードから読める: <名前に注意>) + ros__parameters: + # 並進 速度[m/s],加速度[m/s^2],加減速度[-m/s^2],躍度[m/s^3] + linear_max: + vel : 2.0 + acc: 5.0 + jer: 0.0 + # 舵角 位置[deg] + steering_max : + pos : 15.0 + + tread : 0.6 + wheelbase : 0.8 + wheel_radius : 0.124 + interval_ms : 2 + caster: + max_count : 16384 + gear_ratio : 0.59090909090909 + wheel_radius : 0.12 + reel_radius : 0.02 + steering_radius : 0.032 + preload_length : 0.03 + preload_gain : 0.1 + drive_pid: + p_gain : 3.0 + i_gain : 0.0 + d_gain : 0.1 + +zed_wrapper_node: + ros__parameters: + grab_fps : 15 + # ZED X：HD1200 / HD1080 / SVGA + resolution : "HD1080" + # Depth mode：NONE / PERFORMANCE / QUALITY / ULTRA / NEURAL / NEURAL_PLUS + depth_mode : "NEURAL" + confidence_threshold : 50 + serial_number : 0 # 0 = auto-select + camera_frame_id : "camera_depth_link" + +controller_node: + ros__parameters: + linear_max_vel : 2.0 #[m/s] + +vectormap_server_node: + ros__parameters: + map_path : "aiformula_course.osm" + publish_period_ms : 1000 + earth_frame_id : "earth" + map_axis_convention : "image_x_right_y_up" + map_origin_pixel: + x : 0.0 + y : 0.0 + meter_per_pixel : 0.1739579 + map_origin_geodetic: + latitude : 36.11313110136178 + longitude: 139.98107529988687 + altitude : 50.0 + # map の +x 軸が East から何 rad だけ CCW に回転しているか [rad] + # front_view_extractor 式の +u 方向を ENU に投影して数値的に算出 + map_yaw_from_east : 0.02686650 + +localization_node: + ros__parameters: + update_period_ms : 100 + map_frame_id : "map" + base_frame_id : "base_link" + localized_pose_topic : "/localization/pose" + raw_pose_topic : "/localization/pose_raw" + velocity_topic : "/vectornav/velocity_body" + # VectorNav yaw is heading: 0 rad at North, positive counter-clockwise. + imu_yaw_convention : "heading_north_ccw" + # vectormap_server_node と同じ値を設定すること + map_origin_geodetic: + latitude : 36.11313110136178 + longitude: 139.98107529988687 + altitude : 50.0 + map_yaw_from_east : 0.02686650 + mask_threshold : 128 + pixel_step : 4 + max_observed_points : 2000 + min_observed_points : 5 + min_map_points : 5 + map_sample_interval_m : 0.25 + ground_plane_z_base : 0.0 + min_ground_intersection_distance : 0.2 + max_ground_intersection_distance : 30.0 + camera: + # zedx: width=640, height=360 + fx : 246.55866667 + fy : 246.389 + cx : 318.345 + cy : 186.59033333 + # 画像点をbase_linkへ変換するためのカメラ外部パラメータ + camera_to_base: + x : 0.055 + y : 0.0 + z : 0.54 + roll : -1.53938040026 + pitch : -0.00349065850 + yaw : -1.57428698530 + icp: + max_iterations : 20 + max_correspondence_distance : 1.5 + convergence_translation_epsilon : 0.01 + min_correspondences : 5 + ekf: + initial_position_variance : 1.0 + initial_yaw_variance : 0.3 + initial_velocity_variance : 1.0 + initial_yaw_rate_variance : 0.5 + # 不確かさ + process_position_variance : 0.2 + process_yaw_variance : 0.05 + process_velocity_variance : 0.5 + process_yaw_rate_variance : 0.2 + # 信頼度 + gnss_position_variance : 0.5 + imu_yaw_variance : 0.05 + icp_position_variance : 0.15 + # マハラノビスゲート閾値 (単位: sigma) + position_gate_dist : 10.0 + yaw_gate_dist : 10.0 + +odom_tf_node: + ros__parameters: + publish_period_ms : 20 + odom_frame_id : "odom" + base_frame_id : "base_link" + imu_topic : "/vectornav/imu" + velocity_topic : "/vectornav/velocity_body" + odom_topic : "/localization/odom" + # VectorNav yaw is heading: 0 rad at North, positive counter-clockwise. + imu_yaw_convention : "heading_north_ccw" + max_integration_dt : 0.2 + +map_odom_tf_node: + ros__parameters: + publish_period_ms : 50 # 再送周期 [ms]（20 Hz） + stale_warn_timeout_s : 2.0 # キャッシュ更新が途絶えた場合の警告閾値 [s] + map_frame_id : "map" + odom_frame_id : "odom" + base_frame_id : "base_link" + localized_pose_topic : "/localization/pose" + +lane_planner_node: + ros__parameters: + update_period_ms : 100 + map_frame_id : "map" + base_frame_id : "base_link" + vector_map_topic : "/vector_map" + localization_pose_topic : "/localization/pose" + nav_cmd_topic : "/planning/nav_cmd" + lane_change_topic : "/flag" + default_nav_cmd : "straight" + nav_cmd_fallback_order : ["straight", "left", "right"] + global_path_topic : "/planner/global_path" + route_lanelet_ids : [301, 302, 303, 304, 305, 311, 313, 312, 314, 315, 309, 310] + # aiformula 2026 course: 301, 302, 303, 304, 305, 311, 313, 312, 314, 315, 309, 310 + global_path_resample_interval_m : 0.2 + max_centerline_connection_gap_m : 0.5 + off_route_distance_threshold_m : 3.0 + route_lookahead_lanelet_count : 3 + +local_planner_server_node: + ros__parameters: + local_planner_plugin : "local_planner::VectormapFrenetPlugin" + update_period_ms : 100 + map_frame_id : "map" + base_frame_id : "base_link" + global_path_topic : "/planner/global_path" + local_path_topic : "/planner/local_path" + vector_map_topic : "/vector_map" + localization_pose_topic : "/localization/pose" + velocity_topic : "/vectornav/velocity_body" + objects_topic : "/perception/objects" + lane_switch_trigger_topic : "/flag" + # VectormapFrenetPlugin params + local_path_horizon_m : 15.0 + local_path_resample_interval_m : 0.2 + max_centerline_connection_gap_m : 0.5 + vehicle_width_m : 0.6 + lane_change_length_m : 10.0 + avoidance_detection_forward_distance_m : 15.0 + avoidance_hard_margin_m : 0.2 + avoidance_soft_margin_m : 0.3 + envelope_buffer_margin_m : 0.2 + avoidance_lateral_jerk_mps3 : 1.0 + avoidance_min_velocity_mps : 0.5 + max_avoidance_shift_m : 1.0 + frenet_collision_check_margin_m : 0.2 + frenet_weight_lateral_offset : 1.0 + frenet_weight_lateral_change : 0.2 + frenet_weight_avoidance_shift : 0.1 + +object_detector_node: + ros__parameters: + map_frame_id : "map" + base_frame_id : "base_link" + ground_z_threshold_m : 0.1 # [m] 地面除去Zカット高さ（base_link frame z基準） + voxel_leaf_size_m : 0.1 # [m] ボクセルサイズ + cluster_tolerance_m : 0.5 # [m] クラスタリング距離閾値 + min_cluster_size : 10 + max_cluster_size : 1000 + marker_height_m : 1.0 # [m] 可視化用Marker高さ + +controller_server_node: + ros__parameters: + controller_plugin : "motion_control::PurePursuitPlugin" + path_topic : "/planner/local_path" + pose_topic : "/localization/pose" + autonomous_topic : "/autonomous" + cmd_vel_topic : "/cmd_vel" + target_pose_topic : "/vectormap_control/target_pose" + map_frame_id : "map" + base_frame_id : "base_link" + # PurePursuitPlugin params (linear_max.*, steering_max.*, wheelbase は /** から継承) + lookahead_distance : 5.0 + steered_gain : 1.8 + +chassis_driver_node: + ros__parameters: + interval_ms : 2 + wheel_radius : 0.124 + reduction_ratio : 1.0 + reverse_left_flag : false + reverse_right_flag : false + caster: + max_count : 16384 + gear_ratio : 0.59090909090909 + wheel_radius : 0.12 # [m] + reel_radius : 0.02 # [m] + steering_radius : 0.032 # [m] + preload_length : 0.03 # [m] + preload_gain : 0.1 + drive_pid: + p_gain : 3.0 + i_gain : 0.0 + d_gain : 0.1 + +socketcan_interface_node: + ros__parameters: + if_name : "can_main" diff --git a/sim_executor/launch/sim_exec.launch.py b/sim_executor/launch/sim_exec.launch.py new file mode 100644 index 00000000..d74e8318 --- /dev/null +++ b/sim_executor/launch/sim_exec.launch.py @@ -0,0 +1,108 @@ +import os +import subprocess +import yaml +import launch +from launch import LaunchDescription +from launch.launch_description_sources import AnyLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node + +def generate_launch_description(): + # パラメータファイルのパス設定 + config_file_path = os.path.join( + get_package_share_directory('sim_executor'), + 'config', + 'sim_params.yaml' + ) + + # 起動パラメータファイルのロード + with open(config_file_path, 'r') as file: + launch_params = yaml.safe_load(file)['launch']['ros__parameters'] + + use_sim_time = launch_params.get('sim', False) + + # robot_state_publisher（URDF から TF を publish） + urdf_path = os.path.join( + get_package_share_directory('simulator'), + 'models', 'ai_car1', 'model.urdf', + ) + with open(urdf_path, 'r', encoding='utf-8') as f: + robot_description = f.read() + + robot_state_publisher = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + parameters=[{ + 'robot_description': robot_description, + 'use_sim_time': use_sim_time, + }], + output='screen', + ) + + # カメラフレームの静的TF（chassis → ai_car1/camera_depth_link/camera_depth_link） + # Foxy uses positional args: x y z yaw pitch roll frame_id child_frame_id + camera_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '0.055', '0.0', '0.54', + '0', '0', '0', + 'chassis', 'ai_car1/camera_depth_link/camera_depth_link', + ], + output='screen', + ) + + # メイン実行機ノードの作成 + main_exec_node = Node( + package = 'sim_executor', + executable = 'sim_exec', + parameters = [config_file_path], + output='screen' + ) + # socketcanノードの作成 + socketcan_node = Node( + package = 'socketcan_interface', + executable = 'socketcan_interface_node', + parameters = [config_file_path], + output='screen' + ) + # 操縦機ノードの作成 + joy_node = Node( + package = 'joy', + executable = 'joy_node', + output='screen' + ) + + # vectornav起動の作成 + vectornav_launch = launch.actions.IncludeLaunchDescription( + AnyLaunchDescriptionSource([os.path.join( + get_package_share_directory('vectornav'), 'launch/'), + 'vectornav.launch.py']) + ) + # odrive起動の作成 + # odrive_launch = launch.actions.IncludeLaunchDescription( + # AnyLaunchDescriptionSource([os.path.join( + # get_package_share_directory('sim_executor'), 'launch/'), + # 'odrive_can_launch.yaml']) + # ) + + # 起動エンティティクラスの作成 + launch_discription = LaunchDescription() + + # 起動の追加 + if(launch_params['joy'] is True): + launch_discription.add_action(joy_node) + if(launch_params['vectornav'] is True): + launch_discription.add_action(vectornav_launch) + # if(launch_params['odrive'] is True): + # launch_discription.add_action(odrive_launch) + if(launch_params['socketcan'] is True): + launch_discription.add_action(socketcan_node) + + launch_discription.add_action(robot_state_publisher) + launch_discription.add_action(camera_tf) + launch_discription.add_action(main_exec_node) + + return launch_discription diff --git a/sim_executor/package.xml b/sim_executor/package.xml new file mode 100644 index 00000000..9351f4be --- /dev/null +++ b/sim_executor/package.xml @@ -0,0 +1,37 @@ + + + + sim_executor + 0.0.0 + TODO: Package description + ken222d + TODO: License declaration + + rclcpp + rclcpp + rclcpp + + socketcan_interface + controller + sim_chassis_driver + frenet_planner + path_tracker + utilities + localization + motion_control + lane_planner + local_planner + object_detector + zed_wrapper + vectormap_server + gnssnav + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/sim_executor/src/sim.cpp b/sim_executor/src/sim.cpp new file mode 100644 index 00000000..8cc98a32 --- /dev/null +++ b/sim_executor/src/sim.cpp @@ -0,0 +1,63 @@ +#include + +#include "zed_wrapper/zed_wrapper_node.hpp" +#include "controller/controller_node.hpp" +#include "sim_chassis_driver/sim_chassis_driver.hpp" +#include "localization/map_odom_tf_node.hpp" +#include "localization/odom_tf_node.hpp" +#include "localization/localization_node.hpp" +#include "motion_control/controller_server.hpp" +#include "lane_planner/lane_planner_node.hpp" +#include "local_planner/local_planner_server.hpp" +#include "object_detector/object_detector_node.hpp" +#include "vectormap_server/vectormap_server_node.hpp" + +int main(int argc, char * argv[]){ + rclcpp::init(argc,argv); + rclcpp::executors::MultiThreadedExecutor exec; + + rclcpp::NodeOptions nodes_option; + nodes_option.allow_undeclared_parameters(true); + nodes_option.automatically_declare_parameters_from_overrides(true); + + // Read launch flags from the 'launch' node's parameters + // const bool use_zed = rclcpp::Node("launch", nodes_option).get_parameter("zed").as_bool(); + const bool use_sim = rclcpp::Node("launch", nodes_option).get_parameter("sim").as_bool(); + + if (use_sim) { + nodes_option.parameter_overrides({rclcpp::Parameter("use_sim_time", true)}); + } + + auto controller_node = std::make_shared(nodes_option); + auto sim_chassis_driver_node = std::make_shared(nodes_option); + auto vectormap_server_node = std::make_shared(nodes_option); + auto localization_node = std::make_shared(nodes_option); + auto odom_tf_node = std::make_shared(nodes_option); + auto map_odom_tf_node = std::make_shared(nodes_option); + auto lane_planner_node = std::make_shared(nodes_option); + auto local_planner_server_node = std::make_shared(nodes_option); + auto controller_server_node = std::make_shared(nodes_option); + auto object_detector_node = std::make_shared(nodes_option); + +#ifdef ENABLE_ZED + std::shared_ptr zed_wrapper_node; + if (use_zed) { + zed_wrapper_node = std::make_shared(nodes_option); + exec.add_node(zed_wrapper_node); + } +#endif + exec.add_node(controller_node); + exec.add_node(sim_chassis_driver_node); + exec.add_node(vectormap_server_node); + exec.add_node(localization_node); + exec.add_node(odom_tf_node); + exec.add_node(map_odom_tf_node); + exec.add_node(lane_planner_node); + exec.add_node(local_planner_server_node); + exec.add_node(controller_server_node); + exec.add_node(object_detector_node); + + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/simulator/CMakeLists.txt b/simulator/CMakeLists.txt index 414dc28c..8f168edc 100644 --- a/simulator/CMakeLists.txt +++ b/simulator/CMakeLists.txt @@ -17,11 +17,14 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ignition-gazebo6 REQUIRED) +add_subdirectory(plugin) + # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) install( - DIRECTORY launch world models + DIRECTORY launch world models plugin DESTINATION share/${PROJECT_NAME} ) @@ -46,4 +49,4 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() -ament_package() +ament_package() \ No newline at end of file diff --git a/simulator/launch/gazebo_ignition.launch.py b/simulator/launch/gazebo_ignition.launch.py index 4d2bfce3..a64725a9 100644 --- a/simulator/launch/gazebo_ignition.launch.py +++ b/simulator/launch/gazebo_ignition.launch.py @@ -38,7 +38,9 @@ def generate_launch_description(): '/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry', '/navsat@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat', '/imu_raw@sensor_msgs/msg/Imu@gz.msgs.IMU', - '/cmd_vel_twist@geometry_msgs/msg/Twist@gz.msgs.Twist'], + '/cmd_vel_twists@geometry_msgs/msg/Twist@gz.msgs.Twist', + '/caster_reel_position_cmd@std_msgs/msg/Float64@gz.msgs.Double', + ], output='screen', remappings=[ ('/image_raw', '/zed/zed_node/rgb/image_rect_color'), @@ -48,16 +50,16 @@ def generate_launch_description(): ] ) - steered_to_twist = Node( - package='simulator', - executable='steered_to_twist.py', - output='screen', - parameters=[{ - 'input_topic': '/cmd_vel', - 'output_topic': '/cmd_vel_twist', - 'wheel_base': 0.8, - }] - ) + # steered_to_twist = Node( + # package='simulator', + # executable='steered_to_twist.py', + # output='screen', + # parameters=[{ + # 'input_topic': '/cmd_vel', + # 'output_topic': '/cmd_vel_twist', + # 'wheel_base': 0.8, + # }] + # ) convert_vectornav_pose = Node( package='simulator', @@ -77,47 +79,47 @@ def generate_launch_description(): }] ) - urdf_path = os.path.join( - get_package_share_directory('simulator'), - 'models', - 'ai_car1', - 'model.urdf', - ) - ros2_control_src = os.path.join( - get_package_share_directory('simulator'), - 'models', - 'ai_car1', - 'ros2_control.yaml', - ) - ros2_control_dst = '/tmp/simulator_ai_car1_ros2_control.yaml' - shutil.copyfile(ros2_control_src, ros2_control_dst) - with open(urdf_path, 'r', encoding='utf-8') as urdf_file: - robot_description = urdf_file.read() + # urdf_path = os.path.join( + # get_package_share_directory('simulator'), + # 'models', + # 'ai_car1', + # 'model.urdf', + # ) + # ros2_control_src = os.path.join( + # get_package_share_directory('simulator'), + # 'models', + # 'ai_car1', + # 'ros2_control.yaml', + # ) + # ros2_control_dst = '/tmp/simulator_ai_car1_ros2_control.yaml' + # shutil.copyfile(ros2_control_src, ros2_control_dst) + # with open(urdf_path, 'r', encoding='utf-8') as urdf_file: + # robot_description = urdf_file.read() - robot_state_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - parameters=[{ - 'robot_description': robot_description, - 'use_sim_time': True, - }], - output='screen', - ) + # robot_state_publisher = Node( + # package='robot_state_publisher', + # executable='robot_state_publisher', + # parameters=[{ + # 'robot_description': robot_description, + # 'use_sim_time': True, + # }], + # output='screen', + # ) - caster_yaw_position_spawner = Node( - package='controller_manager', - executable='spawner', - arguments=[ - 'caster_yaw_position_controller', - '--controller-manager', - '/controller_manager', - '--controller-manager-timeout', - '60', - '--switch-timeout', - '60', - ], - output='screen', - ) + # caster_yaw_position_spawner = Node( + # package='controller_manager', + # executable='spawner', + # arguments=[ + # 'caster_yaw_position_controller', + # '--controller-manager', + # '/controller_manager', + # '--controller-manager-timeout', + # '60', + # '--switch-timeout', + # '60', + # ], + # output='screen', + # ) return LaunchDescription([ world_arg, @@ -127,13 +129,13 @@ def generate_launch_description(): launch_arguments=[ ('gz_args', [world_file_path, ' -r'])] ), - steered_to_twist, + #steered_to_twist, bridge, - robot_state_publisher, + #robot_state_publisher, convert_vectornav_pose, convert_vectornav_velocity_body, - TimerAction( - period=2.0, - actions=[caster_yaw_position_spawner], - ), - ]) + # TimerAction( + # period=2.0, + # actions=[caster_yaw_position_spawner], + # ), + ]) \ No newline at end of file diff --git a/simulator/models/ai_car1/meshes/0522/bane.stl b/simulator/models/ai_car1/meshes/0522/bane.stl new file mode 100755 index 00000000..64fc57a5 Binary files /dev/null and b/simulator/models/ai_car1/meshes/0522/bane.stl differ diff --git a/simulator/models/ai_car1/meshes/0522/mover_left.stl b/simulator/models/ai_car1/meshes/0522/mover_left.stl new file mode 100755 index 00000000..397d6bf1 Binary files /dev/null and b/simulator/models/ai_car1/meshes/0522/mover_left.stl differ diff --git a/simulator/models/ai_car1/meshes/0522/mover_right.stl b/simulator/models/ai_car1/meshes/0522/mover_right.stl new file mode 100755 index 00000000..cd448df0 Binary files /dev/null and b/simulator/models/ai_car1/meshes/0522/mover_right.stl differ diff --git a/simulator/models/ai_car1/meshes/0522/pulley.stl b/simulator/models/ai_car1/meshes/0522/pulley.stl new file mode 100755 index 00000000..d1f9cf78 Binary files /dev/null and b/simulator/models/ai_car1/meshes/0522/pulley.stl differ diff --git a/simulator/models/ai_car1/meshes/0522/rotater.stl b/simulator/models/ai_car1/meshes/0522/rotater.stl new file mode 100755 index 00000000..7e0f72f9 Binary files /dev/null and b/simulator/models/ai_car1/meshes/0522/rotater.stl differ diff --git a/simulator/models/ai_car1/meshes/used/bane_double_2.stl b/simulator/models/ai_car1/meshes/used/bane_double_2.stl new file mode 100755 index 00000000..64fc57a5 Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/bane_double_2.stl differ diff --git a/simulator/models/ai_car1/meshes/used/mover_left.stl b/simulator/models/ai_car1/meshes/used/mover_left.stl new file mode 100755 index 00000000..397d6bf1 Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/mover_left.stl differ diff --git a/simulator/models/ai_car1/meshes/used/mover_right.stl b/simulator/models/ai_car1/meshes/used/mover_right.stl new file mode 100755 index 00000000..cd448df0 Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/mover_right.stl differ diff --git a/simulator/models/ai_car1/meshes/used/preload_1_2.stl b/simulator/models/ai_car1/meshes/used/preload_1_2.stl new file mode 100755 index 00000000..6799fcca Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/preload_1_2.stl differ diff --git a/simulator/models/ai_car1/meshes/used/preload_3_2.stl b/simulator/models/ai_car1/meshes/used/preload_3_2.stl new file mode 100755 index 00000000..4457edde Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/preload_3_2.stl differ diff --git a/simulator/models/ai_car1/meshes/used/pulley.stl b/simulator/models/ai_car1/meshes/used/pulley.stl new file mode 100755 index 00000000..d1f9cf78 Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/pulley.stl differ diff --git a/simulator/models/ai_car1/meshes/used/rotater.stl b/simulator/models/ai_car1/meshes/used/rotater.stl new file mode 100755 index 00000000..d8c21eea Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/rotater.stl differ diff --git a/simulator/models/ai_car1/meshes/used/rotater_2.stl b/simulator/models/ai_car1/meshes/used/rotater_2.stl new file mode 100755 index 00000000..7e0f72f9 Binary files /dev/null and b/simulator/models/ai_car1/meshes/used/rotater_2.stl differ diff --git a/simulator/models/ai_car1/model.sdf b/simulator/models/ai_car1/model.sdf index babad369..ac499b5a 100644 --- a/simulator/models/ai_car1/model.sdf +++ b/simulator/models/ai_car1/model.sdf @@ -1,6 +1,7 @@ + 1 0 0 0 0 0 0 @@ -13,42 +14,30 @@ 0.6 0.124 30 - cmd_vel_twist + cmd_vel_twists odom - - robot_description - robot_state_publisher - /tmp/simulator_ai_car1_ros2_control.yaml - controller_manager - - /caster_yaw_position_controller/commands:=/cmd_caster - - - - - gz_ros2_control/GazeboSimSystem - - - - -0.26 - 0.26 - - - - - + + pulley_joint + rotator_joint + caster_reel_position_cmd + caster_orientation + 1000 + 0.262 + 0.040 + 0.040 + @@ -77,12 +66,17 @@ - 0 0 0 0 0 3.14159 + 0 0 0 0 0 3.14 package://simulator/models/ai_car1/meshes/AIF_body.dae + + + 0x04 + + @@ -120,6 +114,11 @@ 0.05 0.12 0.12 + + + 0x01 + + @@ -158,19 +157,364 @@ 0.05 0.12 0.12 + + + 0x01 + + - -0.8 0 0 0 0 1.57 + -0.8 0 0 0 0 -1.57 + + 0 0.1 0.25 0 0 -1.57 + + 1.0 + + 0.04778 + 0 + 0 + 0.04778 + 0 + 0.07875 + + + + + 0 0 0 0 0 0 + + + package://simulator/models/ai_car1/meshes/0522/pulley.stl + 0.001 0.001 0.001 + + + + 1.0 1.0 0.0 1 [ 環境光：黄色 ] + 1.0 1.0 0.0 1 [ 拡散反射：黄色 ] + 0.2 0.2 0.2 1 [ 少し光沢 ] + 0 0 0 1 [ 発光なし ] + + + + + + 0 0 0.233 0 0 0 + + + 0.025 + 0.005 + + + + + 0x01 + + + + + + + 0 0.1 0.25 0 0 -1.57 + + 1.0 + + 0.04778 + 0 + 0 + 0.04778 + 0 + 0.07875 + + + + + 0 0 0 0 0 0 + + + package://simulator/models/ai_car1/meshes/0522/bane.stl + 0.001 0.001 0.001 + + + + 0.0 1.0 0.0 1 [ 環境光：黄色 ] + 0.0 1.0 0.0 1 [ 拡散反射：黄色 ] + 0.2 0.2 0.2 1 [ 少し光沢 ] + 0 0 0 1 [ 発光なし ] + + + + + + + -0.105 0 0.19 0 1.57 0 + + + 0.004 + 0.005 + + + + + 0x01 + + + + + + + + 0 0.1 0.25 0 0 -1.57 + + 1.0 + + 0.04778 + 0 + 0 + 0.04778 + 0 + 0.07875 + + + + + 0 0 0 0 0 0 + + + package://simulator/models/ai_car1/meshes/0522/mover_left.stl + 0.001 0.001 0.001 + + + + 1.0 0.0 0.0 1 [ 環境光：黄色 ] + 1.0 0.0 0.0 1 [ 拡散反射：黄色 ] + 0.2 0.2 0.2 1 [ 少し光沢 ] + 0 0 0 1 [ 発光なし ] + + + + + + + 0.0114 -0.031 0.19 0 0 0 + + + 0.004 + 0.025 + + + + + 0x01 + + + + + + + 0 0.1 0.25 0 0 -1.57 + + 1.0 + + 0.04778 + 0 + 0 + 0.04778 + 0 + 0.07875 + + + + + 0 0 0 0 0 0 + + + package://simulator/models/ai_car1/meshes/0522/mover_right.stl + 0.001 0.001 0.001 + + + + 1.0 0.0 0.0 1 [ 環境光：黄色 ] + 1.0 0.0 0.0 1 [ 拡散反射：黄色 ] + 0.2 0.2 0.2 1 [ 少し光沢 ] + 0 0 0 1 [ 発光なし ] + + + + + + + 0.0114 0.031 0.19 0 0 0 + + + 0.004 + 0.025 + + + + + 0x01 + + + + + + + 0 0.1 0.25 0 0 -1.57 + + 0.7 + + 0.001 + 0 + 0 + 0.001 + 0 + 0.001 + + + + + 0 0 0 0 0 0 + + + package://simulator/models/ai_car1/meshes/0522/rotater.stl + 0.001 0.001 0.001 + + + + 1.0 0.0 1.0 1 [ 環境光：黄色 ] + 1.0 0.0 1.0 1 [ 拡散反射：黄色 ] + 0.2 0.2 0.2 1 [ 少し光沢 ] + 0 0 0 1 [ 発光なし ] + + + + + + -0.0088 0.032 0.2 0 0 0 + + + 0.005 0.005 0.003 + + + + + 0x01 + + + + + + -0.0088 -0.032 0.2 0 0 0 + + + 0.005 0.005 0.003 + + + + + 0x01 + + + + + + 0.018 -0.0305 0.2 0 0 0.17 + + + 0.005 0.005 0.003 + + + + + 0x01 + + + + + + 0.018 0.0305 0.2 0 0 -0.17 + + + 0.005 0.005 0.003 + + + + + 0x01 + + + + + - + 0 0 0 0 0 0 - 5.0 + 0.7 0.001 0 @@ -183,7 +527,7 @@ - + 0.1 0.0 -0.25 0 0 1.57 1.0 @@ -213,11 +557,37 @@ 0.1 0.1 0.1 + + + 0x01 + + + + chassis @@ -225,7 +595,7 @@ 0 1 0 - 84.4 + 9.4 0.9 @@ -240,7 +610,7 @@ 0 1 0 - 84.4 + 9.4 0.9 @@ -248,24 +618,97 @@ - + chassis - caster_yaw + pulley + + 0 0 1 + + + + + + + + chassis + bane + + 1 0 0 + + -0.03 + 0.01 + 9.4 + + + 0.1 + 0.9 + + + + + + + chassis + mover_left + + 1 0 0 + + -0.1 + 0.1 + 9.4 + + + 0.1 + 0.9 + + + + + + + chassis + mover_right + + 1 0 0 + + -0.1 + 0.1 + 9.4 + + + 0.1 + 0.9 + + + + + + chassis + rotator 0 0 1 - -0.26 - 0.26 - 1000000.0 + -0.261 + 0.261 10.0 - 12.0 - 0.0 + + + rotator + caster_yaw + + caster_yaw caster @@ -326,7 +769,6 @@ - 0.055 0.0 0.54 0.0 0.0 0.0 @@ -404,10 +846,37 @@ true - 5.0 + 1.0 navsat + + + diff --git a/simulator/plugin/CMakeLists.txt b/simulator/plugin/CMakeLists.txt new file mode 100644 index 00000000..39c9d86e --- /dev/null +++ b/simulator/plugin/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_minimum_required(VERSION 3.10) +project(gazebo_plugins) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +find_package(ament_index_cpp REQUIRED) +find_package(PkgConfig REQUIRED) +pkg_check_modules(PROJ REQUIRED proj) + +find_package(ignition-gazebo6 REQUIRED) +find_package(ignition-transport11 REQUIRED) +find_package(ignition-msgs8 REQUIRED) +find_package(Eigen3 REQUIRED) + +add_library(CasterControl SHARED + CasterControl.cc +) + +target_include_directories(CasterControl PRIVATE + . + ${PROJ_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} +) + +target_link_libraries(CasterControl + PRIVATE + ignition-gazebo6::core + ignition-transport11::core + ignition-msgs8::core + ament_index_cpp::ament_index_cpp + ${PROJ_LIBRARIES} +) + +install(TARGETS + CasterControl + DESTINATION lib +) \ No newline at end of file diff --git a/simulator/plugin/CasterControl.cc b/simulator/plugin/CasterControl.cc new file mode 100644 index 00000000..e938a4d7 --- /dev/null +++ b/simulator/plugin/CasterControl.cc @@ -0,0 +1,250 @@ +#include "CasterControl.hh" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ + class CasterControlPrivate + { + public: + void JointForceCmdCallback(const ignition::msgs::Double &_msg); + void FindJointEntities(EntityComponentManager &_ecm); + void setBase(); + + ignition::transport::Node node_; + ignition::transport::Node::Publisher jointOrientationPub_; + + //std::string MODEL_NAME; + std::string PULLEY_JOINT_NAME; + std::string ROTATOR_JOINT_NAME; + + double pulley_radius{0.015}; + double rotator_radius{0.035}; + double spring_length{0.0}; + double alpha{0.174}; // 不感帯 + double K{500.0}; // バネ定数 + double Damp{0.02}; + double pulley_theta{0.0}; + double rotator_theta{0.0}; + bool isBaseSet{false}; + double x{0.0}; + double ReactionForce{0.0}; + + std::mutex mutex_; + ignition::gazebo::Model model_; + std::string SubtopicName; + std::string PubtopicName; + Entity modelEntity = kNullEntity; + Entity PulleyJointEntity = kNullEntity; + Entity RotatorJointEntity = kNullEntity; + }; + + CasterControl::CasterControl() : dataPtr(std::make_unique()) + { + } + + CasterControl::~CasterControl() = default; + + void CasterControlPrivate::JointForceCmdCallback(const ignition::msgs::Double &_msg) + { + std::lock_guard lock(mutex_); + pulley_theta = _msg.data(); + } + + void CasterControlPrivate::FindJointEntities(EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, + const components::Joint * /*_joint*/, + const components::Name *_name) -> bool + { + if (_name->Data() == PULLEY_JOINT_NAME) + { + std::cout << "Found Pulley"<< std::endl; + PulleyJointEntity = _entity; + } + else if (_name->Data() == ROTATOR_JOINT_NAME) + { + std::cout << "Found Rotator"<< std::endl; + RotatorJointEntity = _entity; + } + if (PulleyJointEntity != kNullEntity && RotatorJointEntity != kNullEntity) + { + return false; + } + return true; + }); + } + + void CasterControlPrivate::setBase() + { + x = 0.0; + ReactionForce = 0.0; + pulley_theta = 0.0; + rotator_theta = 0.0; + isBaseSet = true; + } + + void CasterControl::Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &/*_eventMgr*/) + { + dataPtr->model_ = Model(_entity); + if (dataPtr->model_.Valid(_ecm)) + { + dataPtr->modelEntity = _entity; + } + else + { + std::cerr << "CasterControl plugin should be attached to a model entity.\n" + << "Plugin will not function correctly." << std::endl; + return; + } + + if (_sdf) + { + dataPtr->PULLEY_JOINT_NAME = (_sdf->HasElement("pulley_joint") ? _sdf->Get("pulley_joint") : "caster_reel_joint"); + dataPtr->ROTATOR_JOINT_NAME = (_sdf->HasElement("rotator_joint") ? _sdf->Get("rotator_joint") : "caster_steer_joint"); + dataPtr->SubtopicName = (_sdf->HasElement("sub_topic_name") ? _sdf->Get("sub_topic_name") : "/caster_reel_position_cmd"); + dataPtr->PubtopicName = (_sdf->HasElement("pub_topic_name") ? _sdf->Get("pub_topic_name") : "/caster_orientation"); + dataPtr->Damp = (_sdf->HasElement("damping") ? _sdf->Get("damping") : 0.02); + dataPtr->K = (_sdf->HasElement("k_spring") ? _sdf->Get("k_spring") : 500.0); + dataPtr->alpha = (_sdf->HasElement("alpha") ? _sdf->Get("alpha") : 0.174); + dataPtr->pulley_radius = (_sdf->HasElement("pulley_radius") ? _sdf->Get("pulley_radius") : 0.015); + dataPtr->rotator_radius = (_sdf->HasElement("rotator_radius") ? _sdf->Get("rotator_radius") : 0.035); + } + + dataPtr->node_.Subscribe(dataPtr->SubtopicName, &CasterControlPrivate::JointForceCmdCallback, dataPtr.get()); + dataPtr->jointOrientationPub_ = dataPtr->node_.Advertise(dataPtr->PubtopicName); + + dataPtr->setBase(); + std::cout << "CasterControl Plugin configured successfully.\n" << std::endl; + std::cout << "Pulley Joint Name: " << dataPtr->PULLEY_JOINT_NAME << std::endl; + std::cout << "Rotator Joint Name: " << dataPtr->ROTATOR_JOINT_NAME << std::endl; + std::cout << "Subtopic Name: " << dataPtr->SubtopicName << std::endl; + std::cout << "Pubtopic Name: " << dataPtr->PubtopicName << std::endl; + std::cout << "Damping: " << dataPtr->Damp << std::endl; + std::cout << "Spring Constant (K): " << dataPtr->K << std::endl; + std::cout << "Alpha: " << dataPtr->alpha << std::endl; + std::cout << "Pulley Radius: " << dataPtr->pulley_radius << std::endl; + std::cout << "Rotator Radius: " << dataPtr->rotator_radius << std::endl; + } + + void CasterControl::PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) + { + if (_info.paused) return; + // ジョイントEntityの初回探索 + if (dataPtr->RotatorJointEntity == kNullEntity) + { + dataPtr->FindJointEntities(_ecm); + if (dataPtr->RotatorJointEntity == kNullEntity) return; + } + // 関節位置 (rotator_theta) の取得 + auto posComp = _ecm.Component(dataPtr->RotatorJointEntity); + std::cout << "RotatorJointEntity: " << dataPtr->RotatorJointEntity << std::endl; + std::cout << "Rotator Joint Position: " << (posComp ? std::to_string(posComp->Data()[0]) : "Component not found") << std::endl; + if (!posComp) + { + _ecm.CreateComponent(this->dataPtr->RotatorJointEntity, + components::JointPosition()); + } + if (!posComp || posComp->Data().empty()) return; + double raw_theta = posComp->Data()[0]; + + // ロボット（親リンク）基準の相対角度（-π ～ +π）に変換 + dataPtr->rotator_theta = std::atan2(std::sin(raw_theta), std::cos(raw_theta)); + std::cout << "rotator_theta (relative to parent): " << dataPtr->rotator_theta << std::endl; + + double current_pulley_theta = 0.0; + { + std::lock_guard lock(dataPtr->mutex_); + current_pulley_theta = dataPtr->pulley_theta; + std::cout << "current_pulley_theta: " << current_pulley_theta << std::endl; + } + + dataPtr->rotator_theta = posComp->Data()[0]; + + // カム機構の計算 + double abs_rotator_theta = std::abs(dataPtr->rotator_theta); + int sign = (dataPtr->rotator_theta > 0) - (dataPtr->rotator_theta < 0); + + + if (abs_rotator_theta < dataPtr->alpha) + { + dataPtr->x = dataPtr->rotator_radius * std::sin(abs_rotator_theta); + } + else + { + dataPtr->x = dataPtr->rotator_radius * std::sin(dataPtr->alpha) + + dataPtr->rotator_radius * std::sin(abs_rotator_theta - dataPtr->alpha); + } + + // バネ力と法線反力の計算 + double preload = dataPtr->pulley_radius * current_pulley_theta; + std::cout << "preload: " << preload << std::endl; + double spring_force = dataPtr->K * (dataPtr->x + preload); + + double F_r = 0.0; + if (abs_rotator_theta < dataPtr->alpha) + { + F_r = spring_force * std::cos(abs_rotator_theta); + } + else + { + F_r = spring_force * std::cos(abs_rotator_theta - dataPtr->alpha); + } + + // 復元トルク + dataPtr->ReactionForce = -sign * dataPtr->rotator_radius * F_r; + std::cout << "ReactionForce: " << dataPtr->ReactionForce << std::endl; + + auto forceCmdComp = _ecm.Component(dataPtr->RotatorJointEntity); + if (!forceCmdComp) + { + _ecm.CreateComponent(dataPtr->RotatorJointEntity, components::JointForceCmd({dataPtr->ReactionForce})); + } + else + { + forceCmdComp->Data()[0] = dataPtr->ReactionForce; + } + } + + void CasterControl::PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &/*_ecm*/) + { + if (_info.paused) return; + + ignition::msgs::Double msg; + msg.set_data(dataPtr->rotator_theta); + dataPtr->jointOrientationPub_.Publish(msg); + } + +} // namespace gazebo +} // namespace ignition + +IGNITION_ADD_PLUGIN( + ignition::gazebo::CasterControl, + ignition::gazebo::System, + ignition::gazebo::CasterControl::ISystemConfigure, + ignition::gazebo::CasterControl::ISystemPreUpdate, + ignition::gazebo::CasterControl::ISystemPostUpdate) + +IGNITION_ADD_PLUGIN_ALIAS(ignition::gazebo::CasterControl, "ignition::gazebo::CasterControl") \ No newline at end of file diff --git a/simulator/plugin/CasterControl.hh b/simulator/plugin/CasterControl.hh new file mode 100644 index 00000000..c4afa17b --- /dev/null +++ b/simulator/plugin/CasterControl.hh @@ -0,0 +1,27 @@ +#include +#include + +namespace ignition +{ +namespace gazebo +{ + class CasterControlPrivate; + + class CasterControl : public System, public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate + { + public: + CasterControl(); + ~CasterControl() override; + void Configure(const Entity &_entity, + const std::shared_ptr &_sdf, + EntityComponentManager &_ecm, + EventManager &_eventMgr) override; + void PreUpdate(const UpdateInfo &_info, + EntityComponentManager &_ecm) override; + void PostUpdate(const UpdateInfo &_info, + const EntityComponentManager &_ecm) override; + + private: std::unique_ptr dataPtr; + }; +}// namespace gazebo +}// namespace ignition \ No newline at end of file diff --git a/simulator/world/shihou_world.sdf b/simulator/world/shihou_world.sdf index ab44e1d1..73d86d82 100644 --- a/simulator/world/shihou_world.sdf +++ b/simulator/world/shihou_world.sdf @@ -209,4 +209,4 @@ - \ No newline at end of file +