[SPIDAR][WIP] the latest version of real platform

aerial_robot_control/include/aerial_robot_control/control/pose_linear_controller.h

@@ -92,7 +92,7 @@ namespace aerial_robot_control
     virtual void sendCmd();
 
 
-    void cfgPidCallback(aerial_robot_control::PidControlConfig &config, uint32_t level, std::vector<int> controller_indices);
+    virtual void cfgPidCallback(aerial_robot_control::PidControlConfig &config, uint32_t level, std::vector<int> controller_indices);
   };
 
 };

aerial_robot_control/include/aerial_robot_control/flight_navigation.h

@@ -296,7 +296,7 @@ namespace aerial_robot_navigation
 
     virtual void rosParamInit();
     void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg);
-    void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
+    virtual void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg);
     void batteryCheckCallback(const std_msgs::Float32ConstPtr &msg);
 
     virtual void halt() {}
@@ -326,7 +326,7 @@ namespace aerial_robot_navigation
       if(!estimator_->getStateStatus(State::Z_BASE, estimate_mode_))
         {
           ROS_ERROR("Flight Navigation: No correct sensor fusion for z(altitude), can not fly");
-          return;
+          // return;
         }
 
       for(const auto& handler: estimator_->getGpsHandlers())
@@ -403,6 +403,15 @@ namespace aerial_robot_navigation
 
       if(!teleop_flag_) return;
 
+      if(getNaviState() < ARM_ON_STATE) return;
+
+      if(getNaviState() == ARM_ON_STATE)
+        {
+          setNaviState(STOP_STATE);
+          ROS_ERROR("not land, but disarm motors directly");
+          return;
+        }
+
       setNaviState(LAND_STATE);
 
       setTargetXyFromCurrentState();

aerial_robot_control/src/control/pose_linear_controller.cpp

@@ -261,6 +261,8 @@ namespace aerial_robot_control
             pid_controllers_.at(Z).setLimitP(0); // no p control in final safe landing phase
             final_landing_phase = true;
           }
+
+        if(err_z > 0) err_z = 0; // avoid unexpected ascending because of increase in I term
       }
 
     if(navigator_->getForceLandingFlag())

aerial_robot_control/src/flight_navigation.cpp

@@ -406,6 +406,13 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
       if(getNaviState() == LAND_STATE) return;
       if(!teleop_flag_) return; /* can not do the process if other processs are running */
 
+      if(getNaviState() == ARM_ON_STATE)
+        {
+          setNaviState(STOP_STATE);
+          ROS_ERROR("Joy Conrol: not land, but disarm motors directly");
+          return;
+        }
+
       setNaviState(LAND_STATE);
       //update
       setTargetXyFromCurrentState();
@@ -607,11 +614,13 @@ void BaseNavigator::update()
   if(estimator_->getUnhealthLevel() == Sensor::UNHEALTH_LEVEL3 && !force_landing_flag_)
     {
       if(getNaviState() == TAKEOFF_STATE || getNaviState() == HOVER_STATE  || getNaviState() == LAND_STATE)
-        ROS_WARN("Sensor Unhealth Level%d: force landing state", estimator_->getUnhealthLevel());
-      spinal::FlightConfigCmd flight_config_cmd;
-      flight_config_cmd.cmd = spinal::FlightConfigCmd::FORCE_LANDING_CMD;
-      flight_config_pub_.publish(flight_config_cmd);
-      force_landing_flag_ = true;
+        {
+          ROS_WARN("Sensor Unhealth Level%d: force landing state", estimator_->getUnhealthLevel());
+          spinal::FlightConfigCmd flight_config_cmd;
+          flight_config_cmd.cmd = spinal::FlightConfigCmd::FORCE_LANDING_CMD;
+          flight_config_pub_.publish(flight_config_cmd);
+          force_landing_flag_ = true;
+        }
     }
 
   if(getNaviState() == TAKEOFF_STATE || getNaviState() == HOVER_STATE)
@@ -705,24 +714,17 @@ void BaseNavigator::update()
         //for estimator landing mode
         estimator_->setLandingMode(true);
 
-        if (getNaviState() > START_STATE)
-          {
-            if (fabs(delta.z()) > z_convergent_thresh_) convergent_start_time_ = ros::Time::now().toSec();
+        if (fabs(delta.z()) > z_convergent_thresh_) convergent_start_time_ = ros::Time::now().toSec();
 
-            if (ros::Time::now().toSec() - convergent_start_time_ > convergent_duration_)
-              {
-                convergent_start_time_ = ros::Time::now().toSec();
+        if (ros::Time::now().toSec() - convergent_start_time_ > convergent_duration_)
+          {
+            convergent_start_time_ = ros::Time::now().toSec();
 
-                ROS_ERROR("disarm motors");
-                setNaviState(STOP_STATE);
+            ROS_ERROR("disarm motors");
+            setNaviState(STOP_STATE);
 
-                setTargetXyFromCurrentState();
-                setTargetYawFromCurrentState();
-              }
-          }
-        else
-          {
-            setNaviState(ARM_OFF_STATE);
+            setTargetXyFromCurrentState();
+            setTargetYawFromCurrentState();
           }
         break;
       }

aerial_robot_estimation/src/state_estimation.cpp

@@ -88,6 +88,14 @@ void StateEstimator::initialize(ros::NodeHandle nh, ros::NodeHandle nh_private,
 void StateEstimator::statePublish(const ros::TimerEvent & e)
 {
   ros::Time imu_stamp = boost::dynamic_pointer_cast<sensor_plugin::Imu>(imu_handlers_.at(0))->getStamp();
+
+  if (imu_stamp.toSec() == 0)
+    {
+      // use the ros time now
+      imu_stamp = ros::Time::now();
+    }
+
+
   aerial_robot_msgs::States full_state;
   full_state.header.stamp = imu_stamp;
 

aerial_robot_model/include/aerial_robot_model/servo_bridge.h

@@ -123,9 +123,15 @@ class SingleServoHandle
 
   inline void setCurrTorqueVal(const double& val)
   {
-      curr_torque_val_ = torque_scale_ * angle_sgn_ * val;
+    curr_torque_val_ = torque_scale_ * angle_sgn_ * val;
   }
 
+  inline void setTargetTorqueVal(const double& val)
+  {
+    target_torque_val_ = val;
+  }
+
+
   inline void setName(const string& name){ name_ = name; }
   inline void setId(const int& id){ id_ = id; }
   inline void setAngleSgn(const int& sgn){ angle_sgn_ = sgn; }
@@ -162,6 +168,19 @@ class SingleServoHandle
     return curr_torque_val_;
   }
 
+  inline const double getTargetTorqueVal(int value_type) const
+  {
+    if(value_type == ValueType::BIT)
+      {
+        return target_torque_val_ * angle_sgn_ / torque_scale_;
+      }
+    else // Nm
+      {
+        return target_torque_val_;
+      }
+  }
+
+
   inline const string& getName(){return name_; }
   inline const int& getId() const {return id_; }
   inline const int& getAngleSgn() const {return angle_sgn_; }
@@ -175,6 +194,7 @@ class SingleServoHandle
   double curr_angle_val_; // radian
   double target_angle_val_; // radian
   double curr_torque_val_; // Nm
+  double target_torque_val_; // Nm
   int angle_sgn_;
   int zero_point_offset_;
   double angle_scale_;
@@ -201,11 +221,14 @@ class ServoBridge
   ros::Publisher servo_states_pub_;
   map<string, ros::Subscriber> servo_states_subs_;
   map<string, ros::Subscriber> servo_ctrl_subs_;
+  map<string, ros::Subscriber> servo_torque_ctrl_subs_;
+  map<string, ros::Subscriber> servo_torque_subs_;
   map<string, bool> no_real_state_flags_;
-  map<string, ros::Publisher> servo_ctrl_pubs_;
-  map<string, ros::ServiceServer> servo_torque_ctrl_srvs_;
-  map<string, ros::Publisher> servo_torque_ctrl_pubs_;
-  map<string, vector<ros::Publisher> > servo_ctrl_sim_pubs_; // TODO: should be actionlib, trajectory controller
+  map<string, ros::Publisher> servo_target_pos_pubs_;
+  map<string, ros::Publisher> servo_target_torque_pubs_;
+  map<string, ros::ServiceServer> servo_enable_srvs_;
+  map<string, ros::Publisher> servo_enable_pubs_;
+  map<string, vector<ros::Publisher> > servo_target_pos_sim_pubs_; // TODO: should be actionlib, trajectory controller
 
   map<string, ServoGroupHandler> servos_handler_;
   double moving_check_rate_;
@@ -216,7 +239,8 @@ class ServoBridge
 
   void servoStatesCallback(const spinal::ServoStatesConstPtr& state_msg, const std::string& servo_group_name);
   void servoCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
-  bool servoTorqueCtrlCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
+  void servoTorqueCtrlCallback(const sensor_msgs::JointStateConstPtr& joints_ctrl_msg, const std::string& servo_group_name);
+  bool servoEnableCallback(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res, const std::string& servo_group_name);
 
 public:
   ServoBridge(ros::NodeHandle nh, ros::NodeHandle nhp);