なんでそこにnotを入れたぁ！ (#510)

ibis-ssl · Aug 4, 2024 · 96c4e6e · 96c4e6e
1 parent bf8bfb5
commit 96c4e6e
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp
@@ -42,7 +42,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
         }
         break;
       case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE:
-        if (not raw_command.simple_velocity_target_mode.empty()) {
+        if (raw_command.simple_velocity_target_mode.empty()) {
           is_valid = false;
           std::stringstream what;
           what << "The robot " << static_cast<int>(raw_command.robot_id)

diff --git a/crane_sender/include/crane_sender/sim_sender.hpp b/crane_sender/include/crane_sender/sim_sender.hpp
@@ -197,6 +197,13 @@ class SimSenderComponent : public SenderBase
           cmd.set__veltangent(vel_local.x());
           cmd.set__velnormal(vel_local.y());
         } break;
+        case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
+          double vx = command.simple_velocity_target_mode.front().target_vx;
+          double vy = command.simple_velocity_target_mode.front().target_vy;
+          double theta = command.current_pose.theta + omega * delay_s;
+          cmd.set__veltangent(vx * cos(-theta) - vy * sin(-theta));
+          cmd.set__velnormal(vx * sin(-theta) + vy * cos(-theta));
+        } break;
         default:
           std::cout << "Invalid control mode" << std::endl;
           break;