Merge remote-tracking branch 'junichiro/develop/Ninja' into develop/N…

…inja

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."

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))

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<QuadState> states_;
 

aerial_robot_control/src/flight_navigation.cpp

@@ -696,57 +696,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);
+                }
             }
         }
     }
@@ -1012,7 +1015,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_)
     {

aerial_robot_control/src/trajectory/trajectory_reference/polynomial_trajectory.cpp

@@ -75,12 +75,10 @@ PolynomialTrajectory<PolyType>::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<PolyType>::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;
 }

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:

aerial_robot_nerve/neuron/neuronlib/Servo/Dynamixel/dynamixel_serial.cpp

@@ -272,6 +272,21 @@ void DynamixelSerial::update()
     } else {
       instruction_buffer_.push(std::make_pair(INST_GET_HARDWARE_ERROR_STATUS, 0));
     }
+
+    // check the latest error status
+    for (unsigned int i = 0; i < servo_num_; i++) {
+      if (servo_[i].hardware_error_status_ != 0) {
+        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;
+      }
+    }
   }
 
 

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_;}

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()
@@ -26,6 +27,7 @@ void Servo::sendData()
 			CANServoData data(static_cast<int16_t>(s.getPresentPosition()),
 							  s.present_temp_,
 							  s.moving_,
+							  !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<uint8_t*>(&data), 1);
@@ -35,6 +37,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:
 		{
@@ -48,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;
 		}

aerial_robot_nerve/neuron/neuronlib/Servo/servo.h

@@ -24,17 +24,24 @@ 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;
 		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_;
 	DynamixelSerial servo_handler_;
 	friend class Initializer;
 };