diff --git a/roboteq_driver/CMakeLists.txt b/chassis_driver/CMakeLists.txt similarity index 84% rename from roboteq_driver/CMakeLists.txt rename to chassis_driver/CMakeLists.txt index 4714f80..e1ff2c3 100644 --- a/roboteq_driver/CMakeLists.txt +++ b/chassis_driver/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(roboteq_driver) +project(chassis_driver) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -14,12 +14,12 @@ find_package(geometry_msgs REQUIRED) find_package(socketcan_interface_msg REQUIRED) find_package(utilities REQUIRED) -add_library(roboteq_driver_node src/roboteq_driver_node.cpp) -target_compile_features(roboteq_driver_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -target_include_directories(roboteq_driver_node PUBLIC +add_library(chassis_driver_node src/chassis_driver_node.cpp) +target_compile_features(chassis_driver_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +target_include_directories(chassis_driver_node PUBLIC $ $) -ament_target_dependencies(roboteq_driver_node +ament_target_dependencies(chassis_driver_node rclcpp std_msgs geometry_msgs @@ -29,7 +29,7 @@ ament_target_dependencies(roboteq_driver_node # 実行可能ノード add_executable(debug_printer - src/debug_printer + src/debug_printer.cpp ) target_include_directories(debug_printer PUBLIC $ @@ -43,14 +43,14 @@ ament_target_dependencies(debug_printer # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. -target_compile_definitions(roboteq_driver_node PRIVATE "ROBOTEQ_DRIVER_BUILDING_LIBRARY") +target_compile_definitions(chassis_driver_node PRIVATE "CHASSIS_DRIVER_BUILDING_LIBRARY") install( DIRECTORY include/ DESTINATION include ) install( - TARGETS roboteq_driver_node + TARGETS chassis_driver_node EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -77,7 +77,7 @@ ament_export_include_directories( include ) ament_export_libraries( - roboteq_driver_node + chassis_driver_node ) ament_export_targets( export_${PROJECT_NAME} diff --git a/roboteq_driver/include/roboteq_driver/roboteq_driver_node.hpp b/chassis_driver/include/chassis_driver/chassis_driver_node.hpp similarity index 73% rename from roboteq_driver/include/roboteq_driver/roboteq_driver_node.hpp rename to chassis_driver/include/chassis_driver/chassis_driver_node.hpp index 910d9bd..a6fcce1 100644 --- a/roboteq_driver/include/roboteq_driver/roboteq_driver_node.hpp +++ b/chassis_driver/include/chassis_driver/chassis_driver_node.hpp @@ -2,21 +2,23 @@ #include #include +#include #include #include #include "socketcan_interface_msg/msg/socketcan_if.hpp" +#include "utilities/velplanner.hpp" -#include "roboteq_driver/visibility_control.h" +#include "chassis_driver/visibility_control.h" -namespace roboteq_driver{ +namespace chassis_driver{ -class RoboteqDriver : public rclcpp::Node { +class ChassisDriver : public rclcpp::Node { public: - ROBOTEQ_DRIVER_PUBLIC - explicit RoboteqDriver(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + CHASSIS_DRIVER_PUBLIC + explicit ChassisDriver(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - ROBOTEQ_DRIVER_PUBLIC - explicit RoboteqDriver(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + CHASSIS_DRIVER_PUBLIC + explicit ChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); private: rclcpp::Subscription::SharedPtr _subscription_vel; @@ -36,9 +38,17 @@ class RoboteqDriver : public rclcpp::Node { rclcpp::Publisher::SharedPtr publisher_can; rclcpp::Publisher::SharedPtr publisher_steer; + rclcpp::Publisher::SharedPtr publisher_ref_vel; rclcpp::QoS _qos = rclcpp::QoS(10); + + // 速度計画機 + velplanner::VelPlanner linear_planner; + const velplanner::Limit linear_limit; + velplanner::VelPlanner angular_planner; + const velplanner::Limit angular_limit; + // 定数 const int interval_ms; const double wheel_radius; @@ -48,9 +58,6 @@ class RoboteqDriver : public rclcpp::Node { const bool is_reverse_left; const bool is_reverse_right; - // 制御 - std::shared_ptr vel; - // 動作モード enum class Mode{ cmd, @@ -60,4 +67,4 @@ class RoboteqDriver : public rclcpp::Node { }; -} // namespace roboteq_driver +} // namespace chassis_driver diff --git a/chassis_driver/include/chassis_driver/visibility_control.h b/chassis_driver/include/chassis_driver/visibility_control.h new file mode 100644 index 0000000..1147bae --- /dev/null +++ b/chassis_driver/include/chassis_driver/visibility_control.h @@ -0,0 +1,35 @@ +#ifndef CHASSIS_DRIVER__VISIBILITY_CONTROL_H_ +#define CHASSIS_DRIVER__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ + #ifdef __GNUC__ + #define CHASSIS_DRIVER_EXPORT __attribute__ ((dllexport)) + #define CHASSIS_DRIVER_IMPORT __attribute__ ((dllimport)) + #else + #define CHASSIS_DRIVER_EXPORT __declspec(dllexport) + #define CHASSIS_DRIVER_IMPORT __declspec(dllimport) + #endif + #ifdef CHASSIS_DRIVER_BUILDING_LIBRARY + #define CHASSIS_DRIVER_PUBLIC CHASSIS_DRIVER_EXPORT + #else + #define CHASSIS_DRIVER_PUBLIC CHASSIS_DRIVER_IMPORT + #endif + #define CHASSIS_DRIVER_PUBLIC_TYPE CHASSIS_DRIVER_PUBLIC + #define CHASSIS_DRIVER_LOCAL +#else + #define CHASSIS_DRIVER_EXPORT __attribute__ ((visibility("default"))) + #define CHASSIS_DRIVER_IMPORT + #if __GNUC__ >= 4 + #define CHASSIS_DRIVER_PUBLIC __attribute__ ((visibility("default"))) + #define CHASSIS_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) + #else + #define CHASSIS_DRIVER_PUBLIC + #define CHASSIS_DRIVER_LOCAL + #endif + #define CHASSIS_DRIVER_PUBLIC_TYPE +#endif + +#endif // CHASSIS_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/roboteq_driver/include/debug_printer/debug_printer.hpp b/chassis_driver/include/debug_printer/debug_printer.hpp similarity index 100% rename from roboteq_driver/include/debug_printer/debug_printer.hpp rename to chassis_driver/include/debug_printer/debug_printer.hpp diff --git a/roboteq_driver/package.xml b/chassis_driver/package.xml similarity index 96% rename from roboteq_driver/package.xml rename to chassis_driver/package.xml index 9cbbc1a..1e154c6 100644 --- a/roboteq_driver/package.xml +++ b/chassis_driver/package.xml @@ -1,7 +1,7 @@ - roboteq_driver + chassis_driver 0.0.0 TODO: Package description kazuma diff --git a/roboteq_driver/src/roboteq_driver_node.cpp b/chassis_driver/src/chassis_driver_node.cpp similarity index 52% rename from roboteq_driver/src/roboteq_driver_node.cpp rename to chassis_driver/src/chassis_driver_node.cpp index b81fb4f..3d0c3c7 100644 --- a/roboteq_driver/src/roboteq_driver_node.cpp +++ b/chassis_driver/src/chassis_driver_node.cpp @@ -1,4 +1,4 @@ -#include "roboteq_driver/roboteq_driver_node.hpp" +#include "chassis_driver/chassis_driver_node.hpp" #include "utilities/data_utils.hpp" #include "utilities/utils.hpp" @@ -7,93 +7,125 @@ using namespace utils; -namespace roboteq_driver{ +namespace chassis_driver{ -RoboteqDriver::RoboteqDriver(const rclcpp::NodeOptions& options) : RoboteqDriver("", options) {} +ChassisDriver::ChassisDriver(const rclcpp::NodeOptions& options) : ChassisDriver("", options) {} -RoboteqDriver::RoboteqDriver(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("roboteq_driver_node", name_space, options), +ChassisDriver::ChassisDriver(const std::string& name_space, const rclcpp::NodeOptions& options) +: rclcpp::Node("chassis_driver_node", name_space, options), interval_ms(get_parameter("interval_ms").as_int()), wheel_radius(get_parameter("wheel_radius").as_double()), tread(get_parameter("tread").as_double()), wheelbase(get_parameter("wheelbase").as_double()), rotate_ratio(1.0 / get_parameter("reduction_ratio").as_double()), is_reverse_left(get_parameter("reverse_left_flag").as_bool()), -is_reverse_right(get_parameter("reverse_right_flag").as_bool()) +is_reverse_right(get_parameter("reverse_right_flag").as_bool()), + +linear_limit(DBL_MAX, +get_parameter("linear_max.vel").as_double(), +get_parameter("linear_max.acc").as_double()), + +angular_limit(DBL_MAX, +get_parameter("angular_max.vel").as_double(), +get_parameter("angular_max.acc").as_double()) { _subscription_vel = this->create_subscription( "cmd_vel", _qos, - std::bind(&RoboteqDriver::_subscriber_callback_vel, this, std::placeholders::_1) + std::bind(&ChassisDriver::_subscriber_callback_vel, this, std::placeholders::_1) ); _subscription_stop = this->create_subscription( "stop", _qos, - std::bind(&RoboteqDriver::_subscriber_callback_stop, this, std::placeholders::_1) + std::bind(&ChassisDriver::_subscriber_callback_stop, this, std::placeholders::_1) ); _subscription_restart = this->create_subscription( "restart", _qos, - std::bind(&RoboteqDriver::_subscriber_callback_restart, this, std::placeholders::_1) + std::bind(&ChassisDriver::_subscriber_callback_restart, this, std::placeholders::_1) ); _subscription_rpm = this->create_subscription( "can_rx_711", _qos, - std::bind(&RoboteqDriver::_subscriber_callback_rpm, this, std::placeholders::_1) + std::bind(&ChassisDriver::_subscriber_callback_rpm, this, std::placeholders::_1) ); _subscription_emergency = this->create_subscription( "can_rx_712", _qos, - std::bind(&RoboteqDriver::_subscriber_callback_emergency, this, std::placeholders::_1) + std::bind(&ChassisDriver::_subscriber_callback_emergency, this, std::placeholders::_1) ); publisher_can = this->create_publisher("can_tx", _qos); publisher_steer = this->create_publisher("cybergear/pos", _qos); + publisher_ref_vel = this->create_publisher("ref_vel", _qos); _pub_timer = this->create_wall_timer( std::chrono::milliseconds(interval_ms), [this] { _publisher_callback(); } ); + + linear_planner.limit(linear_limit); + angular_planner.limit(angular_limit); } -void RoboteqDriver::_subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg){ +void ChassisDriver::_subscriber_callback_vel(const geometry_msgs::msg::Twist::SharedPtr msg){ if(mode == Mode::stop) return; mode = Mode::cmd; - vel = msg; + const double linear_vel = constrain(msg->linear.x, -linear_limit.vel, linear_limit.vel); + const double angular_vel = constrain(msg->angular.z, -angular_limit.vel, angular_limit.vel); + linear_planner.vel(linear_vel); + angular_planner.vel(angular_vel); } -void RoboteqDriver::_publisher_callback(){ +void ChassisDriver::_publisher_callback(){ + linear_planner.cycle(); + angular_planner.cycle(); + if(mode == Mode::stop || mode == Mode::stay){ this->send_rpm(0.0, 0.0); return; } - if(vel == nullptr) return; - send_rpm(vel->linear.x, vel->angular.z); + + const double linear_vel = linear_planner.vel(); + const double angular_vel = angular_planner.vel(); + send_rpm(linear_vel, angular_vel); + + // デバッグ用にロボットの目標速度指令値を出版 + auto msg_ref_vel = std::make_shared(); + 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(); - if(vel->linear.x == 0.0){ + if(linear_vel == 0.0){ msg_steer->data = 0.0; } else{ - msg_steer->data = std::asin((wheelbase*vel->angular.z) / vel->linear.x); + msg_steer->data = std::asin((wheelbase*angular_vel) / linear_vel); if(std::isnan(msg_steer->data)) msg_steer->data = 0.0; } publisher_steer->publish(*msg_steer); } -void RoboteqDriver::_subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg){ +void ChassisDriver::_subscriber_callback_stop(const std_msgs::msg::Empty::SharedPtr msg){ mode = Mode::stop; RCLCPP_INFO(this->get_logger(), "停止"); } -void RoboteqDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg){ +void ChassisDriver::_subscriber_callback_restart(const std_msgs::msg::Empty::SharedPtr msg){ mode = Mode::stay; + + velplanner::Physics_t physics_zero(0.0, 0.0, 0.0); + linear_planner.current(physics_zero); + angular_planner.current(physics_zero); RCLCPP_INFO(this->get_logger(), "再稼働"); } -void RoboteqDriver::_subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ +void ChassisDriver::_subscriber_callback_rpm(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ } -void RoboteqDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ +void ChassisDriver::_subscriber_callback_emergency(const socketcan_interface_msg::msg::SocketcanIF::SharedPtr msg){ uint8_t _candata[8]; for(int i=0; icandlc; i++) _candata[i] = msg->candata[i]; @@ -103,10 +135,12 @@ void RoboteqDriver::_subscriber_callback_emergency(const socketcan_interface_msg } } -void RoboteqDriver::send_rpm(const double linear_vel, const double angular_vel){ +void ChassisDriver::send_rpm(const double linear_vel, const double angular_vel){ // 駆動輪の目標角速度 - const double left_vel = (1.0 / wheel_radius) * linear_vel + (tread / wheel_radius*2.0) * angular_vel; - const double right_vel = (1.0 / wheel_radius) * linear_vel - (tread / wheel_radius*2.0) * angular_vel; + // const double left_vel = (1.0 / wheel_radius) * linear_vel + (tread / wheel_radius*2.0) * angular_vel; + // const double right_vel = (1.0 / wheel_radius) * linear_vel - (tread / wheel_radius*2.0) * angular_vel; + const double left_vel = (-tread*angular_vel + 2.0*linear_vel) / (2.0*wheel_radius); + const double right_vel = (tread*angular_vel + 2.0*linear_vel) / (2.0*wheel_radius); // rad/s -> rpm & 回転方向制御 const double left_rpm = (is_reverse_left ? -1 : 1) * (left_vel*30.0 / d_pi) * rotate_ratio; @@ -127,4 +161,4 @@ void RoboteqDriver::send_rpm(const double linear_vel, const double angular_vel){ } -} // namespace roboteq_driver +} // namespace chassis_driver diff --git a/roboteq_driver/src/debug_printer.cpp b/chassis_driver/src/debug_printer.cpp similarity index 100% rename from roboteq_driver/src/debug_printer.cpp rename to chassis_driver/src/debug_printer.cpp diff --git a/gnssnav/config/course_data/0425_test_curve.csv b/gnssnav/config/course_data/0425_test_curve.csv deleted file mode 100644 index 7580d11..0000000 --- a/gnssnav/config/course_data/0425_test_curve.csv +++ /dev/null @@ -1,122 +0,0 @@ -36.11337895971256,139.98149804279524 -36.11337895970354,139.98149804325507 -36.1133789598516,139.98149804356882 -36.11337895967556,139.98149804383408 -36.11337895969062,139.98149804460746 -36.11337895971175,139.98149804512073 -36.11337895970505,139.98149808975828 -36.1133789583681,139.98149988409818 -36.11337895440175,139.98150228640503 -36.113378951266256,139.98150482295816 -36.11337894417066,139.98150721324296 -36.11337893916725,139.98150943659246 -36.11337900734758,139.98151196040214 -36.11337961874708,139.98151421637064 -36.11338034359793,139.98151657253018 -36.1133811278028,139.9815190730437 -36.11338182208984,139.9815212884596 -36.11338251282652,139.98152351104054 -36.11338329345881,139.9815260065082 -36.11338412429407,139.98152866473697 -36.113384946174,139.9815312835953 -36.11338651428926,139.98153547183017 -36.113386757450186,139.98153599828834 -36.11338687200077,139.98153624759954 -36.11338692797396,139.98153636736083 -36.113387762059,139.981538188775 -36.1133886461506,139.9815401382634 -36.113389418167245,139.98154180700598 -36.1133902555935,139.98154363802 -36.113391258875694,139.981545845704 -36.113392251893536,139.98154803732874 -36.11339308155798,139.9815498572791 -36.113394125308766,139.98155186957834 -36.11339511763776,139.9815537432804 -36.11339610495666,139.98155560185492 -36.11339712096327,139.98155746896077 -36.113398334647,139.9815591174435 -36.11339967934353,139.98156084440794 -36.11340093212608,139.98156244505222 -36.11340224546592,139.9815639857227 -36.113403716426056,139.98156551575352 -36.11340509899092,139.98156695659077 -36.11340664486342,139.9815686636133 -36.11340779742771,139.98157055839937 -36.11340879911391,139.98157226907318 -36.11340980320782,139.98157429432797 -36.11341082081551,139.98157650657433 -36.1134114732822,139.98157891288318 -36.11341205509418,139.98158165692132 -36.11341257265451,139.98158444332128 -36.11341263000008,139.9815871135981 -36.113412668096544,139.98158948728812 -36.11341270332047,139.98159172798452 -36.11341274061155,139.98159428000838 -36.11341277068592,139.98159633706038 -36.11341280461667,139.98159857180636 -36.11341283563289,139.98160081370798 -36.11341275704557,139.9816030648451 -36.1134126548434,139.9816052940308 -36.11341225755514,139.98160763381264 -36.11341169158621,139.9816099154337 -36.1134111658785,139.98161204362145 -36.11341063080702,139.9816141745491 -36.11340998767263,139.98161645418745 -36.113408811874166,139.98161834892693 -36.11340712888967,139.9816195129682 -36.11340534975077,139.98162043532972 -36.11340395518908,139.98162204012226 -36.1134029649879,139.98162365979445 -36.1134028759135,139.9816237939874 -36.11340286642539,139.9816238213248 -36.113402851860165,139.9816238457751 -36.11340284803692,139.9816238520164 -36.11340284114758,139.9816238632315 -36.11340283653474,139.98162387070795 -36.11340282890893,139.981623883036 -36.11340281857343,139.98162389971955 -36.1134028044574,139.98162392244222 -36.11340278938674,139.9816239417795 -36.113402776278384,139.9816239629613 -36.11340276062512,139.9816239934941 -36.11340274144799,139.9816240260774 -36.11340272170579,139.9816240592186 -36.113402702465145,139.98162409148978 -36.11340267599543,139.98162413539237 -36.11340264386305,139.98162417761185 -36.11340263264616,139.98162420179517 -36.11340263204907,139.98162420262335 -36.11340251663246,139.98162438558904 -36.11340157740117,139.98162588612584 -36.11340046103562,139.9816276517331 -36.1133993565244,139.98162940857887 -36.11339831726077,139.98163105157158 -36.11339704809174,139.981633038278 -36.11339585133465,139.9816349072209 -36.11339450409464,139.98163701479024 -36.11339331386337,139.98163907069537 -36.11339238994743,139.98164150273826 -36.11339157982366,139.98164366575284 -36.11339076836122,139.98164582986712 -36.11338999850729,139.98164785603058 -36.11338923408508,139.98164985828376 -36.11338841662295,139.9816520285095 -36.11338759204311,139.98165417510612 -36.11338686603398,139.9816560636553 -36.11338613671166,139.98165794648057 -36.11338535987216,139.9816599649832 -36.11338457200913,139.98166213267325 -36.11338391065764,139.9816642152636 -36.113383228661554,139.9816664389535 -36.11338271752812,139.98166874172207 -36.11338226811574,139.98167105255348 -36.11338190657376,139.9816732576015 -36.11338157761763,139.98167558320176 -36.11338129932546,139.98167780511895 -36.11338103565844,139.9816803176816 -36.11338076725525,139.98168281803638 -36.11338048226606,139.98168547851512 -36.113380235145534,139.98168775394285 -36.1133802327825,139.98168777404538 -36.113380231370094,139.9816877869361 -36.11338023058091,139.98168779414314 diff --git a/gnssnav/config/course_data/0425_test_straight.csv b/gnssnav/config/course_data/0425_test_straight.csv deleted file mode 100644 index f12e76f..0000000 --- a/gnssnav/config/course_data/0425_test_straight.csv +++ /dev/null @@ -1,11 +0,0 @@ -36.11339585571819,139.9815010597697 -36.113395855685,139.98150106025167 -36.11339585555729,139.98150106310823 -36.113395855316384,139.9815010660582 -36.113395855112365,139.9815010693639 -36.11339585311715,139.98150109993202 -36.113399053075504,139.98156892854155 -36.11339616027104,139.98163969302706 -36.1133968899045,139.9817134639211 -36.11339715833002,139.9817872720855 -36.11338383945336,139.98185755792508 diff --git a/gnssnav/config/course_data/circle_shinnara.csv b/gnssnav/config/course_data/circle_shinnara.csv deleted file mode 100644 index 2ab9152..0000000 --- a/gnssnav/config/course_data/circle_shinnara.csv +++ /dev/null @@ -1,5 +0,0 @@ -35.662472,140.012912 -35.662308,140.012851 -35.662155,140.012949 -35.662105,140.013151 -35.662184,140.013337 \ No newline at end of file diff --git a/gnssnav/config/course_data/cit-parking-output.csv b/gnssnav/config/course_data/cit-parking-output.csv deleted file mode 100644 index c4cadde..0000000 --- a/gnssnav/config/course_data/cit-parking-output.csv +++ /dev/null @@ -1,12 +0,0 @@ -35.687979999999996,140.02208000000002 -35.68795,140.02216 -35.68793,140.02222 -35.687909999999995,140.02228 -35.68788,140.02231 -35.68785,140.02229 -35.68781,140.02227 -35.68779,140.02226000000002 -35.68778,140.02221 -35.6878,140.02215 -35.68782,140.02209 -35.68784,140.02203 diff --git a/gnssnav/config/course_data/cit-start-BackGate-output.csv b/gnssnav/config/course_data/cit-start-BackGate-output.csv deleted file mode 100644 index ad1eb3b..0000000 --- a/gnssnav/config/course_data/cit-start-BackGate-output.csv +++ /dev/null @@ -1,12 +0,0 @@ -35.68951,140.02164 -35.689479999999996,140.02154 -35.68946,140.02141 -35.6895,140.02125 -35.68955,140.02107 -35.6896,140.0209 -35.68965,140.02074 -35.6897,140.02057 -35.68971,140.02045 -35.68967,140.02036999999999 -35.689609999999995,140.02033 -35.68953,140.0203 diff --git a/gnssnav/config/course_data/cit-start-FrontGate-output.csv b/gnssnav/config/course_data/cit-start-FrontGate-output.csv deleted file mode 100644 index 730d83a..0000000 --- a/gnssnav/config/course_data/cit-start-FrontGate-output.csv +++ /dev/null @@ -1,15 +0,0 @@ -35.68952,140.02029 -35.68962,140.02033 -35.689679999999996,140.02039 -35.68971,140.02047 -35.68969,140.02057 -35.68967,140.02066000000002 -35.689640000000004,140.02076 -35.689609999999995,140.02086 -35.68959,140.02095 -35.68956,140.02105 -35.68953,140.02116 -35.6895,140.02127 -35.68946,140.02139 -35.68949,140.0215 -35.68952,140.02161 diff --git a/gnssnav/config/course_data/shihou_gnssnav.csv b/gnssnav/config/course_data/shihou_gnssnav.csv new file mode 100644 index 0000000..06d154d --- /dev/null +++ b/gnssnav/config/course_data/shihou_gnssnav.csv @@ -0,0 +1,294 @@ +36.11339126569408,139.98145533892483 +36.113391389305185,139.98145543546926 +36.11339144955584,139.9814553508664 +36.11339161498881,139.98145520471292 +36.11339170879556,139.98145504497592 +36.11339172743383,139.9814548015158 +36.11339177353364,139.9814546532021 +36.113391872050954,139.9814545370102 +36.113391711579006,139.98145427322928 +36.11339168107373,139.9814539235414 +36.11339164775816,139.98145371598252 +36.11339163153462,139.98145351969782 +36.11339164675337,139.98145347229678 +36.1133918733733,139.98145355543934 +36.11339185604933,139.98145342508425 +36.113392010349536,139.9814533522657 +36.11339205987316,139.98145325237184 +36.11339207496862,139.981453085517 +36.11339212816741,139.98145306679046 +36.11339218228707,139.98145305506443 +36.11339220314158,139.9814530282325 +36.11339233711361,139.98145298464485 +36.113392345482595,139.98145288089313 +36.11339239789637,139.98145277489033 +36.11339242161487,139.9814527736696 +36.11339244929931,139.98145276382158 +36.1133926764911,139.981452642983 +36.113392601935026,139.9814523059413 +36.11339235346438,139.9814526734891 +36.11339236431428,139.98145265899603 +36.113392320089254,139.98145268988407 +36.11339347699987,139.98145297708217 +36.113393701147906,139.98145306150212 +36.113393781409734,139.98145304107285 +36.11339368935786,139.9814528415238 +36.11339385126579,139.98145364542827 +36.11339392041104,139.98145772170403 +36.11339438168591,139.98146318397576 +36.113395240883214,139.98147049898333 +36.11339606278752,139.9814787059387 +36.11339693405276,139.9814876740646 +36.11339773605029,139.98149688287037 +36.113398542650266,139.98150619183332 +36.11339923432669,139.9815154773733 +36.11339991601097,139.9815248039219 +36.11340069782403,139.9815339734155 +36.113401243269465,139.98154290503757 +36.113401835412134,139.98155161011454 +36.11340361833585,139.9815898670199 +36.113406260973875,139.98162992517766 +36.11340778917736,139.98166773274048 +36.11340838439558,139.9816760521848 +36.11340898588299,139.9816843940894 +36.113409689391545,139.98169284911003 +36.11341030839775,139.98170111589636 +36.113410983629585,139.98170948786287 +36.113411347389736,139.98171796479232 +36.113411662095004,139.98172638056684 +36.11341196316747,139.98173485347772 +36.11341236543871,139.9817433313144 +36.113412640749836,139.98175158389722 +36.11341299875361,139.98176014973757 +36.113412949416734,139.98176854037254 +36.11341290623598,139.98177621408135 +36.113412624456046,139.98178393001479 +36.11341212335104,139.98179152432962 +36.11341069876311,139.98179916756732 +36.113409422925095,139.98180721120934 +36.11340792478426,139.98181558999119 +36.113406678506664,139.98182385978708 +36.11340465906691,139.98183176730652 +36.11340214686415,139.98183928850676 +36.113389404940826,139.9818725539318 +36.11338608069805,139.98188002815556 +36.11336553387295,139.98191310686457 +36.11333911614271,139.98194079327263 +36.113333640257075,139.98194539261297 +36.113327819993174,139.98194971382463 +36.11332170451706,139.98195343784008 +36.113315406602865,139.9819571059424 +36.11330898815079,139.98196059097205 +36.1133023149956,139.98196377569923 +36.11329560176037,139.98196682695993 +36.113288776684705,139.98196918622926 +36.11328200645467,139.9819708629925 +36.11327511286211,139.9819722467379 +36.11326818233542,139.981973341384 +36.11326122828443,139.98197399921307 +36.11325429264567,139.98197479812643 +36.11324725476825,139.98197520665985 +36.11324035718708,139.98197587343722 +36.11321112632988,139.9819788378175 +36.113176540128656,139.9819827334085 +36.113142423416726,139.9819863337849 +36.113135498333115,139.98198697633157 +36.113128642022446,139.98198774058596 +36.113121810675814,139.98198847542383 +36.113114837141474,139.98198932779738 +36.11310785924482,139.98198990825796 +36.11310105064378,139.9819905782552 +36.113094045196995,139.98199143603645 +36.11308715565227,139.98199211813537 +36.1130804224739,139.98199266421366 +36.11307355657242,139.98199349211788 +36.11306679646136,139.9819941342116 +36.11305981814803,139.98199477010405 +36.1130530015825,139.98199545880436 +36.113046160167514,139.98199605313616 +36.113039400499794,139.9819966971223 +36.1130129195168,139.98199936463118 +36.1129930106858,139.9820008612958 +36.11297312015705,139.98200306116712 +36.11296985798597,139.98200312518065 +36.112966487416195,139.98200288377203 +36.11296210136603,139.98200273001518 +36.11295670566066,139.98200345068375 +36.11295151160668,139.98200448134585 +36.112947224793054,139.98200500241012 +36.11294345085981,139.9820054804326 +36.11294054385965,139.98200592091106 +36.11293597121969,139.98200665247023 +36.11293025106822,139.98200746128828 +36.11292353800004,139.9820080640753 +36.112916596120165,139.98200876061523 +36.11290953201723,139.9820090755675 +36.112904258508806,139.9820093472262 +36.11289992497781,139.98200979356423 +36.11287288956992,139.9820078166817 +36.112839930067125,139.98200095658453 +36.11280722866488,139.98198554389245 +36.11280087764657,139.98198138432974 +36.112794904532244,139.9819769659073 +36.11278914366623,139.98197198549502 +36.112784624685176,139.98196733554488 +36.11278121257094,139.98196369679596 +36.11277839677,139.98196034401542 +36.112774950797544,139.9819557988587 +36.112770698929076,139.98195121992308 +36.11276607115095,139.98194579074777 +36.11276146946253,139.98193952453516 +36.11275689413709,139.98193318977573 +36.112752686956505,139.98192654623773 +36.11274856134091,139.9819194662533 +36.112744520027256,139.98191214614707 +36.11274082489782,139.9819046655774 +36.11272770581139,139.98186751002402 +36.112726292845565,139.9818611137689 +36.1127208732819,139.98183057384185 +36.11271857317226,139.9817868354694 +36.112719290929135,139.98177811911074 +36.11272032495842,139.98176976107564 +36.11272172275005,139.981761327357 +36.112723458285345,139.98175299979215 +36.11272500408954,139.981744723632 +36.11272641409833,139.98173639234423 +36.11272811348983,139.9817281657069 +36.11273049840684,139.9817201078991 +36.11273301200865,139.98171201360478 +36.112735614603366,139.98170415280038 +36.112737410946245,139.98169604223764 +36.112739719415664,139.9816881989788 +36.112742025016864,139.98168027010337 +36.11274427151436,139.98167234314337 +36.11274612586329,139.98166438515972 +36.112747869837456,139.98165613294546 +36.11274982514857,139.98164809881771 +36.11275208873602,139.981640165864 +36.11275421814108,139.98163234568884 +36.11275578373272,139.98162403918585 +36.11275763610959,139.9816160361132 +36.11276003074839,139.98160798449433 +36.11276221734405,139.981600106537 +36.11276438061994,139.9815923521059 +36.11277350296484,139.98155498082153 +36.11277572847529,139.9815468856168 +36.11278648877005,139.98150427821517 +36.112796764338334,139.98146606261525 +36.112798344561554,139.98145790884476 +36.11280020558192,139.98144967958703 +36.11280223754481,139.98144157038624 +36.11280444016419,139.981433639422 +36.112806497832196,139.9814256357122 +36.112808773483366,139.9814176508794 +36.1128106540866,139.98140951755389 +36.11281261321664,139.98140134098603 +36.112814367591795,139.98139280642212 +36.112816569252665,139.98138433966605 +36.11281878767204,139.98137596674712 +36.112821150415805,139.98136759942003 +36.112823572241744,139.98135958557305 +36.11282544985386,139.98135146175818 +36.1128273709626,139.98134332871192 +36.112829213953745,139.9813353919865 +36.112831133581814,139.98132716236995 +36.11283329338839,139.98131899233266 +36.112835538957825,139.98131080165103 +36.11283793006053,139.9813026724001 +36.11284009764755,139.9812946645546 +36.11284215160161,139.98128657364362 +36.112843874416384,139.98127835457154 +36.11284568802141,139.98126998342343 +36.11285485664203,139.9812339003799 +36.11286685301955,139.9811960408838 +36.11288508989191,139.98115904041208 +36.11288921610468,139.98115207366652 +36.11289341904385,139.98114523751053 +36.11289821543794,139.9811392607838 +36.11290315566799,139.9811335722599 +36.112908128171625,139.98112808264435 +36.112913312323236,139.98112271111077 +36.112918898012836,139.98111727883492 +36.112924160941546,139.98111215407187 +36.112930108900784,139.9811075266876 +36.112936165680985,139.98110394347034 +36.11294269399715,139.9811004781908 +36.11294909885099,139.9810973259901 +36.112955470554866,139.98109404628354 +36.11296187077614,139.981090874791 +36.11296829153141,139.981087887612 +36.112974676647156,139.98108516925885 +36.11298134706869,139.981083075802 +36.112987958805824,139.98108086971027 +36.11299461475846,139.98107937464937 +36.11300155611578,139.98107839245185 +36.11300855799703,139.98107811594699 +36.11301549937223,139.98107802575313 +36.11302234634715,139.9810771993462 +36.11302887235798,139.98107636381934 +36.1130555797088,139.98107514078856 +36.11306092715415,139.9810748406646 +36.11308128767401,139.98107147636983 +36.113102537434905,139.98106782907422 +36.113105468533405,139.98106746799917 +36.11310989323953,139.9810666959552 +36.11311520587198,139.98106578257395 +36.11312157453977,139.98106492579745 +36.11312783645265,139.9810641235425 +36.11313298105043,139.98106349270412 +36.11313687576671,139.98106295177126 +36.11314011906337,139.9810627377845 +36.11314417077804,139.9810627968929 +36.11314957675155,139.98106243607907 +36.11315606232163,139.98106188249056 +36.113162742865384,139.9810613842867 +36.113169987343845,139.98106133159433 +36.11317712192717,139.98106077885024 +36.11318336340363,139.98105985474183 +36.11318955892648,139.98105937997607 +36.113195964404404,139.9810590816995 +36.11320246827796,139.98105985758326 +36.1132092783055,139.98106077521277 +36.11321648780013,139.98106158609198 +36.113223485765914,139.98106233661508 +36.11325474212801,139.9810704413882 +36.113287921572265,139.9810879504468 +36.1133168213695,139.98111100312005 +36.11332187531526,139.9811167357324 +36.113327052635285,139.98112238683277 +36.11333219625374,139.98112845432323 +36.113337140481164,139.98113450170675 +36.1133416971893,139.9811405866063 +36.113345621459324,139.98114705046342 +36.11334912115809,139.98115423701088 +36.11335248366635,139.98116126263278 +36.11335560805999,139.98116870499464 +36.11335907142947,139.98117598163955 +36.11336206814401,139.98118382108254 +36.11336484178042,139.9811913763815 +36.11336718235388,139.98119935156325 +36.11337388637313,139.9812365620715 +36.113376221081246,139.9812805925687 +36.11337898983971,139.98132144288843 +36.11337962005235,139.98133012253393 +36.1133800871185,139.9813386874925 +36.11338073941308,139.9813474407054 +36.113381518518366,139.9813557826802 +36.11338205324506,139.98136397618953 +36.11338269350876,139.9813724447564 +36.11338330577966,139.98138115113662 +36.11338404458696,139.98138971006253 +36.11338443880956,139.98139790256434 +36.11338500364066,139.9814049945214 +36.11338513848484,139.98140887779056 +36.11338521691767,139.98141142517645 +36.11338503653369,139.98141151353576 +36.113384804808405,139.9814112542428 +36.11338474437926,139.98141115390686 +36.11338475674041,139.98141104088108 +36.11338460777009,139.98141083923568 +36.11338451335436,139.98141071432005 +36.113384406693335,139.98141058066113 +36.113384339645066,139.9814104160584 +36.11338418196263,139.98141019240708 +36.11338411486075,139.9814102165778 diff --git a/gnssnav/config/course_data/shihou_limit_area.csv b/gnssnav/config/course_data/shihou_limit_area.csv deleted file mode 100644 index d6fbb9d..0000000 --- a/gnssnav/config/course_data/shihou_limit_area.csv +++ /dev/null @@ -1,18 +0,0 @@ -36.1127084 ,139.9817579 -36.1127245 ,139.9816949 -36.1127413 ,139.9816258 -36.1127564 ,139.9815682 -36.1127711 ,139.9815098 -36.1127819 ,139.9814669 -36.1127976 ,139.9814086 -36.1128095 ,139.981359 -36.1128252 ,139.9813006 -36.112838 ,139.981243 -36.1128583 ,139.9811807 -36.1128892 ,139.9811331 -36.1129293 ,139.9810975 -36.1129802 ,139.9810761 -36.1130538 ,139.9810676 -36.1131075 ,139.9810629 -36.1131633 ,139.9810562 -36.1132126 ,139.9810542 \ No newline at end of file diff --git a/gnssnav/config/course_data/shinnara_gym_cource.csv b/gnssnav/config/course_data/shinnara_gym_cource.csv deleted file mode 100755 index 16cc299..0000000 --- a/gnssnav/config/course_data/shinnara_gym_cource.csv +++ /dev/null @@ -1,519 +0,0 @@ -35.66091423,140.0150152 -35.6609141,140.0150156 -35.66091395,140.0150158 -35.66091378,140.015016 -35.66091361,140.0150163 -35.66091335,140.0150165 -35.66091318,140.0150167 -35.66091303,140.015017 -35.66091288,140.0150172 -35.6609127,140.0150174 -35.66091255,140.0150176 -35.66091241,140.0150177 -35.66091228,140.0150179 -35.66091215,140.0150182 -35.66091203,140.0150183 -35.6609119,140.0150185 -35.66091176,140.0150187 -35.66091155,140.0150188 -35.66091139,140.015019 -35.66091122,140.0150192 -35.66091105,140.0150195 -35.66091074,140.0150197 -35.66091057,140.0150199 -35.66091042,140.0150201 -35.66091027,140.0150203 -35.66091,140.0150205 -35.66090988,140.0150206 -35.66090975,140.0150208 -35.66090964,140.0150209 -35.66090947,140.0150211 -35.66090937,140.0150212 -35.66090927,140.0150214 -35.66090916,140.0150215 -35.66090902,140.0150216 -35.66090889,140.0150218 -35.66090875,140.015022 -35.6609086,140.0150222 -35.66090823,140.0150223 -35.66090807,140.0150225 -35.6609079,140.0150228 -35.66090774,140.015023 -35.66090744,140.0150231 -35.66090727,140.0150233 -35.6609071,140.0150235 -35.66090694,140.0150238 -35.66090655,140.0150239 -35.66090642,140.0150241 -35.66090629,140.0150243 -35.66090617,140.0150245 -35.66090603,140.0150243 -35.66090592,140.0150245 -35.66090582,140.0150247 -35.66090572,140.0150248 -35.66090534,140.0150248 -35.66090524,140.015025 -35.66090515,140.0150252 -35.66090507,140.0150253 -35.66090489,140.0150255 -35.6609048,140.0150257 -35.66090472,140.0150258 -35.66090462,140.015026 -35.66090436,140.015026 -35.66090425,140.0150262 -35.66090413,140.0150264 -35.660904,140.0150266 -35.66090361,140.0150268 -35.66090345,140.0150271 -35.66090326,140.0150274 -35.66090307,140.0150277 -35.66090276,140.0150278 -35.66090257,140.015028 -35.6609024,140.0150283 -35.66090224,140.0150286 -35.66090183,140.0150287 -35.66090167,140.015029 -35.66090151,140.0150293 -35.66090136,140.0150295 -35.66090111,140.0150296 -35.66090097,140.0150299 -35.66090084,140.0150301 -35.66090072,140.0150303 -35.66090034,140.0150304 -35.6609002,140.0150307 -35.66090004,140.015031 -35.66089987,140.0150313 -35.66089961,140.0150314 -35.66089944,140.0150317 -35.66089929,140.015032 -35.66089914,140.0150322 -35.66089881,140.0150323 -35.66089868,140.0150326 -35.66089854,140.0150328 -35.6608984,140.0150331 -35.66089818,140.0150332 -35.66089803,140.0150335 -35.66089786,140.0150338 -35.66089768,140.0150341 -35.6608974,140.0150344 -35.66089719,140.0150347 -35.660897,140.0150351 -35.66089681,140.0150354 -35.66089658,140.0150355 -35.66089641,140.0150358 -35.66089627,140.0150361 -35.66089612,140.0150364 -35.66089583,140.0150365 -35.6608957,140.0150368 -35.66089557,140.015037 -35.66089545,140.0150373 -35.66089523,140.0150373 -35.66089513,140.0150375 -35.66089502,140.0150377 -35.6608949,140.015038 -35.6608948,140.0150381 -35.66089465,140.0150384 -35.66089451,140.0150387 -35.66089437,140.015039 -35.66089396,140.0150392 -35.66089381,140.0150394 -35.66089366,140.0150397 -35.6608935,140.01504 -35.66089327,140.0150402 -35.66089312,140.0150405 -35.66089298,140.0150408 -35.66089285,140.015041 -35.66089268,140.0150412 -35.66089255,140.0150414 -35.66089242,140.0150417 -35.66089228,140.0150419 -35.66089208,140.0150421 -35.66089194,140.0150423 -35.6608918,140.0150426 -35.66089165,140.0150429 -35.66089142,140.015043 -35.66089127,140.0150433 -35.66089111,140.0150435 -35.66089095,140.0150438 -35.6608908,140.015044 -35.66088421,140.0150534 -35.66088397,140.0150536 -35.66088384,140.0150538 -35.66088371,140.015054 -35.66088359,140.0150542 -35.66088335,140.0150543 -35.66088322,140.0150545 -35.66088309,140.0150548 -35.66088295,140.015055 -35.66088265,140.0150551 -35.66088251,140.0150553 -35.66088236,140.0150555 -35.66088221,140.0150557 -35.66088195,140.0150559 -35.6608818,140.0150562 -35.66087585,140.0150633 -35.66087579,140.0150634 -35.66087573,140.0150635 -35.66087552,140.0150634 -35.66087546,140.0150635 -35.66087541,140.0150636 -35.66087537,140.0150637 -35.66087524,140.0150637 -35.66087521,140.0150637 -35.6608752,140.0150637 -35.6608752,140.0150638 -35.660875,140.0150639 -35.66087501,140.0150639 -35.66087503,140.015064 -35.66087504,140.015064 -35.66087467,140.0150638 -35.66087468,140.0150638 -35.66087469,140.0150638 -35.6608746,140.0150638 -35.66087461,140.0150638 -35.66087462,140.0150638 -35.66087463,140.0150638 -35.66087458,140.0150638 -35.66087459,140.0150638 -35.6608746,140.0150638 -35.66087461,140.0150638 -35.66087453,140.0150638 -35.66087454,140.0150638 -35.66087455,140.0150638 -35.66087456,140.0150638 -35.66087442,140.0150638 -35.66087443,140.0150638 -35.66087443,140.0150638 -35.66087443,140.0150638 -35.66087437,140.0150638 -35.66087435,140.0150638 -35.66087434,140.0150639 -35.66087432,140.0150639 -35.66087422,140.0150638 -35.66087422,140.0150638 -35.66087423,140.0150638 -35.66087424,140.0150638 -35.66087421,140.0150638 -35.66087423,140.0150638 -35.66087424,140.0150638 -35.66087424,140.0150638 -35.66087413,140.0150638 -35.66087414,140.0150638 -35.66087415,140.0150638 -35.66087416,140.0150638 -35.66087417,140.0150636 -35.66087418,140.0150636 -35.66087419,140.0150636 -35.6608742,140.0150636 -35.660874,140.0150636 -35.660874,140.0150636 -35.66087401,140.0150637 -35.66087402,140.0150637 -35.66087386,140.0150637 -35.66087386,140.0150637 -35.66087387,140.0150637 -35.66087387,140.0150637 -35.66087377,140.0150637 -35.66087378,140.0150637 -35.66087378,140.0150637 -35.66087379,140.0150637 -35.66087371,140.0150637 -35.66087372,140.0150637 -35.66087372,140.0150637 -35.66087373,140.0150637 -35.66087371,140.0150637 -35.66087371,140.0150637 -35.66087372,140.0150637 -35.66087373,140.0150637 -35.66087361,140.0150637 -35.66087361,140.0150637 -35.66087362,140.0150637 -35.66087362,140.0150637 -35.66087345,140.0150636 -35.66087344,140.0150636 -35.66087344,140.0150637 -35.66087343,140.0150637 -35.66087329,140.0150636 -35.66087327,140.0150636 -35.66087325,140.0150637 -35.66087323,140.0150637 -35.66087315,140.0150636 -35.66087312,140.0150637 -35.66087309,140.0150638 -35.66087305,140.0150638 -35.66087301,140.0150638 -35.66087296,140.0150639 -35.6608729,140.015064 -35.66087283,140.0150641 -35.66087267,140.0150641 -35.66087259,140.0150643 -35.66087249,140.0150644 -35.66087239,140.0150646 -35.66087237,140.0150645 -35.66087226,140.0150646 -35.66087216,140.0150648 -35.66087204,140.015065 -35.66087186,140.0150652 -35.66087173,140.0150654 -35.6608716,140.0150656 -35.66087146,140.0150658 -35.66087131,140.0150659 -35.66087116,140.0150662 -35.660871,140.0150664 -35.66087084,140.0150667 -35.66087051,140.0150668 -35.66087033,140.015067 -35.66087015,140.0150673 -35.66086996,140.0150676 -35.66086982,140.0150677 -35.66086963,140.0150681 -35.66086945,140.0150684 -35.66086926,140.0150687 -35.66086891,140.015069 -35.66086871,140.0150694 -35.66086852,140.0150697 -35.66086833,140.0150701 -35.66086834,140.0150704 -35.66086817,140.0150707 -35.660868,140.015071 -35.66086784,140.0150714 -35.66086749,140.0150717 -35.66086733,140.0150721 -35.66086718,140.0150724 -35.66086702,140.0150727 -35.66086691,140.0150731 -35.66086677,140.0150734 -35.66086664,140.0150737 -35.6608665,140.015074 -35.6608663,140.0150743 -35.66086617,140.0150746 -35.66086604,140.0150749 -35.66086592,140.0150752 -35.66086612,140.0150755 -35.66086601,140.0150758 -35.66086591,140.0150761 -35.66086581,140.0150763 -35.66086544,140.0150764 -35.66086533,140.0150766 -35.66086523,140.0150768 -35.66086513,140.0150771 -35.66086503,140.0150774 -35.66086493,140.0150776 -35.66086484,140.0150778 -35.66086475,140.015078 -35.66086471,140.0150783 -35.66086463,140.0150785 -35.66086454,140.0150786 -35.66086446,140.0150788 -35.66086435,140.015079 -35.66086426,140.0150792 -35.66086418,140.0150793 -35.6608641,140.0150794 -35.66086404,140.0150797 -35.66086397,140.0150798 -35.66086389,140.01508 -35.66086381,140.0150801 -35.66086366,140.0150803 -35.66086357,140.0150804 -35.66086348,140.0150806 -35.66086338,140.0150807 -35.66086329,140.015081 -35.66086318,140.0150811 -35.66086307,140.0150813 -35.66086295,140.0150815 -35.66086289,140.0150818 -35.66086276,140.015082 -35.66086262,140.0150822 -35.66086249,140.0150825 -35.66085544,140.0150916 -35.66085525,140.0150918 -35.66085518,140.0150921 -35.66085499,140.0150922 -35.66085481,140.0150924 -35.66085464,140.0150925 -35.66085448,140.0150927 -35.66085431,140.0150929 -35.66085415,140.015093 -35.66085399,140.0150932 -35.6608538,140.0150934 -35.66085364,140.0150935 -35.66085348,140.0150937 -35.66085332,140.0150938 -35.66084875,140.0150993 -35.66084866,140.0150995 -35.66084851,140.0150997 -35.66084834,140.0150999 -35.66084817,140.0151001 -35.66084787,140.0151004 -35.66084768,140.0151007 -35.66084747,140.015101 -35.66084726,140.0151013 -35.66084709,140.0151016 -35.66084687,140.015102 -35.66084666,140.0151023 -35.66084645,140.0151027 -35.66084628,140.0151029 -35.66084607,140.0151033 -35.66084122,140.0151157 -35.66084107,140.015116 -35.66084092,140.0151164 -35.66084098,140.0151168 -35.66084084,140.0151171 -35.66084069,140.0151174 -35.66084054,140.0151177 -35.6608404,140.0151182 -35.66084025,140.0151185 -35.66084009,140.0151188 -35.66083994,140.0151191 -35.66083998,140.0151194 -35.66083982,140.0151197 -35.66083967,140.01512 -35.66083952,140.0151203 -35.66083934,140.0151206 -35.66083919,140.0151208 -35.66083904,140.0151211 -35.66083889,140.0151213 -35.66083882,140.0151216 -35.66083867,140.0151219 -35.66083853,140.0151221 -35.66083838,140.0151223 -35.6608382,140.0151227 -35.66083805,140.015123 -35.6608379,140.0151232 -35.66083775,140.0151234 -35.66083745,140.0151239 -35.66083729,140.0151241 -35.66083712,140.0151244 -35.66083695,140.0151246 -35.66083683,140.0151248 -35.66083666,140.0151251 -35.66083649,140.0151253 -35.66083631,140.0151256 -35.66083631,140.0151259 -35.66083613,140.0151262 -35.66083595,140.0151264 -35.66083577,140.0151267 -35.66083558,140.015127 -35.6608354,140.0151273 -35.66083522,140.0151275 -35.66083503,140.0151278 -35.66083483,140.0151282 -35.66083464,140.0151284 -35.66083445,140.0151287 -35.66083427,140.0151289 -35.66083411,140.0151292 -35.66083392,140.0151295 -35.66083372,140.0151297 -35.66083351,140.01513 -35.66083343,140.0151302 -35.66083322,140.0151305 -35.660833,140.0151307 -35.66083279,140.015131 -35.66083238,140.0151314 -35.66083215,140.0151317 -35.66083192,140.0151319 -35.66083169,140.0151322 -35.6608316,140.0151323 -35.66083137,140.0151325 -35.66083114,140.0151328 -35.66083091,140.015133 -35.6608306,140.0151335 -35.66083038,140.0151337 -35.66083016,140.0151339 -35.66082994,140.0151342 -35.66082982,140.0151344 -35.66082961,140.0151346 -35.66082941,140.0151349 -35.66082922,140.0151351 -35.66082891,140.0151353 -35.66082872,140.0151355 -35.66082854,140.0151357 -35.66082837,140.0151358 -35.66082814,140.015136 -35.66082799,140.0151362 -35.66082785,140.0151363 -35.66082771,140.0151364 -35.66082771,140.0151365 -35.6608276,140.0151366 -35.66082749,140.0151367 -35.6608274,140.0151368 -35.6608274,140.0151368 -35.66082732,140.0151369 -35.66082724,140.015137 -35.66082714,140.0151371 -35.66082703,140.0151372 -35.66082694,140.0151373 -35.66082685,140.0151374 -35.66082676,140.0151375 -35.66082663,140.0151376 -35.66082654,140.0151377 -35.66082647,140.0151377 -35.66082639,140.0151378 -35.66082641,140.0151377 -35.66082635,140.0151378 -35.66082629,140.0151379 -35.66082624,140.0151379 -35.66082609,140.015138 -35.66082603,140.0151381 -35.66082597,140.0151382 -35.6608259,140.0151383 -35.66082586,140.0151383 -35.66082577,140.0151384 -35.66082568,140.0151385 -35.66082558,140.0151386 -35.66082555,140.0151386 -35.66082544,140.0151388 -35.66082533,140.0151389 -35.66082521,140.0151391 -35.66082507,140.015139 -35.66082493,140.0151392 -35.6608248,140.0151394 -35.66082466,140.0151395 -35.66082454,140.0151395 -35.66082441,140.0151396 -35.66082428,140.0151398 -35.66082416,140.0151399 -35.66082406,140.01514 -35.66082395,140.0151401 -35.66082384,140.0151403 -35.66082373,140.0151404 -35.66082345,140.0151406 -35.66082334,140.0151407 -35.66082323,140.0151409 -35.66082312,140.0151411 -35.66082288,140.0151412 -35.66082276,140.0151414 -35.66082263,140.0151416 -35.66082251,140.0151418 -35.66082228,140.0151419 -35.66082215,140.0151421 -35.66082203,140.0151422 -35.66082191,140.0151424 -35.66082169,140.0151424 -35.66082155,140.0151426 -35.66082142,140.0151428 -35.66082128,140.015143 -35.66082106,140.0151431 -35.66082092,140.0151433 -35.66082078,140.0151435 -35.66082064,140.0151437 -35.66082029,140.015144 -35.66082015,140.0151442 -35.66082001,140.0151444 -35.66081988,140.0151446 -35.66081959,140.0151449 -35.66081945,140.0151451 -35.6608193,140.0151453 -35.66081916,140.0151455 -35.6608189,140.0151458 -35.66081873,140.0151461 -35.66081856,140.0151463 -35.66081838,140.0151466 -35.66081802,140.0151467 -35.66081782,140.015147 -35.66081762,140.0151473 -35.6608174,140.0151476 -35.66081709,140.0151479 -35.66081687,140.0151482 -35.66081665,140.0151486 -35.66081641,140.0151489 diff --git a/gnssnav/config/course_data/shinnara_gym_cource_manual_work.csv b/gnssnav/config/course_data/shinnara_gym_cource_manual_work.csv deleted file mode 100755 index 36500cb..0000000 --- a/gnssnav/config/course_data/shinnara_gym_cource_manual_work.csv +++ /dev/null @@ -1,45 +0,0 @@ -35.660964,140.01494 -35.660962,140.014943 -35.660955,140.014957 -35.660944,140.014972 -35.660938,140.014985 -35.660929,140.015001 -35.66092,140.015012 -35.660912,140.015026 -35.660903,140.015041 -35.660893,140.015056 -35.66088,140.015063 -35.660865,140.015077 -35.660855,140.015093 -35.660845,140.015106 -35.660833,140.01512 -35.660823,140.015136 -35.660812,140.015151 -35.660803,140.015169 -35.660794,140.015184 -35.660783,140.015201 -35.660774,140.015219 -35.660764,140.015232 -35.660755,140.015247 -35.660739,140.01526 -35.660729,140.015275 -35.660718,140.015287 -35.660712,140.015273 -35.660715,140.015248 -35.660727,140.015224 -35.660743,140.015203 -35.660756,140.015181 -35.660774,140.015157 -35.660789,140.015136 -35.660803,140.015112 -35.660819,140.01509 -35.660835,140.015069 -35.66085,140.015047 -35.660867,140.015023 -35.660879,140.015002 -35.660894,140.014981 -35.66091,140.014956 -35.660922,140.014939 -35.660934,140.014926 -35.660949,140.014924 -35.660958,140.014929 diff --git a/gnssnav/config/course_data/straight_shinnara.csv b/gnssnav/config/course_data/straight_shinnara.csv deleted file mode 100644 index e057fcd..0000000 --- a/gnssnav/config/course_data/straight_shinnara.csv +++ /dev/null @@ -1,117 +0,0 @@ -35.66258272999848,140.0130393645845 -35.66258421751185,140.01303957122477 -35.66258455671921,140.01303946897863 -35.66258550052764,140.01303931303138 -35.6625859299926,140.0130393838393 -35.66258626112044,140.0130394724766 -35.662586636859,140.013039403154 -35.6625867565837,140.01303955981746 -35.66258699539589,140.01303944835533 -35.66258705724916,140.01303917805024 -35.66258712727824,140.01303891180376 -35.662587206592306,140.01303904879316 -35.66258760382061,140.01303949975065 -35.66258788511432,140.01303954764757 -35.66258813391708,140.01303971288374 -35.66258833511933,140.0130396572968 -35.66258867965469,140.0130396384679 -35.66258884387621,140.0130395558764 -35.66258909874502,140.01303956320913 -35.66258905603327,140.01303956507633 -35.662587892079955,140.01303883871944 -35.66258566193608,140.01303726592224 -35.6625826826048,140.0130351858509 -35.66257917161368,140.01303265189148 -35.662575436214944,140.01302983360216 -35.662571461361004,140.01302693553865 -35.66256726507662,140.0130240345394 -35.662563163143275,140.01302106171198 -35.662558759155964,140.0130187637507 -35.66255441235535,140.01301636208453 -35.66255019533915,140.01301424030785 -35.66254699527273,140.01301329948683 -35.66254455549821,140.01301299234865 -35.662541759866286,140.01301197512802 -35.66253974348663,140.0130117176286 -35.66253789091458,140.0130112052999 -35.662536395348816,140.01301082281014 -35.66253522829447,140.01301064839933 -35.66252712741055,140.01300781035872 -35.66251476481145,140.01299626478527 -35.66250582853112,140.01298282073554 -35.662502979367325,140.01297968871418 -35.662499766273314,140.01297686952904 -35.66249599882036,140.0129742144515 -35.66249190001528,140.01297164599734 -35.66248795710405,140.01296925977658 -35.66248456422554,140.0129675741261 -35.6624815825414,140.0129661870945 -35.66246051429139,140.0129515915299 -35.662457132645216,140.012948172644 -35.662453651835705,140.01294431479 -35.66245001073269,140.01294062271748 -35.66244596553344,140.01293722225498 -35.662441796469494,140.01293434714424 -35.662437738597745,140.01293206381607 -35.66243354841825,140.0129299623138 -35.66241493133476,140.01292100623576 -35.66239739445881,140.01290397425737 -35.66238073176611,140.01289016692178 -35.66237654300806,140.0128874192851 -35.66237266830764,140.01288470424976 -35.662369014469505,140.01288142739756 -35.66236563030566,140.01287772090816 -35.662362424590334,140.0128736490917 -35.66235954468147,140.0128690781008 -35.66235709035334,140.0128640619446 -35.662354483313514,140.01285939594044 -35.662352187885624,140.01285568362334 -35.66235014587768,140.01285327661782 -35.66234848506176,140.01285202063698 -35.662347008623854,140.0128510914289 -35.66234561057615,140.0128504167348 -35.66234443634127,140.012850054645 -35.662344023309,140.01285016335032 -35.662343999671826,140.01285017493927 -35.662343879091246,140.01285038495945 -35.66234390496981,140.01285035608277 -35.662343837063645,140.01285028350202 -35.66234374988865,140.01285018768343 -35.66234365285874,140.01285025756596 -35.66234370814781,140.01285023554635 -35.66234360005008,140.0128502941951 -35.66234367307191,140.01285018287822 -35.6623435015427,140.01285021656105 -35.66234349253664,140.0128502865391 -35.66234313818591,140.01285025720694 -35.662342836117055,140.01285022281695 -35.66234239563553,140.0128499484663 -35.662342151950625,140.01284981986663 -35.66234201645613,140.01284993637964 -35.66234191260112,140.01284981683247 -35.66234188471223,140.01284979180505 -35.66234181361243,140.01284962598618 -35.66234165439253,140.0128496616906 -35.66234162146257,140.01284987013383 -35.66234152076703,140.01284996848815 -35.662341725767305,140.01284989596527 -35.662341619323,140.01284969060646 -35.662341459341974,140.01284960034 -35.6623412531873,140.01284934955564 -35.66234120989789,140.0128492559613 -35.66234127768518,140.01284907605046 -35.66234107706774,140.0128490041296 -35.662341076481674,140.01284871106895 -35.66234106353848,140.01284873776314 -35.662340678051095,140.01284871503142 -35.66234073292436,140.012848672461 -35.66234048629266,140.0128483693109 -35.662340311302884,140.01284831218248 -35.66234018311796,140.01284833331383 -35.66233999213032,140.01284771062257 -35.662339559886135,140.0128475712654 -35.66233950383531,140.01284749823563 -35.66233946598771,140.01284738656722 -35.662339437920856,140.01284745738232 -35.66233946362308,140.0128473613168 -35.662339284283696,140.01284744824628 \ No newline at end of file diff --git a/gnssnav/include/gnssnav/follower_node.hpp b/gnssnav/include/gnssnav/follower_node.hpp index 8bcec73..f96a076 100644 --- a/gnssnav/include/gnssnav/follower_node.hpp +++ b/gnssnav/include/gnssnav/follower_node.hpp @@ -21,8 +21,6 @@ namespace gnssnav{ class Follower : public rclcpp::Node{ public: - Follower(); - void loop(void); int freq_; @@ -32,6 +30,8 @@ class Follower : public rclcpp::Node{ GNSSNAV_PUBLIC explicit Follower(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + GNSSNAV_PUBLIC ~Follower(); + private: bool autonomous_flag_=false; bool nav_start_flag_; @@ -111,6 +111,9 @@ class Follower : public rclcpp::Node{ double init_yaw=0; std::vector pre_point; + + PJ_CONTEXT *C; + PJ *P; }; } // namespace gnssnav diff --git a/gnssnav/include/gnssnav/odom_follower_node.hpp b/gnssnav/include/gnssnav/odom_follower_node.hpp deleted file mode 100644 index 57e0cd3..0000000 --- a/gnssnav/include/gnssnav/odom_follower_node.hpp +++ /dev/null @@ -1,121 +0,0 @@ -#pragma once - -#include "gnssnav/visibility_control.h" - -#include - -#include - -#include "nav_msgs/msg/path.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/vector3.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" - -#include -#include -#include -#include - -// #include -#include - -namespace gnssnav{ - -class Follower : public rclcpp::Node{ -public: - Follower(); - - void loop(void); - int freq_; - - GNSSNAV_PUBLIC - explicit Follower(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - - GNSSNAV_PUBLIC - explicit Follower(const std::string& name_space, const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); - -private: - bool autonomous_flag_; - bool nav_start_flag_; - bool init_base_flag_; - bool init_flag_; - - double ld_; - double ld_x_; - double ld_y_; - double base_x; - double base_y; - double base_z; - double base_w; - double ld_min_; - double ld_gain_; - double v_; - double w_; - double v_max_; - double w_max_; - double cte_gain_; - double wheel_base_; - double distance_; - double theta; - - int freq; - - // geometry_msgs::msg::PoseStamped pose_msg; - - // rclcpp::Subscription::SharedPtr vectornav_subscriber_; - rclcpp::Subscription::SharedPtr odom_sub_; - rclcpp::Subscription::SharedPtr path_subscriber_; - rclcpp::Subscription::SharedPtr nav_start_subscriber_; - rclcpp::Subscription::SharedPtr autonomous_flag_subscriber_; - - rclcpp::Publisher::SharedPtr cmd_pub_; - rclcpp::Publisher::SharedPtr current_pose_pub_; - rclcpp::Publisher::SharedPtr current_ld_pub_; - rclcpp::Publisher::SharedPtr theta_pub_; - - rclcpp::TimerBase::SharedPtr timer_; - - std::vector front_wheel_pos; - std::vector point_; - - void declareParameter(void); - - // void vectornavCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg); - void vectornavCallback(const nav_msgs::msg::Odometry::SharedPtr msg); - void pathCallback(const nav_msgs::msg::Path::SharedPtr msg); - void navStartCallback(const std_msgs::msg::Empty::SharedPtr&); - void autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg); - - double calculateCrossError(); - double calculateHeadingError(); - - void findNearestIndex(const geometry_msgs::msg::Pose front_wheel_pos); - void publishCurrentPose(void); - void publishLookahead(void); - void followPath(); - void findLookaheadDistance(); - void setBasePose(void); - void calcPathDirection(void); - - double calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion&); - std::pair convertECEFtoUTM(double x, double y, double z); - double radian2deg(double rad); - - double current_position_x_; - double current_position_y_; - double current_yaw_; - size_t idx_; - size_t pre_point_idx; - double vectornav_base_x_; - double vectornav_base_y_; - double vectornav_base_yaw_; - double path_direction_; - double pose_orientation_z_; - double count; - double init_yaw; - - std::vector pre_point; -}; - -} // namespace gnssnav diff --git a/gnssnav/src/follower_node.cpp b/gnssnav/src/follower_node.cpp index 48f062d..b3f04b4 100644 --- a/gnssnav/src/follower_node.cpp +++ b/gnssnav/src/follower_node.cpp @@ -35,6 +35,18 @@ wheel_base_(get_parameter("wheelbase").as_double()) timer_ = this->create_wall_timer( std::chrono::milliseconds(freq), std::bind(&Follower::loop, this)); + + C = proj_context_create(); + P = proj_create_crs_to_crs(C, + "EPSG:4978", // ECEF + "EPSG:32654", // UTMゾーン54N + NULL); +} + +Follower::~Follower(){ + // リソースの解放 + proj_destroy(P); + proj_context_destroy(C); } // vectornav/pose callback @@ -142,11 +154,6 @@ void Follower::findNearestIndex(geometry_msgs::msg::Pose front_wheel_pos){ if(distance_ > ld_ && idx_ > pre_point_idx && point_[idx_].pose.position.x > 30000){ pre_point_idx = idx_ - 1; - // RCLCPP_INFO(this->get_logger(), "idx : %d\npre_idx : %d", idx_, pre_point_idx); - // std::cerr << "point_x:" << point_[idx_].pose.position.x << "\npoint_y:"<< point_[idx_].pose.position.y << std::endl; - // std::cerr << "point_size:" << point_.size() << std::endl; - // std::cerr << "distance" << distance_ << std::endl; - // std::cerr << "wheel_pos_x:" << front_wheel_pos.position.x << "\nwheel_pos_y:" << front_wheel_pos.position.y << std::endl; break; } } @@ -181,7 +188,7 @@ double Follower::calculateCrossError(){ theta = target_angle - angle; theta = std::atan2(std::sin(theta), std::cos(theta)); - double cross_error = dy * std::cos(theta) - dx * std::sin(theta); + double cross_error = dy * std::cos(current_yaw_) - dx * std::sin(current_yaw_); RCLCPP_INFO_EXPRESSION(this->get_logger(), is_debug, "target:%lf° current:%lf°", rtod(target_angle), rtod(angle)); return cross_error; @@ -194,7 +201,7 @@ double Follower::calculateHeadingError(){ point_[idx_].pose.position.x - point_[pre_point_idx].pose.position.x); double heading_error = traj_theta - theta; - heading_error = std::atan2(std::sin(heading_error), std::cos(heading_error)); + heading_error = std::atan2(std::sin(heading_error), std::cos(heading_error)) *-1; return heading_error; } @@ -215,14 +222,13 @@ void Follower::followPath(){ // pointとpre_pointを結ぶ先に対する現在の車体の角度 double he = calculateHeadingError(); - v_ = std::min(v_max_, ld_); - // RCLCPP_INFO(this->get_logger(), "cmdvel:%lf vmax:%lf ld:%lf", v_,v_max_,ld_); + v_ = v_max_; + // RCLCPP_INFO(this->get_logger(), "distance : %lf idx_ : %d", distance_, idx_); w_ = he + std::atan2(cte_gain_ * cte, v_); geometry_msgs::msg::Twist cmd_vel; cmd_vel.linear.x = v_; - // cmd_vel.angular.z = std::max(std::min(theta, 1.0), -1.0); - cmd_vel.angular.z = constrain(theta, -w_max_, w_max_); + cmd_vel.angular.z = constrain(w_, -w_max_, w_max_); // 完走した判定 if(idx_ >= point_.size() - 5){ @@ -249,11 +255,6 @@ double Follower::calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion } std::pair Follower::convertECEFtoUTM(double x, double y, double z){ - PJ_CONTEXT *C = proj_context_create(); - PJ *P = proj_create_crs_to_crs(C, - "EPSG:4978", // ECEF - "EPSG:32654", // UTMゾーン54N - NULL); if(P == NULL) { std::cerr << "PROJ transformation creation failed." << std::endl; } @@ -265,10 +266,6 @@ std::pair Follower::convertECEFtoUTM(double x, double y, double b = proj_trans(P, PJ_FWD, a); - // リソースの解放 - proj_destroy(P); - proj_context_destroy(C); - return {b.enu.e, b.enu.n}; } diff --git a/gnssnav/src/odom_follower_node.cpp b/gnssnav/src/odom_follower_node.cpp deleted file mode 100644 index 512f750..0000000 --- a/gnssnav/src/odom_follower_node.cpp +++ /dev/null @@ -1,312 +0,0 @@ -#include "gnssnav/odom_follower_node.hpp" - -namespace gnssnav{ - -Follower::Follower(const rclcpp::NodeOptions& options) : Follower("", options) {} - -// pub, sub, param -Follower::Follower(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("gnssnav_follower_node", name_space, options) -{ - auto callback = [this](const std_msgs::msg::Empty::SharedPtr msg) { this->navStartCallback(msg); }; - - autonomous_flag_subscriber_ = this->create_subscription("/autonomous", 10, std::bind(&Follower::autonomousFlagCallback, this, std::placeholders::_1)); - nav_start_subscriber_ = this->create_subscription("/nav_start", 10, callback); - // vectornav_subscriber_ = this->create_subscription("/vectornav/pose", 10, std::bind(&Follower::vectornavCallback, this, std::placeholders::_1)); - odom_sub_ = this->create_subscription("/odom", 10, std::bind(&Follower::vectornavCallback, this, std::placeholders::_1)); - path_subscriber_ = this->create_subscription("/gnss_path", 10, std::bind(&Follower::pathCallback, this, std::placeholders::_1)); - - cmd_pub_ = this->create_publisher("/cmd_vel", 10); - current_pose_pub_ = this->create_publisher("/current_pose", 10); - current_ld_pub_ = this->create_publisher("/current_ld", 10); - theta_pub_ =this->create_publisher("/theta", 10); - - this->get_parameter("lookahead_gain", ld_gain_); - this->get_parameter("cte_gain", cte_gain_); - this->get_parameter("min_lookahead_distance", ld_min_); - this->get_parameter("max_linear_vel", v_max_); - this->get_parameter("max_angular_vel", w_max_); - this->get_parameter("follower_freq", freq); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(freq), - std::bind(&Follower::loop, this)); -} - -// vectornav/pose callback -void Follower::vectornavCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { - // auto [x, y] = convertECEFtoUTM(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - - current_position_x_ = msg->pose.pose.position.x; - current_position_y_ = msg->pose.pose.position.y; - //if(!init_base_flag_ && count >= 100){ - // init_yaw = calculateYawFromQuaternion(msg->pose.pose.orientation); - //} - // RCLCPP_INFO(this->get_logger(), "current_position_x: %f\ncurrent_position_y: %f", current_position_x_, current_position_y_); - - current_yaw_ = calculateYawFromQuaternion(msg->pose.pose.orientation); - double current_yaw_deg = radian2deg(current_yaw_); - // RCLCPP_INFO(this->get_logger(), "current_yaw_deg: %f", current_yaw_deg); - - // RCLCPP_INFO(this->get_logger(), "base_x: %f\ncurrent_position_x: %f\nbase_yaw: %f", vectornav_base_x_, current_position_x_, vectornav_base_yaw_); - // std::cerr << init_base_flag_ << std::endl; - if(!init_base_flag_) { - setBasePose(); - } else { - publishCurrentPose(); - } -} - -// received path -void Follower::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { - point_ = msg->poses; - // RCLCPP_INFO(this->get_logger(), "received %zu pose", point_.size()); -} - -// gnssnav permit -void Follower::navStartCallback(const std_msgs::msg::Empty::SharedPtr&) { - nav_start_flag_ = true; - RCLCPP_INFO(this->get_logger(), "自律走行開始"); -} - -// autonomous_flag_を更新 -void Follower::autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg) { - autonomous_flag_ = msg->data; - RCLCPP_INFO(this->get_logger(), "Autonomous flag updated to: %s", autonomous_flag_ ? "true" : "false"); -} - -void Follower::setBasePose(){ - vectornav_base_x_ = current_position_x_; - vectornav_base_y_ = current_position_y_; - vectornav_base_yaw_ = current_yaw_; - - std::cerr << "set Base Pose" << std::endl; - init_base_flag_ = true; - - // for(const auto &pose : point_){ - // RCLCPP_INFO(this->get_logger(), "path_x:%f , path_y:%f", pose.pose.position.x, pose.pose.position.y); - // } -} - -// 現在地をパブリッシュ -void Follower::publishCurrentPose(){ - double dx = point_[1].pose.position.x - point_[0].pose.position.x; - double dy = point_[1].pose.position.y - point_[0].pose.position.y; - path_direction_ = std::atan2(dy, dx); - - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = this->now(); - pose_msg.header.frame_id = "map"; - - tf2::Quaternion initial_q, current_q, result_q; - initial_q.setRPY(0, 0, path_direction_ - vectornav_base_yaw_); - initial_q = initial_q.inverse(); - - tf2::Vector3 current_position( - current_position_x_ - vectornav_base_x_, - current_position_y_ - vectornav_base_y_, - 0.0 - ); - - tf2::Vector3 corrected_position = tf2::quatRotate(initial_q, current_position); - - pose_msg.pose.position.x = corrected_position.x(); - pose_msg.pose.position.y = corrected_position.y(); - pose_msg.pose.position.z = corrected_position.z(); - - current_q.setRPY(0, 0, current_yaw_); - result_q = initial_q * current_q; - - pose_msg.pose.orientation = tf2::toMsg(result_q); - current_pose_pub_->publish(pose_msg); - - pose_orientation_z_ = pose_msg.pose.orientation.z; -} - -void Follower::publishLookahead(){ - geometry_msgs::msg::PoseStamped pose_msg; - pose_msg.header.stamp = this->now(); - pose_msg.header.frame_id = "map"; - - pose_msg.pose.position.x = point_[idx_].pose.position.x - vectornav_base_x_; - pose_msg.pose.position.y = point_[idx_].pose.position.y - vectornav_base_y_; - pose_msg.pose.position.z = 0.0; - - current_ld_pub_->publish(pose_msg); -} - -// 最も近い経路点を見つける(座標とidx) -void Follower::findNearestIndex(geometry_msgs::msg::Pose front_wheel_pos){ - for(idx_ = 0;idx_ < point_.size(); idx_++){ - double dx = point_[idx_].pose.position.x - front_wheel_pos.position.x; - double dy = point_[idx_].pose.position.y - front_wheel_pos.position.y; - distance_ = std::hypot(dx, dy); - -// std::cerr << "idx:" << idx_ << std::endl; -// std::cerr << "dx:" << dx << " " <<"dy" << dy << std::endl; -//if(idx_ >= 100) - - if(distance_ > ld_ && idx_ > pre_point_idx){ - pre_point_idx = idx_; - RCLCPP_INFO(this->get_logger(), "idx:%d", idx_); - std::cerr << "point_x:" << point_[idx_].pose.position.x << "\npoint_y:"<< point_[idx_].pose.position.y << std::endl; - // std::cerr << "distance" << distance_ << std::endl; - std::cerr << "wheel_pos_x:" << front_wheel_pos.position.x << "\nwheel_pos_y:" << front_wheel_pos.position.y << std::endl; - break; - } - } - return ; -} - -// 目標地点を探索する -void Follower::findLookaheadDistance(){ - wheel_base_ = 600; - double front_x_ = - current_position_x_; - //current_position_x_ + wheel_base_ / 2.0 * std::cos(pose_orientation_z_); - double front_y_ = - current_position_y_; - //current_position_y_ + wheel_base_ / 2.0 * std::sin(pose_orientation_z_); - - geometry_msgs::msg::Pose front_wheel_pos; - front_wheel_pos.position.x = front_x_; - front_wheel_pos.position.y = front_y_; - - //std::cerr << "current_y_2: "< 0){ - pre_point_idx = idx_ - 1; - }else{ - pre_point_idx = idx_; - } -} - -double Follower::radian2deg(double rad){ - double deg = rad * (180 / M_PI); - return deg; -} - -// not scope 経路に対しての横方向のズレを計算 -double Follower::calculateCrossError(){ - double dx = point_[idx_].pose.position.x - current_position_x_; - double dy = point_[idx_].pose.position.y - current_position_y_; - - double target_angle = std::atan2(dy, dx); - double target_angle_deg = radian2deg(target_angle); - // RCLCPP_INFO(this->get_logger(), "target_angle_deg: %f", target_angle_deg); - theta = target_angle - current_yaw_; - theta = std::atan2(std::sin(theta), std::cos(theta)); - - // RCLCPP_INFO(this->get_logger(), "target_angle:%f, theta_deg: %f", target_angle, radian2deg(theta)); - - double cross_error = dy * std::cos(theta) - dx * std::sin(theta); - - return cross_error; -} - -// pointとpre_pointを結ぶ先に対する現在の車体の角度を計算 -double Follower::calculateHeadingError(){ - double traj_theta = - std::atan2(point_[idx_].pose.position.y - point_[pre_point_idx].pose.position.y, - point_[idx_].pose.position.x - point_[pre_point_idx].pose.position.x); - - double heading_error = traj_theta - theta; - heading_error = std::atan2(std::sin(heading_error), std::cos(heading_error)); - - return heading_error; -} - -void Follower::followPath(){ - if(point_.empty()){ - std::cerr << "point_empty error" << std::endl; - return; - } - - ld_ = ld_gain_ * v_ + ld_min_; - - findLookaheadDistance(); - publishLookahead(); - - // 横方向のズレ - double cte = calculateCrossError(); - // pointとpre_pointを結ぶ先に対する現在の車体の角度 - double he = calculateHeadingError(); - - // v_ = std::min(v_max_, ld_); - v_ = 1.5; - w_ = he + std::atan2(cte_gain_ * cte, v_); - - double theta_deg = radian2deg(theta); - // RCLCPP_INFO(this->get_logger(), "distance: %f meters, theta_deg: %f deg, idx %d", distance_, theta_deg, idx_); - - //theta_degをパブリッシュ - theta_pub_->publish(theta_deg); - //RCLCPP_INFO(this->get_logger(), "hand error : %f", he); - - geometry_msgs::msg::Twist cmd_vel; - cmd_vel.linear.x = v_; - cmd_vel.angular.z = std::max(std::min(theta, 1.0), -1.0); - - // 完走した判定 - if(idx_ >= point_.size() - 5){ - cmd_vel.linear.x = 0.0; - cmd_vel.angular.z = 0.0; - RCLCPP_INFO(this->get_logger(), "Goal to reach"); - } - - // 自律フラグonのときのみパブリッシュ - if(autonomous_flag_){ - // std::cerr << "nav_start_flag error" << std::endl; - cmd_pub_->publish(cmd_vel); - } -} - -// クオータニオンからオイラーへ変換 -double Follower::calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion& quat){ - - tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - return yaw; -} - -std::pair Follower::convertECEFtoUTM(double x, double y, double z){ - PJ_CONTEXT *C = proj_context_create(); - PJ *P = proj_create_crs_to_crs(C, - "EPSG:4978", // ECEF - "EPSG:32654", // UTMゾーン54N - NULL); - if(P == NULL) { - std::cerr << "PROJ transformation creation failed." << std::endl; - } - PJ_COORD a, b; - - a.xyz.x = x; - a.xyz.y = y; - a.xyz.z = z; - - b = proj_trans(P, PJ_FWD, a); - - // リソースの解放 - proj_destroy(P); - proj_context_destroy(C); - - return {b.enu.e, b.enu.n}; -} - -void Follower::loop(void){ - followPath(); -} - -} // namespace gnssnav - -int main(int argc, char * argv[]){ - rclcpp::init(argc, argv); - auto node = std::make_shared(rclcpp::NodeOptions()); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} diff --git a/gnssnav/src/test_follower_node.cpp b/gnssnav/src/test_follower_node.cpp deleted file mode 100644 index 3a847d4..0000000 --- a/gnssnav/src/test_follower_node.cpp +++ /dev/null @@ -1,144 +0,0 @@ -#include "gnssnav/test_follower_node.hpp" - -namespace gnssnav{ - -Follower::Follower(const rclcpp::NodeOptions& options) : Follower("", options) {} - -// pub, sub, param -Follower::Follower(const std::string& name_space, const rclcpp::NodeOptions& options) -: rclcpp::Node("gnssnav_follower_node", name_space, options) -{ - auto callback = [this](const std_msgs::msg::Empty::SharedPtr msg) { this->navStartCallback(msg); }; - - autonomous_flag_subscriber_ = this->create_subscription("/autonomous", 10, std::bind(&Follower::autonomousFlagCallback, this, std::placeholders::_1)); - nav_start_subscriber_ = this->create_subscription("/nav_start", 10, callback); - vectornav_subscriber_ = this->create_subscription("/vectornav/pose", 10, std::bind(&Follower::vectornavCallback, this, std::placeholders::_1)); - path_subscriber_ = this->create_subscription("/gnss_path", 10, std::bind(&Follower::pathCallback, this, std::placeholders::_1)); - - cmd_pub_ = this->create_publisher("/cmd_vel", 10); - current_pose_pub_ = this->create_publisher("/current_pose", 10); - current_ld_pub_ = this->create_publisher("/current_ld", 10); - theta_pub_ =this->create_publisher("/theta", 10); - - this->get_parameter("lookahead_gain", ld_gain_); - this->get_parameter("cte_gain", cte_gain_); - this->get_parameter("min_lookahead_distance", ld_min_); - this->get_parameter("max_linear_vel", v_max_); - this->get_parameter("max_angular_vel", w_max_); - this->get_parameter("follower_freq", freq); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(freq), - std::bind(&Follower::onTimer, this)); -} - -void Follower::vectornavCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg) { - // auto [x, y] = convertECEFtoUTM(msg->pose.pose.position.x, msg->pose.pose.position.y, msg->pose.pose.position.z); - - // current_position_x_ = x; - // current_position_y_ = y; - - RCLCPP_INFO(this->get_logger(), "current_position_x: %f\ncurrent_position_y: %f", current_position_x_, current_position_y_); - - current_yaw_ = calculateYawFromQuaternion(msg->pose.pose.orientation); - // double current_yaw_deg = radian2deg(current_yaw_); -} - -// received path -void Follower::pathCallback(const nav_msgs::msg::Path::SharedPtr msg) { - point_ = msg->poses; - // RCLCPP_INFO(this->get_logger(), "received %zu pose", point_.size()); -} - -void Follower::onTimer(){ - // if(!autonomous()) - // return; - - size_t closest_idx_ = - findNearestIndex(); - - geometry_msgs::msg::Twist cmd_vel; - if(closest_idx_ == point_.size() - 3){ - // cmd_vel.linear.x = 0.0; - // cmd_vel.angular.z = 0.0; - // RCLCPP_INFO(this->get_logger(), "Goal to reach"); - }else{ - // cmd_vel.linear.x = 3.0; - // cmd_vel.angular.z = 3.0; - - // cmd_pub_->publish(cmd_vel); - } -} - -int Follower::findNearestIndex(){ - int num = 1; - return num; -} - - -// クオータニオンからオイラーへ変換 -double Follower::calculateYawFromQuaternion(const geometry_msgs::msg::Quaternion& quat){ - - tf2::Quaternion q(quat.x, quat.y, quat.z, quat.w); - tf2::Matrix3x3 m(q); - double roll, pitch, yaw; - m.getRPY(roll, pitch, yaw); - return yaw; -} - -double Follower::radian2deg(double rad){ - double deg = rad * (180 / M_PI); - return deg; -} - -std::pair Follower::convertECEFtoUTM(double x, double y, double z){ - PJ_CONTEXT *C = proj_context_create(); - PJ *P = proj_create_crs_to_crs(C, - "EPSG:4978", // ECEF - "EPSG:32654", // UTMゾーン54N - NULL); - if(P == NULL) { - std::cerr << "PROJ transformation creation failed." << std::endl; - } - PJ_COORD a, b; - - a.xyz.x = x; - a.xyz.y = y; - a.xyz.z = z; - - b = proj_trans(P, PJ_FWD, a); - - // リソースの解放 - proj_destroy(P); - proj_context_destroy(C); - - return {b.enu.e, b.enu.n}; -} - -// gnssnav permit -void Follower::navStartCallback(const std_msgs::msg::Empty::SharedPtr&) { - nav_start_flag_ = true; - RCLCPP_INFO(this->get_logger(), "自律走行開始"); -} - -// autonomous_flag_を更新 -void Follower::autonomousFlagCallback(const std_msgs::msg::Bool::SharedPtr msg) { - autonomous_flag_ = msg->data; - RCLCPP_INFO(this->get_logger(), "Autonomous flag updated to: %s", autonomous_flag_ ? "true" : "false"); -} - -bool Follower::autonomous(){ - if(!autonomous_flag_) - return false; - if(point_.empty()) - return false; - return true; -} - -} // namespace gnssnav - -int main(int argc, char * argv[]){ - rclcpp::init(argc, argv); - rclcpp::shutdown(); - return 0; -} diff --git a/main_executor/CMakeLists.txt b/main_executor/CMakeLists.txt index 6dc371d..5da15b7 100644 --- a/main_executor/CMakeLists.txt +++ b/main_executor/CMakeLists.txt @@ -9,7 +9,7 @@ endif() find_package(rclcpp REQUIRED) find_package(socketcan_interface REQUIRED) find_package(controller REQUIRED) -find_package(roboteq_driver REQUIRED) +find_package(chassis_driver REQUIRED) find_package(cybergear_interface REQUIRED) find_package(gnssnav REQUIRED) @@ -21,7 +21,7 @@ ament_target_dependencies(main_exec rclcpp socketcan_interface controller - roboteq_driver + chassis_driver cybergear_interface gnssnav ) @@ -32,6 +32,21 @@ target_include_directories(main_exec $ ) +add_executable(main_sim_exec + src/main_sim.cpp + ) + +ament_target_dependencies(main_sim_exec + rclcpp + controller + gnssnav + ) + +target_include_directories(main_sim_exec + PUBLIC + $ + $ + ) # find dependencies find_package(ament_cmake REQUIRED) @@ -58,4 +73,9 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +install(TARGETS + main_sim_exec + DESTINATION lib/${PROJECT_NAME} + ) + ament_package() diff --git a/main_executor/config/main_params.yaml b/main_executor/config/main_params.yaml index ae44a15..c2d81da 100644 --- a/main_executor/config/main_params.yaml +++ b/main_executor/config/main_params.yaml @@ -1,7 +1,7 @@ launch: #起動パラメータ ros__parameters: - can: false - joy: false + can: true + joy: true vectornav : false log_level : 10 @@ -12,15 +12,13 @@ launch: #起動パラメータ # 並進 速度[m/s],加速度[m/s^2],加減速度[-m/s^2],躍度[m/s^3] linear_max: - vel : 1.5 - acc: 2.0 - dec: 2.0 + vel : 3.0 + acc: 9.0 jer: 0.0 # 回転 速度[deg/s],加速度[deg/s^2],加減速度[-deg/s^2],躍度[deg/s^3] angular_max : - vel: 30.0 - acc: 40.0 - dec: 40.0 + vel: 60.0 + acc: 180.0 jer: 0.0 tread : 0.6 @@ -39,7 +37,7 @@ cybergear: # controller_node: # ros__parameters: -roboteq_driver_node: +chassis_driver_node: ros__parameters: interval_ms : 10 wheel_radius : 0.124 @@ -63,15 +61,15 @@ cybergear_interface_node: path_publisher_node: ros__parameters: - interval_ms : 100 - path_file_name : "gazebo_shihou_course" + interval_ms : 1000 + path_file_name : "shihou_gnssnav" gnssnav_follower_node: ros__parameters: debug_flag : false - interval_ms : 100 + interval_ms : 50 lookahead_gain : 1.0 - cte_gain : 1.0 + cte_gain : 1.5 min_lookahead_distance : 3.0 - max_linear_vel : 7.0 + max_linear_vel : 2.0 max_angular_vel : 1.5 diff --git a/main_executor/launch/main_sim_exec.launch.py b/main_executor/launch/main_sim_exec.launch.py new file mode 100755 index 0000000..2f78b97 --- /dev/null +++ b/main_executor/launch/main_sim_exec.launch.py @@ -0,0 +1,97 @@ +import os +import subprocess +import yaml +import launch +from launch import LaunchDescription +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.actions import DeclareLaunchArgument, ExecuteProcess +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('main_executor'), + 'config', + 'main_params.yaml' + ) + + convert_sim_to_vectornav_pose_path = os.path.join( + "/home/mirai/ros2_ws/src/aiformula", + 'scripts', + 'convert_sim_to_vectornav_pose.py' + ) + + # 引数の宣言 + simulator_launch_arg = DeclareLaunchArgument( + 'simulator_launch_path', + default_value=os.path.join( + get_package_share_directory('simulator'), + 'launch', + 'gazebo_simulator.launch.py' + ), + description='Path to the Gazebo simulator launch file' + ) + + # シミュレータ起動ファイルのパス設定 + simulator_launch_path = LaunchConfiguration('simulator_launch_path') + + # 起動パラメータファイルのロード + with open(config_file_path, 'r') as file: + launch_params = yaml.safe_load(file)['launch']['ros__parameters'] + + + # メイン実行機のログレベルの設定 + log_level = LaunchConfiguration("log_level") + # デフォルトのログレベルを'info'に設定 + log_level_arg = DeclareLaunchArgument( + "log_level", + default_value=["info"], + description="Logging level", + ) + + # シミュレータ起動アクション + simulator_launch = ExecuteProcess( + cmd=['ros2', 'launch', simulator_launch_path], + output='screen' + ) + + # スクリプト実行アクション + convert_sim_to_vectornav_pose = ExecuteProcess( + cmd=['python3', convert_sim_to_vectornav_pose_path], + output='screen' + ) + + # メイン実行機ノードの作成 + main_sim_exec_node = Node( + package = 'main_executor', + executable = 'main_sim_exec', + parameters = [config_file_path], + arguments=["--ros-args", "--log-level", log_level], + output='screen' + ) + + # 操縦機ノードの作成 + joy_node = Node( + package = 'joy', + executable = 'joy_node', + output='screen' + ) + + # 起動エンティティクラスの作成 + launch_discription = LaunchDescription() + + # 起動の追加 + # if(launch_params['can'] is True): + # subprocess.run(['sudo', 'sh', can_launch_path]) + # if(launch_params['joy'] is True): + # launch_discription.add_entity(joy_node) + + launch_discription.add_action(simulator_launch_arg) + launch_discription.add_entity(simulator_launch) + launch_discription.add_entity(convert_sim_to_vectornav_pose) + launch_discription.add_action(log_level_arg) + launch_discription.add_entity(main_sim_exec_node) + + return launch_discription diff --git a/main_executor/package.xml b/main_executor/package.xml index 54f3adb..4491e9b 100644 --- a/main_executor/package.xml +++ b/main_executor/package.xml @@ -13,7 +13,7 @@ socketcan_interface controller - roboteq_driver + chassis_driver cybergear_interface gnssnav diff --git a/main_executor/src/main.cpp b/main_executor/src/main.cpp index c114abe..c9ff5f5 100644 --- a/main_executor/src/main.cpp +++ b/main_executor/src/main.cpp @@ -2,7 +2,7 @@ #include "socketcan_interface/socketcan_interface_node.hpp" #include "controller/controller_node.hpp" -#include "roboteq_driver/roboteq_driver_node.hpp" +#include "chassis_driver/chassis_driver_node.hpp" #include "cybergear_interface/cybergear_interface_node.hpp" #include "gnssnav/path_publisher_node.hpp" #include "gnssnav/follower_node.hpp" @@ -15,19 +15,19 @@ int main(int argc, char * argv[]){ nodes_option.allow_undeclared_parameters(true); nodes_option.automatically_declare_parameters_from_overrides(true); - //auto socketcan_node = std::make_shared(nodes_option); - //auto socketcan_cybergear_node = std::make_shared("cybergear", nodes_option); - //auto controller_node = std::make_shared(nodes_option); - //auto roboteq_driver_node = std::make_shared(nodes_option); - //auto cybergear_interface_node = std::make_shared(nodes_option); + auto socketcan_node = std::make_shared(nodes_option); + auto socketcan_cybergear_node = std::make_shared("cybergear", nodes_option); + auto controller_node = std::make_shared(nodes_option); + auto chassis_driver_node = std::make_shared(nodes_option); + auto cybergear_interface_node = std::make_shared(nodes_option); auto path_publisher_node = std::make_shared(nodes_option); auto follower_node = std::make_shared(nodes_option); - //exec.add_node(socketcan_node); - //exec.add_node(socketcan_cybergear_node); - //exec.add_node(controller_node); - //exec.add_node(roboteq_driver_node); - //exec.add_node(cybergear_interface_node); + exec.add_node(socketcan_node); + exec.add_node(socketcan_cybergear_node); + exec.add_node(controller_node); + exec.add_node(chassis_driver_node); + exec.add_node(cybergear_interface_node); exec.add_node(path_publisher_node); exec.add_node(follower_node); diff --git a/main_executor/src/main_sim.cpp b/main_executor/src/main_sim.cpp new file mode 100644 index 0000000..0f5b1e7 --- /dev/null +++ b/main_executor/src/main_sim.cpp @@ -0,0 +1,24 @@ +#include + +#include "controller/controller_node.hpp" +#include "gnssnav/path_publisher_node.hpp" +#include "gnssnav/follower_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); + + auto path_publisher_node = std::make_shared(nodes_option); + auto follower_node = std::make_shared(nodes_option); + + exec.add_node(path_publisher_node); + exec.add_node(follower_node); + + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/roboteq_driver/include/roboteq_driver/visibility_control.h b/roboteq_driver/include/roboteq_driver/visibility_control.h deleted file mode 100644 index f6b5040..0000000 --- a/roboteq_driver/include/roboteq_driver/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef ROBOTEQ_DRIVER__VISIBILITY_CONTROL_H_ -#define ROBOTEQ_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 ROBOTEQ_DRIVER_EXPORT __attribute__ ((dllexport)) - #define ROBOTEQ_DRIVER_IMPORT __attribute__ ((dllimport)) - #else - #define ROBOTEQ_DRIVER_EXPORT __declspec(dllexport) - #define ROBOTEQ_DRIVER_IMPORT __declspec(dllimport) - #endif - #ifdef ROBOTEQ_DRIVER_BUILDING_LIBRARY - #define ROBOTEQ_DRIVER_PUBLIC ROBOTEQ_DRIVER_EXPORT - #else - #define ROBOTEQ_DRIVER_PUBLIC ROBOTEQ_DRIVER_IMPORT - #endif - #define ROBOTEQ_DRIVER_PUBLIC_TYPE ROBOTEQ_DRIVER_PUBLIC - #define ROBOTEQ_DRIVER_LOCAL -#else - #define ROBOTEQ_DRIVER_EXPORT __attribute__ ((visibility("default"))) - #define ROBOTEQ_DRIVER_IMPORT - #if __GNUC__ >= 4 - #define ROBOTEQ_DRIVER_PUBLIC __attribute__ ((visibility("default"))) - #define ROBOTEQ_DRIVER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define ROBOTEQ_DRIVER_PUBLIC - #define ROBOTEQ_DRIVER_LOCAL - #endif - #define ROBOTEQ_DRIVER_PUBLIC_TYPE -#endif - -#endif // ROBOTEQ_DRIVER__VISIBILITY_CONTROL_H_ diff --git a/scripts/csv_input_soft.py b/scripts/csv_input_soft.py new file mode 100644 index 0000000..4f2ae6c --- /dev/null +++ b/scripts/csv_input_soft.py @@ -0,0 +1,43 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import NavSatFix +import csv + +class Converter(Node): + def __init__(self): + super().__init__("gps_csv_node") + self.subscription = self.create_subscription(NavSatFix, "/vectornav/gnss", self.callback, 10) + self.subscription + self.get_logger().info("vectornav/gnssをcsvファイルにします。") + + self.csv_file = open('shihou_gnssnav.csv', mode='w', newline='') + self.csv_writer = csv.writer(self.csv_file) + self.csv_writer.writerow(['Latitude', 'Longitude']) + + self.counter = 0 # カウンタを初期化 + + def callback(self, msg): + self.counter += 1 # コールバックが呼ばれるたびにカウンタをインクリメント + if self.counter % 10 == 0: # 10回に1度だけ書き込み + latitude = msg.latitude + longitude = msg.longitude + print(latitude, longitude) + self.csv_writer.writerow([latitude, longitude]) + # self.get_logger().info(f'Latitude: {latitude}, Longitude: {longitude}') + + def destroy_node(self): + self.csv_file.close() + super().destroy_node() + +def main(): + rclpy.init() + node = Converter() + + rclpy.spin(node) + node.destroy_node() + + rclpy.shutdown() + +if __name__ == "__main__": + main() + diff --git a/utilities/CMakeLists.txt b/utilities/CMakeLists.txt index 31a8772..8426c54 100644 --- a/utilities/CMakeLists.txt +++ b/utilities/CMakeLists.txt @@ -17,6 +17,7 @@ include_directories(include) add_library(utilities src/utils.cpp + src/velplanner.cpp ) target_compile_features(utilities PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 target_include_directories(utilities PUBLIC diff --git a/utilities/include/utilities/velplanner.hpp b/utilities/include/utilities/velplanner.hpp new file mode 100644 index 0000000..44b293e --- /dev/null +++ b/utilities/include/utilities/velplanner.hpp @@ -0,0 +1,54 @@ +#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/utilities/src/velplanner.cpp b/utilities/src/velplanner.cpp new file mode 100644 index 0000000..3f16386 --- /dev/null +++ b/utilities/src/velplanner.cpp @@ -0,0 +1,69 @@ +#include "utilities/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/wiring.drawio b/wiring.drawio new file mode 100644 index 0000000..4f5dc7a --- /dev/null +++ b/wiring.drawio @@ -0,0 +1,550 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +