Merge pull request #79 from Jaeyoung-Lim/pr-display-path

Publish mav path

geometric_controller/include/geometric_controller/geometric_controller.h

@@ -49,6 +49,7 @@ class geometricCtrl
     ros::Subscriber yawreferenceSub_;
     ros::Publisher rotorVelPub_, angularVelPub_, target_pose_pub_;
     ros::Publisher referencePosePub_;
+    ros::Publisher posehistoryPub_;
     ros::ServiceClient arming_client_;
     ros::ServiceClient set_mode_client_;
     ros::ServiceServer ctrltriggerServ_;
@@ -74,6 +75,7 @@ class geometricCtrl
     mavros_msgs::CommandBool arm_cmd_;
     mavros_msgs::AttitudeTarget angularVelMsg_;
     geometry_msgs::PoseStamped referencePoseMsg_;
+    std::vector<geometry_msgs::PoseStamped> posehistory_vector_;
 
     Eigen::Vector3d targetPos_, targetVel_, targetAcc_, targetJerk_, targetSnap_, targetPos_prev_, targetVel_prev_;
     Eigen::Vector3d mavPos_, mavVel_, mavRate_;
@@ -86,10 +88,13 @@ class geometricCtrl
     Eigen::Vector3d errorPos_, errorVel_;
     double tau_x, tau_y, tau_z;
     double Kpos_x_, Kpos_y_, Kpos_z_, Kvel_x_, Kvel_y_, Kvel_z_;
+    int posehistory_window_;
 
     void pubMotorCommands();
     void pubRateCommands();
     void pubReferencePose();
+    void pubPoseHistory();
+    void appendPoseHistory();
     void odomCallback(const nav_msgs::OdometryConstPtr& odomMsg);
     void targetCallback(const geometry_msgs::TwistStamped& msg);
     void flattargetCallback(const controller_msgs::FlatTarget& msg);

geometric_controller/launch/config_file.rviz

@@ -10,8 +10,9 @@ Panels:
         - /Pose1
         - /Pose2
         - /Path1
+        - /Path2
       Splitter Ratio: 0.5
-    Tree Height: 775
+    Tree Height: 768
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -105,6 +106,29 @@ Visualization Manager:
       Topic: /trajectory_publisher/trajectory
       Unreliable: false
       Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 0; 0; 255
+      Enabled: true
+      Head Diameter: 0.300000012
+      Head Length: 0.200000003
+      Length: 0.300000012
+      Line Style: Lines
+      Line Width: 0.0299999993
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Radius: 0.0299999993
+      Shaft Diameter: 0.100000001
+      Shaft Length: 0.100000001
+      Topic: /geometric_controller/path
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -156,7 +180,7 @@ Window Geometry:
   Height: 1056
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a00000388fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003a00000388000000c600fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000388fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003a000003880000009e00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000024400fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000038800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -166,5 +190,5 @@ Window Geometry:
   Views:
     collapsed: false
   Width: 1920
-  X: 0
+  X: 65
   Y: 24

geometric_controller/src/geometric_controller.cpp

@@ -28,6 +28,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle& nh, const ros::NodeHandle& n
   angularVelPub_ = nh_.advertise<mavros_msgs::AttitudeTarget>("command/bodyrate_command", 1);
   referencePosePub_ = nh_.advertise<geometry_msgs::PoseStamped>("reference/pose", 1);
   target_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/mavros/setpoint_position/local", 10);
+  posehistoryPub_ = nh_.advertise<nav_msgs::Path>("/geometric_controller/path", 10);
 
   arming_client_ = nh_.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
   set_mode_client_ = nh_.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
@@ -50,6 +51,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle& nh, const ros::NodeHandle& n
   nh_.param<double>("/geometric_controller/Kv_x", Kvel_x_, 1.5);
   nh_.param<double>("/geometric_controller/Kv_y", Kvel_y_, 1.5);
   nh_.param<double>("/geometric_controller/Kv_z", Kvel_z_, 3.3);
+  nh_.param<int>("/geometric_controller/posehistory_window", posehistory_window_, 200);
 
   targetPos_ << 0.0, 0.0, 2.0; //Initial Position
   targetVel_ << 0.0, 0.0, 0.0;
@@ -59,6 +61,7 @@ geometricCtrl::geometricCtrl(const ros::NodeHandle& nh, const ros::NodeHandle& n
   D_ << dx_, dy_, dz_;
 
   tau << tau_x, tau_y, tau_z;
+
 }
 geometricCtrl::~geometricCtrl() {
   //Destructor
@@ -189,6 +192,8 @@ void geometricCtrl::cmdloopCallback(const ros::TimerEvent& event){
     if(!feedthrough_enable_)  computeBodyRateCmd(false);
     pubReferencePose();
     pubRateCommands();
+    appendPoseHistory();
+    pubPoseHistory();
     break;
   case LANDING: {
     geometry_msgs::PoseStamped landingmsg;
@@ -257,6 +262,36 @@ void geometricCtrl::pubRateCommands(){
   angularVelPub_.publish(angularVelMsg_);
 }
 
+void geometricCtrl::pubPoseHistory(){
+  nav_msgs::Path msg;
+  msg.header.stamp = ros::Time::now();
+  msg.header.frame_id = "map";
+  msg.poses = posehistory_vector_;
+  posehistoryPub_.publish(msg);
+}
+
+void geometricCtrl::appendPoseHistory(){
+  posehistory_vector_.insert(posehistory_vector_.begin(), vector3d2PoseStampedMsg(mavPos_, mavAtt_));
+  if(posehistory_vector_.size() > posehistory_window_){
+    posehistory_vector_.pop_back();
+  }
+}
+
+geometry_msgs::PoseStamped geometricCtrl::vector3d2PoseStampedMsg(Eigen::Vector3d &position, Eigen::Vector4d &orientation){
+  geometry_msgs::PoseStamped encode_msg;
+  encode_msg.header.stamp = ros::Time::now();
+  encode_msg.header.frame_id = "map";
+  encode_msg.pose.orientation.w = orientation(0);
+  encode_msg.pose.orientation.x = orientation(1);
+  encode_msg.pose.orientation.y = orientation(2);
+  encode_msg.pose.orientation.z = orientation(3);
+  encode_msg.pose.position.x = position(0);
+  encode_msg.pose.position.y = position(1);
+  encode_msg.pose.position.z = position(2);
+  return encode_msg;
+}
+
+
 void geometricCtrl::computeBodyRateCmd(bool ctrl_mode){
   Eigen::Matrix3d R_ref;
 
@@ -401,4 +436,4 @@ void geometricCtrl::setBodyRateCommand(Eigen::Vector4d bodyrate_command){
 void geometricCtrl::setFeedthrough(bool feed_through){
   feedthrough_enable_ = feed_through;
 
-}
+}