[Control] Implemented control topics

itahara-shotaro · Nov 30, 2023 · 3109faf · 3109faf
1 parent 1885c80
commit 3109faf
Show file tree

Hide file tree

Showing 2 changed files with 65 additions and 2 deletions.
diff --git a/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h b/aerial_robot_control/include/aerial_robot_control/control/fully_actuated_nobend_controller.h
@@ -99,7 +99,8 @@ namespace aerial_robot_control
     geometry_msgs::Point CoG;
     boost::mutex pos1_mutex, pos2_mutex, cog_mutex;
     inline void CalculateCoG();
-    void pos1Callback(const geometry_msgs::PoseStamped& msg), pos2Callback(const geometry_msgs::PoseStamped& msg);
+    void pos1Callback(const geometry_msgs::PoseStamped& msg);
+    void pos2Callback(const geometry_msgs::PoseStamped& msg);
 
     // Position PID for new CoG
     boost::shared_ptr<PID> pid_x, pid_y, pid_z;
@@ -131,6 +132,21 @@ namespace aerial_robot_control
     int control_method;
     void rosParamInit();
 
+    // // target angle
+    ros::Subscriber TargetPitch1Subscriber;
+    ros::Subscriber TargetYaw1Subscriber;
+    ros::Subscriber TargetPitch2Subscriber;
+    ros::Subscriber TargetYaw2Subscriber;
+
+    ros::Publisher YawPublisher;
+
+    double TargetPitch1, TargetPitch2, TargetYaw1, TargetYaw2;
+    boost::mutex Pitch1mutex, Pitch2mutex, Yaw1mutex, Yaw2mutex;
+
+    void TargetPitch1Callback(const std_msgs::Float64& msg);
+    void TargetYaw1Callback(const std_msgs::Float64& msg);
+    void TargetPitch2Callback(const std_msgs::Float64& msg);
+    void TargetYaw2Callback(const std_msgs::Float64& msg);
 
   };
 } //namespace aerial_robot_control
diff --git a/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp b/aerial_robot_control/src/control/fully_actuated_nobend_controller.cpp
@@ -96,6 +96,12 @@ namespace aerial_robot_control
     msg.data=robot_id;
     robot_id_pub_.publish(msg);
 
+    TargetPitch1Subscriber = nh_.subscribe("/target_pitch1",1,&FullyActuatedNobendController::TargetPitch1Callback, this);
+    TargetYaw1Subscriber   = nh_.subscribe("/target_yaw1",1,&FullyActuatedNobendController::TargetYaw1Callback, this);
+    TargetPitch2Subscriber = nh_.subscribe("/target_pitch2",1,&FullyActuatedNobendController::TargetPitch2Callback, this);
+    TargetYaw2Subscriber   = nh_.subscribe("/target_yaw2",1,&FullyActuatedNobendController::TargetYaw2Callback, this);
+
+    YawPublisher = nh_.advertise<aerial_robot_msgs::FlightNav>("uav/nav", 1);
   }
 
   inline void FullyActuatedNobendController::CalculateCoG(){
@@ -181,6 +187,47 @@ namespace aerial_robot_control
     return;
   }
 
+  void FullyActuatedNobendController::TargetPitch1Callback(const std_msgs::Float64& msg){
+    boost::lock_guard<boost::mutex> lock(Pitch1mutex);
+    TargetPitch1 = (msg).data;
+    if(robot_id == 1){
+      navigator_->setTargetPitch(TargetPitch1);
+    }
+  }
+
+  void FullyActuatedNobendController::TargetYaw1Callback(const std_msgs::Float64& msg){
+    boost::lock_guard<boost::mutex> lock(Yaw1mutex);
+    TargetYaw1 = (msg).data;
+    if(robot_id == 1){
+      aerial_robot_msgs::FlightNav send_msg;
+      send_msg.pos_xy_nav_mode=0;
+      send_msg.yaw_nav_mode=2;
+      send_msg.pos_z_nav_mode=0;
+      send_msg.target_yaw=TargetYaw1;
+      YawPublisher.publish(send_msg);
+    }
+  }
+
+  void FullyActuatedNobendController::TargetPitch2Callback(const std_msgs::Float64& msg){
+    boost::lock_guard<boost::mutex> lock(Pitch2mutex);
+    if(robot_id == 2){
+      navigator_->setTargetPitch(TargetPitch2);
+    }
+  }
+
+  void FullyActuatedNobendController::TargetYaw2Callback(const std_msgs::Float64& msg){
+    boost::lock_guard<boost::mutex> lock(Yaw2mutex);
+    TargetYaw2 = (msg).data;
+    if(robot_id == 2){
+      aerial_robot_msgs::FlightNav send_msg;
+      send_msg.pos_xy_nav_mode=0;
+      send_msg.yaw_nav_mode=2;
+      send_msg.pos_z_nav_mode=0;
+      send_msg.target_yaw=TargetYaw2;
+      YawPublisher.publish(send_msg);
+    }
+  }
+
   void FullyActuatedNobendController::PIDupdate(){
     vel_ = estimator_->getVel(Frame::COG, estimate_mode_);
     target_pos_ = navigator_->getTargetPos();
@@ -441,7 +488,7 @@ namespace aerial_robot_control
       sr_inv.block(4,6,4,1) = Eigen::Vector4d::Zero(4);
       sr_inv.block(0,7,4,1) = Eigen::Vector4d::Zero(4);
       q_mat_inv_ = 10*sr_inv;
-      // ROS_INFO_STREAM(testaaaa);
+      // ROS_INFO_STREAM(q);
 
       //step3: calculate thrust accounting for softness & linear acc