From 8b27d4f9060b428ebfc96fb7fd7584ee68337ec0 Mon Sep 17 00:00:00 2001 From: Liyunong20000 <148530741+Liyunong20000@users.noreply.github.com> Date: Wed, 27 Nov 2024 14:33:16 +0900 Subject: [PATCH 1/8] [control][navigation] Skip following target_pose trajectory during landing (#633) --- .../src/flight_navigation.cpp | 89 ++++++++++--------- 1 file changed, 46 insertions(+), 43 deletions(-) diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index c6cc14f1f..06dd64345 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -686,57 +686,60 @@ void BaseNavigator::update() /* update the target pos and velocity */ if (trajectory_mode_) { - if (traj_generator_ptr_.get() == nullptr) + if(getNaviState() == HOVER_STATE) { - if (ros::Time::now().toSec() > trajectory_reset_time_) + if (traj_generator_ptr_.get() == nullptr) { - setTargetZeroVel(); - setTargetZeroAcc(); + if (ros::Time::now().toSec() > trajectory_reset_time_) + { + setTargetZeroVel(); + setTargetZeroAcc(); - setTargetZeroOmega(); - setTargetZeroAngAcc(); + setTargetZeroOmega(); + setTargetZeroAngAcc(); - trajectory_mode_ = false; + trajectory_mode_ = false; - ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode"); - } - } - else - { - // trajectory following mode - double t = ros::Time::now().toSec(); - double end_t = traj_generator_ptr_->getEndSetpoint().state.t; - if (t > end_t) - { - ROS_INFO("[Nav] reach the end of trajectory"); - - setTargetZeroVel(); - setTargetZeroAcc(); - - setTargetZeroOmega(); - setTargetZeroAngAcc(); - - trajectory_mode_ = false; - - traj_generator_ptr_.reset(); + ROS_INFO("[Flight nav] stop trajectory mode in POS-VEL mode"); + } } else { - agi::QuadState target_state = traj_generator_ptr_->getState(t); - setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2))); - setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2))); - setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2))); - - double target_yaw = target_state.getYaw(); - double target_omega_z = target_state.w(2); - double target_ang_acc_z = target_state.tau(2); - setTargetYaw(target_yaw); - setTargetOmegaZ(target_omega_z); - setTargetAngAccZ(target_ang_acc_z); - - tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_); - double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0]; - ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle); + // trajectory following mode + double t = ros::Time::now().toSec(); + double end_t = traj_generator_ptr_->getEndSetpoint().state.t; + if (t > end_t) + { + ROS_INFO("[Nav] reach the end of trajectory"); + + setTargetZeroVel(); + setTargetZeroAcc(); + + setTargetZeroOmega(); + setTargetZeroAngAcc(); + + trajectory_mode_ = false; + + traj_generator_ptr_.reset(); + } + else + { + agi::QuadState target_state = traj_generator_ptr_->getState(t); + setTargetPos(tf::Vector3(target_state.p(0), target_state.p(1), target_state.p(2))); + setTargetVel(tf::Vector3(target_state.v(0), target_state.v(1), target_state.v(2))); + setTargetAcc(tf::Vector3(target_state.a(0), target_state.a(1), target_state.a(2))); + + double target_yaw = target_state.getYaw(); + double target_omega_z = target_state.w(2); + double target_ang_acc_z = target_state.tau(2); + setTargetYaw(target_yaw); + setTargetOmegaZ(target_omega_z); + setTargetAngAccZ(target_ang_acc_z); + + tf::Vector3 curr_pos = estimator_->getPos(Frame::COG, estimate_mode_); + double yaw_angle = estimator_->getState(State::YAW_COG, estimate_mode_)[0]; + ROS_INFO_THROTTLE(0.5, "[Nav] trajectory mode, target pos&yaw: [%f, %f, %f, %f], curr pos&yaw: [%f, %f, %f, %f]", target_state.p(0), target_state.p(1), target_state.p(2), target_yaw, curr_pos.x(), curr_pos.y(), curr_pos.z(), yaw_angle); + } } } } From 7653ff932a73415b16951f8fffc98cb00986ed3b Mon Sep 17 00:00:00 2001 From: Liyunong20000 <148530741+Liyunong20000@users.noreply.github.com> Date: Wed, 27 Nov 2024 14:33:32 +0900 Subject: [PATCH 2/8] [Navigation][Trajectory] solve the discontinued problem of yaw rotation (#634) --- .../polynomial_trajectory.hpp | 1 + .../src/flight_navigation.cpp | 5 ++++- .../polynomial_trajectory.cpp | 20 ++++++++++++++----- 3 files changed, 20 insertions(+), 6 deletions(-) diff --git a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp index 4f198071d..187f4cb4d 100644 --- a/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp +++ b/aerial_robot_control/include/aerial_robot_control/trajectory/trajectory_reference/polynomial_trajectory.hpp @@ -57,6 +57,7 @@ class PolynomialTrajectory : public ReferenceBase { PolyType y_; PolyType z_; PolyType yaw_; + QuadState prev_constraint_; std::vector states_; diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 06dd64345..42c7c47b6 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -1005,7 +1005,10 @@ void BaseNavigator::generateNewTrajectory(geometry_msgs::PoseStamped pose) } double du_tran = (end_state.p - start_state.p).norm() / trajectory_mean_vel_; - double du_rot = fabs(end_state.getYaw() - start_state.getYaw()) / trajectory_mean_yaw_rate_; + double delta_yaw = end_state.getYaw() - start_state.getYaw(); + if (delta_yaw > M_PI) delta_yaw -= 2 * M_PI; + if (delta_yaw < -M_PI) delta_yaw += 2 * M_PI; + double du_rot = fabs(delta_yaw) / trajectory_mean_yaw_rate_; double du = std::max(du_tran, trajectory_min_du_); if (!enable_latch_yaw_trajectory_) { diff --git a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp index 2b1112ef3..92af636bd 100644 --- a/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp +++ b/aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp @@ -75,12 +75,10 @@ PolynomialTrajectory::PolynomialTrajectory( yaw_.scale(start_state_.t, duration_); addStateConstraint(start_state_); - addStateConstraint(end_state_); - for (size_t i = 1; i < states_.size() - 1; i++) { addStateConstraint(states_.at(i), 0); // only consider the position constraints for the intermediate points } - + addStateConstraint(end_state_); if (!x_.solve()) std::cout << "Could not solve x-axis!" << std::endl; if (!y_.solve()) std::cout << "Could not solve y-axis!" << std::endl; @@ -116,9 +114,21 @@ bool PolynomialTrajectory::addStateConstraint(const QuadState& state, z_.addConstraint(state.t, constraints.row(2).transpose()); // ToDo: Yaw relative to initial yaw. - const Scalar yaw_angle = state.getYaw(); + Scalar yaw_angle = state.getYaw(); if (std::isfinite(yaw_angle)) - yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + { + if (std::isfinite(prev_constraint_.t)) + { + Scalar prev_yaw_angle = prev_constraint_.getYaw(); + Scalar diff = yaw_angle - prev_yaw_angle; + if (diff > M_PI) yaw_angle -= 2 * M_PI; + if (diff < -M_PI) yaw_angle += 2 * M_PI; + } + yaw_.addConstraint(state.t, Vector<3>(yaw_angle, state.w.z(), 0)); + } + + + prev_constraint_ = state; return true; } From d9923e2219f9bc6b621041dc849f4fdd7a1f6c13 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Wed, 27 Nov 2024 14:34:05 +0900 Subject: [PATCH 3/8] [Neuron][Servo][Inialize] introduce a new variable to check the connnection between spinal and neuron (#635) --- .../neuron/neuronlib/Initializer/initializer.cpp | 1 + aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp | 3 +++ aerial_robot_nerve/neuron/neuronlib/Servo/servo.h | 4 ++++ 3 files changed, 8 insertions(+) diff --git a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp index e5425f915..47d7549c5 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Initializer/initializer.cpp @@ -62,6 +62,7 @@ void Initializer::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* case CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST: { sendBoardConfig(); + servo_.setConnect(true); } break; case CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST: diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 63fa3b5cd..5477e84e1 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -11,6 +11,7 @@ void Servo::init(UART_HandleTypeDef* huart, I2C_HandleTypeDef* hi2c, osMutexId* mutex = NULL) { servo_handler_.init(huart, hi2c, mutex); + connect_ = false; } void Servo::update() @@ -35,6 +36,8 @@ void Servo::sendData() void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) { + if (!connect_) return; + switch (message_id) { case CAN::MESSAGEID_RECEIVE_SERVO_ANGLE: { diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h index eabd39e46..9d8162bd4 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h @@ -24,6 +24,9 @@ class Servo : public CANDevice void sendData() override; void receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) override; + bool getConnect() {return connect_;} + void setConnect(bool connect) {connect_ = connect;} + private: struct CANServoData{ int16_t angle; @@ -35,6 +38,7 @@ class Servo : public CANDevice :angle(angle), temperature(temperature), moving(moving), current(current), error(error){} }; + bool connect_; DynamixelSerial servo_handler_; friend class Initializer; }; From 469059f6320ca8cad8947f77021762713050ba5d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Wed, 27 Nov 2024 14:34:42 +0900 Subject: [PATCH 4/8] [Spinal][Neuron][Servo] refactor the torque off rule (#636) * [Servo][Spinal][Neuron] refactor the force servo off rule, which is determined by neuron but not spinal * [Neuron][Dynamixel] add original rule to force switching off servo according to overload error --- .../Servo/Dynamixel/dynamixel_serial.cpp | 26 +++++++++++++++++++ .../Servo/Dynamixel/dynamixel_serial.h | 10 ++++++- .../neuron/neuronlib/Servo/servo.cpp | 11 +++++--- .../neuron/neuronlib/Servo/servo.h | 9 ++++--- .../Hydrus_Lib/CANDevice/servo/can_servo.cpp | 14 +++++++--- .../Hydrus_Lib/CANDevice/servo/can_servo.h | 8 +++--- 6 files changed, 63 insertions(+), 15 deletions(-) diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp index 56ea7ed65..77f097134 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -272,6 +272,32 @@ void DynamixelSerial::update() } else { instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0)); } + + // check the latest error status with original rule + for (unsigned int i = 0; i < servo_num_; i++) { + if (servo_[i].hardware_error_status_ != 0) { + + // ingnore overload error for current based position control + if (servo_[i].hardware_error_status_ == (1 << OVERLOAD_ERROR)) { + if (servo_[i].operating_mode_ == CURRENT_BASE_POSITION_CONTROL_MODE) { + if (servo_[i].goal_current_ < servo_[i].current_limit_) { + servo_[i].force_servo_off_ = false; + continue; + } + } + } + + servo_[i].force_servo_off_ = true; + + if (servo_[i].torque_enable_) { + servo_[i].torque_enable_= false; + setTorque(i); // servo off + } + } + else { + servo_[i].force_servo_off_ = false; + } + } } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h index 50aeff4e3..87a7f77d7 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.h @@ -278,7 +278,14 @@ class RingBuffer class ServoData { public: ServoData(){} - ServoData(uint8_t id): id_(id), torque_enable_(false), first_get_pos_flag_(true), internal_offset_(0){} + ServoData(uint8_t id): + id_(id), + internal_offset_(0), + hardware_error_status_(0), + torque_enable_(false), + force_servo_off_(false), + first_get_pos_flag_(true) + {} uint8_t id_; int32_t present_position_; @@ -301,6 +308,7 @@ class ServoData { float resolution_ratio_; bool led_; bool torque_enable_; + bool force_servo_off_; bool first_get_pos_flag_; void updateHomingOffset() { homing_offset_ = calib_value_ - present_position_;} diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 5477e84e1..116b85126 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -27,6 +27,7 @@ void Servo::sendData() CANServoData data(static_cast(s.getPresentPosition()), s.present_temp_, s.moving_, + s.force_servo_off_, s.present_current_, s.hardware_error_status_); sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast(&data), 1); @@ -51,10 +52,12 @@ void Servo::receiveDataCallback(uint8_t message_id, uint32_t DLC, uint8_t* data) } s.setGoalPosition(goal_pos); bool torque_enable = (((data[i * 2 + 1] >> 7) & 0x01) != 0) ? true : false; - if (s.torque_enable_ != torque_enable) { - s.torque_enable_ = torque_enable; - servo_handler_.setTorque(i); - } + if (!s.force_servo_off_) { + if (s.torque_enable_ != torque_enable) { + s.torque_enable_ = torque_enable; + servo_handler_.setTorque(i); + } + } } break; } diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h index 9d8162bd4..29cba4257 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.h @@ -31,11 +31,14 @@ class Servo : public CANDevice struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; - CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, int16_t current, uint8_t error) - :angle(angle), temperature(temperature), moving(moving), current(current), error(error){} + CANServoData(uint16_t angle, uint8_t temperature, uint8_t moving, bool force_servo_disable, int16_t current, uint8_t error) + :angle(angle), temperature(temperature), current(current), error(error) + { + status = (force_servo_disable? 1 : 0) | moving << 1; + } }; bool connect_; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp index bfaf9b0a1..d0989306c 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.cpp @@ -7,10 +7,11 @@ #include "can_servo.h" -bool Servo::getTorqueEnable() +void Servo::setTorqueEnable(bool torque_enable) { - if(error_ != 0) torque_enable_ = false; - return torque_enable_; + if (force_servo_off_) return; + + torque_enable_ = torque_enable; } void CANServo::sendData() @@ -27,7 +28,12 @@ void CANServo::receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_ CANServoData servo_data = *(reinterpret_cast(data)); servo_[message_id].present_position_ = servo_data.angle; servo_[message_id].present_temperature_ = servo_data.temperature; - servo_[message_id].moving_ = servo_data.moving; + servo_[message_id].moving_ = (servo_data.status >> 1) & 0x01 ; + servo_[message_id].force_servo_off_ = ((servo_data.status & 0x01) != 0)? true: false; servo_[message_id].present_current_ = servo_data.current; servo_[message_id].error_ = servo_data.error; + + if (servo_[message_id].force_servo_off_) { + servo_[message_id].torque_enable_ = false; + } } diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h index 060dbcb77..6f9ff5ec0 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/servo/can_servo.h @@ -26,6 +26,7 @@ class Servo uint8_t error_; bool send_data_flag_; bool torque_enable_; + bool force_servo_off_; bool external_encoder_flag_; uint16_t joint_resolution_; uint16_t servo_resolution_; @@ -50,10 +51,11 @@ class Servo uint8_t getPresentTemperature() const {return present_temperature_;} uint8_t getMoving() const {return moving_;} uint8_t getError() const {return error_;} - bool getTorqueEnable(); + bool getTorqueEnable() const {return torque_enable_;} + bool getForceServoOffFlag() const {return force_servo_off_;} void setIndex(uint8_t index) {index_ = index;} void setGoalPosition(int16_t goal_position) {goal_position_ = goal_position;} - void setTorqueEnable(bool torque_enable) {torque_enable_ = torque_enable;} + void setTorqueEnable(bool torque_enable); }; class CANServo : public CANDevice @@ -73,7 +75,7 @@ class CANServo : public CANDevice struct CANServoData{ int16_t angle; uint8_t temperature; - uint8_t moving; + uint8_t status; int16_t current; uint8_t error; }; From 6e3219bde756195a3a8ed24332de55efbcf617a1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Sun, 1 Dec 2024 22:10:52 +0900 Subject: [PATCH 5/8] Revert "[Neuron][Dynamixel] add original rule to force switching off servo according to overload error" (#639) This reverts commit 75bd14085b608e480dc4d9192fe5c144e4e85aac. --- .../neuronlib/Servo/Dynamixel/dynamixel_serial.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp index 77f097134..4839fee9c 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp @@ -273,20 +273,9 @@ void DynamixelSerial::update() instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0)); } - // check the latest error status with original rule + // check the latest error status for (unsigned int i = 0; i < servo_num_; i++) { if (servo_[i].hardware_error_status_ != 0) { - - // ingnore overload error for current based position control - if (servo_[i].hardware_error_status_ == (1 << OVERLOAD_ERROR)) { - if (servo_[i].operating_mode_ == CURRENT_BASE_POSITION_CONTROL_MODE) { - if (servo_[i].goal_current_ < servo_[i].current_limit_) { - servo_[i].force_servo_off_ = false; - continue; - } - } - } - servo_[i].force_servo_off_ = true; if (servo_[i].torque_enable_) { From ab2ae0db69035dac05ba16adb9c0c7dee2bb7ad9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Mon, 2 Dec 2024 00:17:24 +0900 Subject: [PATCH 6/8] [Spinal][Neuron] refactor the initialization/reinitialization process (#640) * [Neuron][Servo] forbid spinal to set servo torque if connection between spinal and neuron is not established * [Spinal][Neuron][Reboot] recall config reqeust to the reboot neuron to reconect between spinal and neuron --- .../neuron/neuronlib/Servo/servo.cpp | 2 +- .../CANDevice/initializer/can_initializer.cpp | 21 +++++++++++++++++-- .../CANDevice/initializer/can_initializer.h | 6 +++++- .../lib/Hydrus_Lib/Spine/spine.cpp | 2 ++ 4 files changed, 27 insertions(+), 4 deletions(-) diff --git a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp index 116b85126..575c30478 100644 --- a/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp +++ b/aerial_robot_nerve/neuron/neuronlib/Servo/servo.cpp @@ -27,7 +27,7 @@ void Servo::sendData() CANServoData data(static_cast(s.getPresentPosition()), s.present_temp_, s.moving_, - s.force_servo_off_, + !connect_ || s.force_servo_off_, s.present_current_, s.hardware_error_status_); sendMessage(CAN::MESSAGEID_SEND_SERVO_LIST[i], m_slave_id, 8, reinterpret_cast(&data), 1); diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp index c4f1e8af6..7c8816dc6 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.cpp @@ -9,7 +9,16 @@ void CANInitializer::sendData() { - return; + // reconnection with reboot neuron + if (reboot_id_ > 0) { + + // wait for the neuron completely reboot, 5000ms + if (HAL_GetTick() - reboot_time_ > 5000) { + requestConfig(reboot_id_); + reboot_time_ = 0; + reboot_id_ = 0; + } + } } void CANInitializer::initDevices() @@ -18,11 +27,17 @@ void CANInitializer::initDevices() HAL_Delay(200); std::sort(neuron_.begin(), neuron_.end()); for (unsigned int i = 0; i < neuron_.size(); i++) { - sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, neuron_.at(i).getSlaveId(), 0, nullptr, 1); + uint8_t slave_id = neuron_.at(i).getSlaveId(); + requestConfig(slave_id); HAL_Delay(200); } } +void CANInitializer::requestConfig(uint8_t slave_id) +{ + sendMessage(CAN::MESSAGEID_RECEIVE_INITIAL_CONFIG_REQUEST, slave_id, 0, nullptr, 1); +} + void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req) { uint8_t slave_id = static_cast(req.data[0]); @@ -144,6 +159,8 @@ void CANInitializer::configDevice(const spinal::SetBoardConfig::Request& req) uint8_t send_data[1]; send_data[0] = CAN::BOARD_CONFIG_REBOOT; sendMessage(CAN::MESSAGEID_RECEIVE_BOARD_CONFIG_REQUEST, slave_id, 1, send_data, 1); + reboot_id_ = slave_id; + reboot_time_ = HAL_GetTick(); break; } case spinal::SetBoardConfig::Request::SET_DYNAMIXEL_TTL_RS485_MIXED: diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h index 70b17278e..864ad0941 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/CANDevice/initializer/can_initializer.h @@ -18,9 +18,13 @@ class CANInitializer : public CANDevice { private: std::vector& neuron_; + uint8_t reboot_id_; + uint32_t reboot_time_; + public: - CANInitializer(std::vector& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron){} + CANInitializer(std::vector& neuron):CANDevice(CAN::DEVICEID_INITIALIZER, CAN::MASTER_ID), neuron_(neuron), reboot_id_(0), reboot_time_(0){} void initDevices(); + void requestConfig(uint8_t slave_id); void configDevice(const spinal::SetBoardConfig::Request& req); void sendData() override; void receiveDataCallback(uint8_t slave_id, uint8_t message_id, uint32_t DLC, uint8_t* data) override; diff --git a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp index fafcf577f..8410f4ecf 100644 --- a/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp +++ b/aerial_robot_nerve/spinal/mcu_project/lib/Hydrus_Lib/Spine/spine.cpp @@ -228,6 +228,8 @@ namespace Spine if (send_board_index == slave_num_) send_board_index = 0; } } + + can_initializer_.sendData(); // if necessary } void update(void) From 88b7930f3dab534cfd7c06869ef9414094a13d74 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E8=B6=99=E3=80=80=E6=BC=A0=E5=B1=85=28Zhao=2C=20Moju=29?= Date: Fri, 6 Dec 2024 13:54:40 +0900 Subject: [PATCH 7/8] [Mocap][Multicast][IGMP] create a script to force send IGMP report to router periodically (#641) --- aerial_robot_base/bin/install_igmp_report.sh | 23 ++++++ aerial_robot_base/scripts/send_igmp_report.py | 72 +++++++++++++++++++ 2 files changed, 95 insertions(+) create mode 100755 aerial_robot_base/bin/install_igmp_report.sh create mode 100755 aerial_robot_base/scripts/send_igmp_report.py diff --git a/aerial_robot_base/bin/install_igmp_report.sh b/aerial_robot_base/bin/install_igmp_report.sh new file mode 100755 index 000000000..67d303f0e --- /dev/null +++ b/aerial_robot_base/bin/install_igmp_report.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +# Define the path for the temporary file +TEMP_CRON_FILE=$(mktemp) + +# Copy python file to the home directory +FILE_NAME="send_igmp_report.py" +cp `rospack find aerial_robot_base`/scripts/$FILE_NAME $HOME/$FILE_NAME + +# Retrieve the current crontab and save it to the temporary file +sudo crontab -l 2>/dev/null > "$TEMP_CRON_FILE" + +# Add a new job +CRON_COMMAND="*/2 * * * * /usr/bin/python3 $HOME/$FILE_NAME >> /var/log/force_send_GIMP_report.log 2>&1" +grep -qxF "$CRON_COMMAND" "$TEMP_CRON_FILE" || echo "$CRON_COMMAND" >> "$TEMP_CRON_FILE" + +# Update the crontab +sudo crontab "$TEMP_CRON_FILE" + +# Delete the temporary file +rm -f "$TEMP_CRON_FILE" + +echo "Crontab has been updated." diff --git a/aerial_robot_base/scripts/send_igmp_report.py b/aerial_robot_base/scripts/send_igmp_report.py new file mode 100755 index 000000000..0283b8b7c --- /dev/null +++ b/aerial_robot_base/scripts/send_igmp_report.py @@ -0,0 +1,72 @@ +import socket +import struct +import netifaces +import datetime + +def get_wifi_ip(): + # Get the default gateway interface + gateway_interface = netifaces.gateways().get('default') + if gateway_interface: + gateway_interface_name = gateway_interface[2][-1] + + # Get the IP address of the Wi-Fi interface + wifi_ip = netifaces.ifaddresses(gateway_interface_name).get(netifaces.AF_INET) + if wifi_ip: + return wifi_ip[0]['addr'] + return None # Return None if Wi-Fi interface is not found + +def send_igmp_v2_membership_report(group_ip, interface_ip): + # IGMPv2 Membership Report packet format + # Type (1 byte), Max Resp Time (1 byte), Checksum (2 bytes), Group Address (4 bytes) + + IGMP_MEMBERSHIP_REPORT_TYPE = 0x16 # IGMPv2 Membership Report + MAX_RESP_TIME = 0 # Not used in Membership Report + CHECKSUM_PLACEHOLDER = 0 + + # Convert group IP to bytes + group_bytes = socket.inet_aton(group_ip) + + # Create IGMP packet with a placeholder checksum + igmp_packet = struct.pack('!BBH4s', + IGMP_MEMBERSHIP_REPORT_TYPE, + MAX_RESP_TIME, + CHECKSUM_PLACEHOLDER, + group_bytes) + + # Calculate checksum + def calculate_checksum(data): + if len(data) % 2: + data += b'\x00' + checksum = sum(struct.unpack('!%dH' % (len(data) // 2), data)) + checksum = (checksum >> 16) + (checksum & 0xFFFF) + checksum += checksum >> 16 + return ~checksum & 0xFFFF + + checksum = calculate_checksum(igmp_packet) + + # Rebuild the packet with the correct checksum + igmp_packet = struct.pack('!BBH4s', + IGMP_MEMBERSHIP_REPORT_TYPE, + MAX_RESP_TIME, + checksum, + group_bytes) + + # Create a raw socket + sock = socket.socket(socket.AF_INET, socket.SOCK_RAW, socket.IPPROTO_IGMP) + + # Bind to the interface + sock.setsockopt(socket.SOL_IP, socket.IP_MULTICAST_IF, socket.inet_aton(interface_ip)) + + # Send the IGMP packet + sock.sendto(igmp_packet, (group_ip, 0)) + dt_now = datetime.datetime.now() + print("[{}] IGMPv2 Membership Report sent to {} from {}".format(dt_now, group_ip, interface_ip)) + +# Example usage +group_ip = "239.255.42.99" # Multicast group address +interface_ip = get_wifi_ip() # Automatically detect the IP address of the Wi-Fi interface +if interface_ip: + send_igmp_v2_membership_report(group_ip, interface_ip) +else: + dt_now = datetime.datetime.now() + print("[{}] Wi-Fi interface not found.".format(dt_now)) From 255e7f7fac8d86081d86d1370e2fe1e3d3c8cef4 Mon Sep 17 00:00:00 2001 From: Junichiro Sugihara Date: Wed, 18 Dec 2024 17:23:26 +0900 Subject: [PATCH 8/8] [Ninja][URDF] update dock mech inertia. --- robots/ninja/urdf/ninja.urdf.xacro | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/robots/ninja/urdf/ninja.urdf.xacro b/robots/ninja/urdf/ninja.urdf.xacro index ae8ecee73..7a7b9dc2a 100644 --- a/robots/ninja/urdf/ninja.urdf.xacro +++ b/robots/ninja/urdf/ninja.urdf.xacro @@ -123,12 +123,12 @@ - - + + + ixx = "0.00047120" ixy = "-0.00000696" ixz = "-0.00002605" + iyx = "-0.00000696" iyy = "0.00089048" iyz = "0.00000963" + izx = "-0.00002605" izy = "0.00000963" izz = "0.00082655"/> @@ -326,12 +326,12 @@ - - + + + ixx = "0.00035938" ixy = "-0.00001594" ixz = "0.00000012" + iyx = "-0.00001594" iyy = "0.00038919" iyz = "0.00006568" + izx = "0.00000012" izy = "0.00006568" izz = "0.00032884"/>