vector map navigation

.gitignore

@@ -1,6 +1,5 @@
 #拡張子で指定
 *.log
-*.urdf
 *.pyc
 *.pt
 
@@ -17,6 +16,9 @@ log/
 
 simulator_old/
 #ファイル名で指定
-
+AGENTS.md
+CLAUDE.md
+.codex
+*project.md
 
 #例外ファイルを指定

.gitmodules

@@ -1,5 +1,5 @@
 [submodule "vectornav"]
-	path = vectornav
+	path = sensing/vectornav
 	url = https://github.com/open-rdc/vectornav
 [submodule "socketcan_interface"]
 	path = socketcan_interface

controller/include/controller/controller_node.hpp

@@ -5,6 +5,8 @@
 #include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/bool.hpp>
 #include <std_msgs/msg/char.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <string>
 #include "socketcan_interface_msg/msg/socketcan_if.hpp"
 #include "steered_drive_msg/msg/steered_drive.hpp"
 
@@ -25,10 +27,13 @@ class Controller : public rclcpp::Node {
     rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr _subscription_joy;
 
     void _subscriber_callback_joy(const sensor_msgs::msg::Joy::SharedPtr msg);
+    void publish_nav_cmd(const std::string& command);
 
     rclcpp::Publisher<steered_drive_msg::msg::SteeredDrive>::SharedPtr publisher_vel;
     rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr publisher_restart;
     rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr publisher_autonomous;
+    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_nav_cmd;
+    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr publisher_lane_switch_flag;
 
     rclcpp::QoS _qos = rclcpp::QoS(10);
 
@@ -39,6 +44,13 @@ class Controller : public rclcpp::Node {
 
     utils::UpEdge upedge_share;
     utils::UpEdge upedge_options;
+    utils::UpEdge upedge_cross;
+    utils::UpEdge upedge_l1;
+    utils::UpEdge upedge_r1;
+    utils::UpEdge upedge_l2;
+    utils::DownEdge downedge_l1;
+    utils::DownEdge downedge_r1;
+    utils::DownEdge downedge_l2;
 
     enum class Axes{
         L_x,

controller/src/controller_node.cpp

@@ -22,6 +22,8 @@ steering_max_angle(dtor(get_parameter("steering_max.pos").as_double()))
     publisher_vel = this->create_publisher<steered_drive_msg::msg::SteeredDrive>("cmd_vel", _qos);
     publisher_restart = this->create_publisher<std_msgs::msg::Empty>("restart", _qos);
     publisher_autonomous = this->create_publisher<std_msgs::msg::Bool>("autonomous", _qos);
+    publisher_nav_cmd = this->create_publisher<std_msgs::msg::String>("/planning/nav_cmd", _qos);
+    publisher_lane_switch_flag = this->create_publisher<std_msgs::msg::Empty>("/flag", _qos);
 
     // 駆動系に電源が行っている可能性もあるのでリスタートする
     // publisher_restart->publish(*std::make_shared<std_msgs::msg::Empty>());
@@ -40,6 +42,31 @@ void Controller::_subscriber_callback_joy(const sensor_msgs::msg::Joy::SharedPtr
         publisher_restart->publish(*std::make_shared<std_msgs::msg::Empty>());
         RCLCPP_INFO(this->get_logger(), "再稼働");
     }
+    // 経路選択
+    const bool l1_button = msg->buttons[static_cast<int>(Buttons::L1)];
+    const bool r1_button = msg->buttons[static_cast<int>(Buttons::R1)];
+    const bool l2_button = msg->buttons[static_cast<int>(Buttons::L2)];
+    const bool l1_pressed = upedge_l1(l1_button);
+    const bool r1_pressed = upedge_r1(r1_button);
+    const bool l2_pressed = upedge_l2(l2_button);
+    const bool l1_released = downedge_l1(l1_button);
+    const bool r1_released = downedge_r1(r1_button);
+    const bool l2_released = downedge_l2(l2_button);
+
+    if(l1_pressed || l2_pressed){
+        publish_nav_cmd("left");
+    }
+    if(r1_pressed){
+        publish_nav_cmd("right");
+    }
+    if((l1_released || r1_released || l2_released) && !l1_button && !r1_button && !l2_button){
+        publish_nav_cmd("straight");
+    }
+    // レーン切り替え
+    if(upedge_cross(msg->buttons[static_cast<int>(Buttons::Cross)])){
+        publisher_lane_switch_flag->publish(*std::make_shared<std_msgs::msg::Empty>());
+        RCLCPP_INFO(this->get_logger(), "レーン切り替え");
+    }
     // 手動の場合、速度指令値を送る
     if(!is_autonomous){
         auto msg_vel = std::make_shared<steered_drive_msg::msg::SteeredDrive>();
@@ -50,4 +77,11 @@ void Controller::_subscriber_callback_joy(const sensor_msgs::msg::Joy::SharedPtr
 
 }
 
+void Controller::publish_nav_cmd(const std::string& command){
+    auto msg_nav_cmd = std::make_shared<std_msgs::msg::String>();
+    msg_nav_cmd->data = command;
+    publisher_nav_cmd->publish(*msg_nav_cmd);
+    RCLCPP_INFO(this->get_logger(), "経路選択 : %s", msg_nav_cmd->data.c_str());
+}
+
 }  // namespace controller