diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml
index 045e1ccce..f545ae38d 100644
--- a/crane_bringup/launch/crane.launch.xml
+++ b/crane_bringup/launch/crane.launch.xml
@@ -79,6 +79,17 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/crane_sender/CMakeLists.txt b/crane_sender/CMakeLists.txt
index 5cc17aaf7..86dfc0b08 100755
--- a/crane_sender/CMakeLists.txt
+++ b/crane_sender/CMakeLists.txt
@@ -22,6 +22,7 @@ ament_auto_add_library(sim_sender_component SHARED
ament_auto_add_executable(sim_sender_node src/sim_sender_node.cpp)
ament_auto_add_executable(ibis_sender_node src/ibis_sender_node.cpp)
+ament_auto_add_executable(ibis_velocity_sender_node src/ibis_velocity_sender_node.cpp)
if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
diff --git a/crane_sender/src/ibis_velocity_sender_node.cpp b/crane_sender/src/ibis_velocity_sender_node.cpp
new file mode 100644
index 000000000..c6c8a19e1
--- /dev/null
+++ b/crane_sender/src/ibis_velocity_sender_node.cpp
@@ -0,0 +1,321 @@
+// Copyright (c) 2024 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "crane_sender/parameter_with_event.hpp"
+#include "crane_sender/robot_packet.h"
+#include "crane_sender/sender_base.hpp"
+
+namespace crane
+{
+class RobotCommandSender
+{
+public:
+ explicit RobotCommandSender(uint8_t robot_id, bool sim_mode)
+ : io_service(), socket(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::udp::v4(), 0))
+ {
+ boost::asio::ip::udp::resolver resolver(io_service);
+ endpoint = [&]() -> boost::asio::ip::udp::endpoint {
+ if (sim_mode) {
+ std::string host = "localhost";
+ boost::asio::ip::udp::resolver::query query(host, std::to_string(50100 + robot_id));
+ std::cout << "made commander for " << host << ":" << 50100 + robot_id << std::endl;
+ return *resolver.resolve(query);
+ } else {
+ std::string host = "192.168.20." + std::to_string(100 + robot_id);
+ boost::asio::ip::udp::resolver::query query(host, "12345");
+ std::cout << "made commander for " << host << ":12345" << std::endl;
+ return *resolver.resolve(query);
+ }
+ }();
+ }
+
+ RobotCommandSerializedV2 send(RobotCommandV2 packet)
+ {
+ if (++check > 200) {
+ check = 0;
+ }
+
+ packet.check_counter = check;
+ RobotCommandSerializedV2 serialized_packet;
+ RobotCommandSerializedV2_serialize(&serialized_packet, &packet);
+
+ uint8_t send_packet[64] = {};
+ for (int i = 0; i < 64; ++i) {
+ send_packet[i] = serialized_packet.data[i];
+ }
+
+ socket.send_to(boost::asio::buffer(send_packet), endpoint);
+
+ return serialized_packet;
+ }
+
+protected:
+ boost::asio::io_service io_service;
+
+ boost::asio::ip::udp::endpoint endpoint;
+
+ boost::asio::ip::udp::socket socket;
+
+ int check = 0;
+};
+
+class IbisSenderNode : public SenderBase
+{
+private:
+ int debug_id;
+
+ std::shared_ptr parameter_subscriber;
+
+ std::shared_ptr parameter_callback_handle;
+
+ WorldModelWrapper::SharedPtr world_model;
+
+ std::array, 20> senders;
+
+ bool sim_mode;
+
+ rclcpp::Clock clock;
+
+ std::array vx_controllers;
+ std::array vy_controllers;
+
+ ParameterWithEvent p_gain;
+ ParameterWithEvent i_gain;
+ ParameterWithEvent d_gain;
+
+ double I_SATURATION = 0.0;
+
+public:
+ CLASS_LOADER_PUBLIC
+ explicit IbisSenderNode(const rclcpp::NodeOptions & options)
+ : SenderBase("ibis_sender", options),
+ clock(RCL_ROS_TIME),
+ p_gain("p_gain", *this),
+ i_gain("i_gain", *this),
+ d_gain("d_gain", *this)
+ {
+ declare_parameter("debug_id", -1);
+ get_parameter("debug_id", debug_id);
+
+ declare_parameter("p_gain", 4.0);
+ p_gain.value = get_parameter("p_gain").as_double();
+ declare_parameter("i_gain", 0.0);
+ i_gain.value = get_parameter("i_gain").as_double();
+ declare_parameter("d_gain", 0.0);
+ d_gain.value = get_parameter("d_gain").as_double();
+
+ declare_parameter("i_saturation", I_SATURATION);
+ I_SATURATION = get_parameter("i_saturation").as_double();
+
+ declare_parameter("sim_mode", true);
+ get_parameter("sim_mode", sim_mode);
+ parameter_subscriber = std::make_shared(this);
+ parameter_callback_handle =
+ parameter_subscriber->add_parameter_callback("debug_id", [&](const rclcpp::Parameter & p) {
+ if (p.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) {
+ debug_id = p.as_int();
+ } else {
+ std::cout << "debug_id is not integer" << std::endl;
+ }
+ });
+
+ p_gain.callback = [&](double value) {
+ for (auto & controller : vx_controllers) {
+ controller.setGain(value, i_gain.getValue(), d_gain.getValue());
+ }
+ for (auto & controller : vy_controllers) {
+ controller.setGain(value, i_gain.getValue(), d_gain.getValue());
+ }
+ };
+
+ i_gain.callback = [&](double value) {
+ for (auto & controller : vx_controllers) {
+ controller.setGain(p_gain.getValue(), value, d_gain.getValue());
+ }
+ for (auto & controller : vy_controllers) {
+ controller.setGain(p_gain.getValue(), value, d_gain.getValue());
+ }
+ };
+
+ d_gain.callback = [&](double value) {
+ for (auto & controller : vx_controllers) {
+ controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
+ }
+ for (auto & controller : vy_controllers) {
+ controller.setGain(p_gain.getValue(), i_gain.getValue(), value);
+ }
+ };
+
+ for (auto & controller : vx_controllers) {
+ controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
+ }
+
+ for (auto & controller : vy_controllers) {
+ controller.setGain(p_gain.getValue(), i_gain.getValue(), d_gain.getValue(), I_SATURATION);
+ }
+
+ // declare_parameter("interface", "enp4s0");
+ // get_parameter("interface", interface);
+ //
+ // declare_parameter("sim", false);
+ // get_parameter("sim", sim_mode);
+ // if (sim_mode) {
+ // interface = "lo";
+ // }
+
+ for (std::size_t i = 0; i < senders.size(); i++) {
+ senders[i] = std::make_shared(i, sim_mode);
+ }
+
+ world_model = std::make_shared(*this);
+
+ std::cout << "start" << std::endl;
+ }
+
+ void sendCommands(const crane_msgs::msg::RobotCommands & msg) override
+ {
+ if (not world_model->hasUpdated()) {
+ return;
+ }
+
+ auto now = clock.now();
+
+ for (auto command : msg.robot_commands) {
+ RobotCommandV2 packet;
+ packet.header = 0x00;
+ packet.check_counter = 0;
+ packet.vision_global_pos[0] = command.current_pose.x;
+ packet.vision_global_pos[1] = command.current_pose.y;
+ packet.vision_global_theta = command.current_pose.theta;
+ packet.is_vision_available = [&]() -> bool {
+ std::vector available_ids = world_model->ours.getAvailableRobotIds();
+ return std::count(available_ids.begin(), available_ids.end(), command.robot_id) == 1;
+ }();
+ packet.latency_time_ms = current_latency_ms; // TODO(Hans): ちゃんと計測する
+ packet.target_global_theta = command.target_theta;
+ packet.kick_power = [&]() {
+ if (command.chip_enable) {
+ return std::clamp(command.kick_power, 0.f, static_cast(kick_power_limit_chip));
+ } else {
+ return std::clamp(command.kick_power, 0.f, static_cast(kick_power_limit_straight));
+ }
+ }();
+ packet.dribble_power = std::clamp(command.dribble_power, 0.f, 1.f);
+ packet.enable_chip = command.chip_enable;
+ packet.lift_dribbler = command.lift_up_dribbler_flag;
+ packet.stop_emergency = command.stop_flag;
+ packet.speed_limit = command.local_planner_config.max_velocity;
+ packet.omega_limit = command.omega_limit;
+ packet.prioritize_move = true;
+ packet.prioritize_accurate_acceleration = true;
+
+ // auto elapsed_time = now - world_model->getOurRobot(command.robot_id)->detection_stamp;
+ packet.elapsed_time_ms_since_last_vision = 16.0; // elapsed_time.nanoseconds() / 1e6;
+
+ switch (command.control_mode) {
+ case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE: {
+ packet.control_mode = SIMPLE_VELOCITY_TARGET_MODE;
+ if (not command.position_target_mode.empty()) {
+ Velocity vel;
+ vel << vx_controllers[command.robot_id].update(
+ command.position_target_mode.front().target_x - command.current_pose.x, 1.f / 30.f),
+ vy_controllers[command.robot_id].update(
+ command.position_target_mode.front().target_y - command.current_pose.y, 1.f / 30.f);
+ vel += vel.normalized() * command.local_planner_config.terminal_velocity;
+ double max_velocity = command.local_planner_config.max_velocity;
+ double current_velocity =
+ std::hypot(command.current_velocity.x, command.current_velocity.y);
+ max_velocity = std::min(
+ max_velocity, current_velocity + command.local_planner_config.max_acceleration * 0.1);
+ if (vel.norm() > max_velocity) {
+ vel = vel.normalized() * max_velocity;
+ }
+ packet.mode_args.simple_velocity.target_global_vel[0] = vel.x();
+ packet.mode_args.simple_velocity.target_global_vel[1] = vel.y();
+ } else {
+ packet.mode_args.simple_velocity.target_global_vel[0] = 0.0;
+ packet.mode_args.simple_velocity.target_global_vel[1] = 0.0;
+ std::cout << "empty position_target_mode" << std::endl;
+ }
+ } break;
+ case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
+ packet.control_mode = SIMPLE_VELOCITY_TARGET_MODE;
+ if (not command.simple_velocity_target_mode.empty()) {
+ packet.mode_args.simple_velocity.target_global_vel[0] =
+ command.simple_velocity_target_mode.front().target_vx;
+ packet.mode_args.simple_velocity.target_global_vel[1] =
+ command.simple_velocity_target_mode.front().target_vy;
+ } else {
+ packet.mode_args.simple_velocity.target_global_vel[0] = 0.0;
+ packet.mode_args.simple_velocity.target_global_vel[1] = 0.0;
+ std::cout << "empty simple_velocity_target_mode" << std::endl;
+ }
+ } break;
+ case crane_msgs::msg::RobotCommand::LOCAL_CAMERA_MODE: {
+ packet.control_mode = SIMPLE_VELOCITY_TARGET_MODE;
+ if (not command.local_camera_mode.empty()) {
+ double vx = command.local_camera_mode.front().target_global_vx;
+ double vy = command.local_camera_mode.front().target_global_vy;
+ packet.mode_args.simple_velocity.target_global_vel[0] = vx;
+ packet.mode_args.simple_velocity.target_global_vel[1] = vy;
+ } else {
+ packet.mode_args.simple_velocity.target_global_vel[0] = 0.0;
+ packet.mode_args.simple_velocity.target_global_vel[1] = 0.0;
+ std::cout << "empty local_camera_mode" << std::endl;
+ }
+ } break;
+ default:
+ packet.control_mode = SIMPLE_VELOCITY_TARGET_MODE;
+ packet.mode_args.simple_velocity.target_global_vel[0] = 0.0;
+ packet.mode_args.simple_velocity.target_global_vel[1] = 0.0;
+ std::cout << "Invalid control mode" << std::endl;
+ break;
+ }
+ if (command.stop_flag) {
+ packet.mode_args.simple_velocity.target_global_vel[0] = 0.0;
+ packet.mode_args.simple_velocity.target_global_vel[1] = 0.0;
+ }
+ senders[command.robot_id]->send(packet);
+ }
+ }
+};
+} // namespace crane
+
+int main(int argc, char ** argv)
+{
+ rclcpp::init(argc, argv);
+ rclcpp::executors::SingleThreadedExecutor exe;
+ rclcpp::NodeOptions options;
+ std::shared_ptr ibis_sender_node =
+ std::make_shared(options);
+
+ exe.add_node(ibis_sender_node->get_node_base_interface());
+ exe.spin();
+ rclcpp::shutdown();
+ return 0;
+}