フォーマット

ibis-ssl · Nov 13, 2023 · ea6424a · ea6424a
1 parent 130074d
commit ea6424a
Show file tree

Hide file tree

Showing 3 changed files with 27 additions and 21 deletions.
diff --git a/crane_local_planner/include/crane_local_planner/local_planner.hpp b/crane_local_planner/include/crane_local_planner/local_planner.hpp
@@ -52,6 +52,7 @@ class LocalPlannerComponent : public rclcpp::Node
 
     float error_prev;
   };
+
 public:
   COMPOSITION_PUBLIC
   explicit LocalPlannerComponent(const rclcpp::NodeOptions & options)
@@ -90,11 +91,11 @@ class LocalPlannerComponent : public rclcpp::Node
     declare_parameter("non_rvo_d_gain", NON_RVO_D_GAIN);
     NON_RVO_D_GAIN = get_parameter("non_rvo_d_gain").as_double();
 
-    for(auto & controller : vx_controllers){
+    for (auto & controller : vx_controllers) {
       controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
     }
 
-    for(auto & controller : vy_controllers){
+    for (auto & controller : vy_controllers) {
       controller.setGain(NON_RVO_P_GAIN, NON_RVO_I_GAIN, NON_RVO_D_GAIN);
     }
 

diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp
@@ -221,8 +221,10 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
 
         // 速度に変換する
         Velocity vel;
-        vel << vx_controllers[command.robot_id].update(command.target_x.front() - command.current_pose.x, 1.f/30.f),
-          vy_controllers[command.robot_id].update(command.target_y.front() - command.current_pose.y, 1.f/30.f);
+        vel << vx_controllers[command.robot_id].update(
+          command.target_x.front() - command.current_pose.x, 1.f / 30.f),
+          vy_controllers[command.robot_id].update(
+            command.target_y.front() - command.current_pose.y, 1.f / 30.f);
         vel += vel.normalized() * command.local_planner_config.terminal_velocity;
         if (vel.norm() > max_vel) {
           vel = vel.normalized() * max_vel;
@@ -231,19 +233,19 @@ void LocalPlannerComponent::callbackControlTarget(const crane_msgs::msg::RobotCo
         command.target_velocity.x = vel.x();
         command.target_velocity.y = vel.y();
 
-//　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする
-// そしてこの過ちを再びおかさぬようここに残しておく． R.I.P.
-//        double MAX_THETA_DIFF = max_omega / 30.0f;
-//        // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する
-//        if (not command.target_theta.empty()) {
-//          double theta_diff =
-//            getAngleDiff(command.target_theta.front(), command.current_pose.theta);
-//          if (std::fabs(theta_diff) > MAX_THETA_DIFF) {
-//            theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff);
-//          }
-//
-//          command.target_theta.front() = command.current_pose.theta + theta_diff;
-//        }
+        //　2023/11/12 出力の目標角度制限をしたらVisionの遅れと相まってロボットが角度方向に発振したのでコメントアウトする
+        // そしてこの過ちを再びおかさぬようここに残しておく． R.I.P.
+        //        double MAX_THETA_DIFF = max_omega / 30.0f;
+        //        // 1フレームで変化するthetaの量が大きすぎると急に回転するので制限する
+        //        if (not command.target_theta.empty()) {
+        //          double theta_diff =
+        //            getAngleDiff(command.target_theta.front(), command.current_pose.theta);
+        //          if (std::fabs(theta_diff) > MAX_THETA_DIFF) {
+        //            theta_diff = std::copysign(MAX_THETA_DIFF, theta_diff);
+        //          }
+        //
+        //          command.target_theta.front() = command.current_pose.theta + theta_diff;
+        //        }
 
         command.current_ball_x = world_model->ball.pos.x();
         command.current_ball_y = world_model->ball.pos.y();

diff --git a/crane_sender/src/real_sender_node.cpp b/crane_sender/src/real_sender_node.cpp
@@ -18,14 +18,14 @@
 #include <netinet/udp.h>
 
 #include <class_loader/visibility_control.hpp>
+#include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/robot_commands.hpp>
 #include <iostream>
 #include <memory>
 #include <rclcpp/rclcpp.hpp>
 #include <string>
 #include <vector>
 
-#include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include "crane_sender/sender_base.hpp"
 
 int check;
@@ -46,6 +46,7 @@ class RealSenderNode : public SenderBase
   std::shared_ptr<rclcpp::ParameterCallbackHandle> parameter_callback_handle;
 
   WorldModelWrapper::SharedPtr world_model;
+
 public:
   CLASS_LOADER_PUBLIC
   explicit RealSenderNode(const rclcpp::NodeOptions & options) : SenderBase("real_sender", options)
@@ -69,7 +70,7 @@ class RealSenderNode : public SenderBase
 
   void sendCommands(const crane_msgs::msg::RobotCommands & msg) override
   {
-    if(not world_model->hasUpdated()){
+    if (not world_model->hasUpdated()) {
       return;
     }
     uint8_t send_packet[32] = {};
@@ -196,8 +197,10 @@ class RealSenderNode : public SenderBase
       enable_local_feedback = false;
 
       std::vector<uint8_t> available_ids = world_model->ours.getAvailableRobotIds();
-      bool is_id_available = std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1;
-      std::cout << "id( " << command.robot_id << " ) is available: " << is_id_available << std::endl;
+      bool is_id_available =
+        std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1;
+      std::cout << "id( " << command.robot_id << " ) is available: " << is_id_available
+                << std::endl;
       // キーパーEN
       // 0 or 1