Rework

gzsitl/CMakeLists.txt

@@ -9,10 +9,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CUSTOM_COMPILE_FLAGS}")
 
 add_library(gzsitl_plugin SHARED gzsitl_plugin.cc)
-add_library(gzsitl_visflags_plugin SHARED gzsitl_visflags_plugin.cc)
 
 target_link_libraries(gzsitl_plugin mavlink_vehicles ${GAZEBO_libraries})
-target_link_libraries(gzsitl_visflags_plugin ${GAZEBO_libraries})
 
 install(TARGETS gzsitl_plugin DESTINATION ${GZSITL_PLUGIN_PATH})
-install(TARGETS gzsitl_visflags_plugin DESTINATION ${GZSITL_PLUGIN_PATH})

gzsitl/gzsitl_plugin.cc

@@ -21,12 +21,8 @@
 
 namespace defaults
 {
-const uint16_t GZSITL_PERM_TARG_POSE_PUB_FREQ_HZ = 50;
+const uint16_t GZSITL_MISSION_TARGET_POSE_PUB_FREQ_HZ = 50;
 const uint16_t GZSITL_VEHICLE_POSE_PUB_FREQ_HZ = 50;
-const double GZSITL_LOOKAT_TARG_ANG_LIMIT = 60.0;
-const double GZSITL_LOOKAT_ROT_ANG_THRESH_DEG = 10.0;
-const double GZSITL_LOOKAT_ROT_SPEED_DEGPS = 90.0;
-const double GZSITL_MIN_ROT_DIST_M = 0.5;
 }
 
 inline double rad2deg(double x)
@@ -111,26 +107,15 @@ void GZSitlPlugin::OnUpdate()
     // Publish current gazebo vehicle pose
     this->vehicle_pose_pub->Publish(msgs::Convert(vehicle_pose));
 
-    // Get pointer to the permanent target control model if exists
-    this->perm_target = model->GetWorld()->ModelByName(perm_target_name);
-    this->perm_target_exists = (bool)this->perm_target;
+    // Get pointer to the mission target model if exists
+    this->mission_target = model->GetWorld()->ModelByName(mission_target_name);
+    this->mission_target_exists = (bool)this->mission_target;
 
-    // Retrieve and publish current permanent target pose if target exists
-    if (this->perm_target_exists) {
-        this->perm_target_pose = this->perm_target->WorldPose();
-        this->perm_target_pose_pub->Publish(
-            msgs::Convert(this->perm_target_pose));
-    }
-
-    // Update permanent target visualization according to the vehicle
-    mavlink_vehicles::local_pos perm_targ_pos =
-        mavlink_vehicles::math::global_to_local_ned(
-            this->mav->get_mission_waypoint(),
-            this->home_position);
-    if (this->perm_target_vis =
-            model->GetWorld()->ModelByName(perm_target_vis_name)) {
-        this->perm_target_vis->SetWorldPose(Pose3d(perm_targ_pos.y,
-                    perm_targ_pos.x, -perm_targ_pos.z, 0, 0, 0));
+    // Retrieve and publish current mission target pose if it exists
+    if (this->mission_target_exists) {
+        this->mission_target_pose = this->mission_target->WorldPose();
+        this->mission_target_pose_pub->Publish(
+            msgs::Convert(this->mission_target_pose));
     }
 
     // Execute according to simulation state
@@ -200,16 +185,16 @@ void GZSitlPlugin::OnUpdate()
 
     case ACTIVE_AIRBORNE: {
 
-        // Send the permanent target to the vehicle
-        if (this->perm_target_pose != this->perm_target_pose_prev) {
+        // Send the mission target to the vehicle
+        if (this->mission_target_pose != this->mission_target_pose_prev) {
             mavlink_vehicles::global_pos_int global_coord =
                 mavlink_vehicles::math::local_ned_to_global(
-                    mavlink_vehicles::local_pos(this->perm_target_pose.Pos()[1],
-                                                this->perm_target_pose.Pos()[0],
-                                                -this->perm_target_pose.Pos()[2]),
+                    mavlink_vehicles::local_pos(this->mission_target_pose.Pos()[1],
+                                                this->mission_target_pose.Pos()[0],
+                                                -this->mission_target_pose.Pos()[2]),
                     this->home_position);
             this->mav->send_mission_waypoint(global_coord, true);
-            this->perm_target_pose_prev = this->perm_target_pose;
+            this->mission_target_pose_prev = this->mission_target_pose;
         }
 
         break;
@@ -232,14 +217,13 @@ void GZSitlPlugin::Load(physics::ModelPtr m, sdf::ElementPtr sdf)
     // Store the model pointer for convenience
     model = m;
 
-    // Get name for the permanent target
-    perm_target_name = sdf->Get<std::string>("perm_target_ctrl_name");
-    perm_target_vis_name = sdf->Get<std::string>("perm_target_vis_name");
+    // Get name for the mission target
+    mission_target_name = sdf->Get<std::string>("mission_target_name");
 
     // Get topic names
-    if (sdf->HasElement("perm_target_topic_name")) {
-        perm_target_pub_topic_name =
-            sdf->Get<std::string>("perm_target_topic_name");
+    if (sdf->HasElement("mission_target_topic_name")) {
+        mission_target_pub_topic_name =
+            sdf->Get<std::string>("mission_target_topic_name");
     }
     if (sdf->HasElement("vehicle_topic_name")) {
         vehicle_pub_topic_name = sdf->Get<std::string>("vehicle_topic_name");
@@ -249,9 +233,9 @@ void GZSitlPlugin::Load(physics::ModelPtr m, sdf::ElementPtr sdf)
     this->node = transport::NodePtr(new transport::Node());
     this->node->Init(this->model->GetWorld()->Name());
 
-    this->perm_target_pose_pub = this->node->Advertise<msgs::Pose>(
-        "~/" + this->model->GetName() + "/" + perm_target_pub_topic_name, 1,
-        defaults::GZSITL_PERM_TARG_POSE_PUB_FREQ_HZ);
+    this->mission_target_pose_pub = this->node->Advertise<msgs::Pose>(
+        "~/" + this->model->GetName() + "/" + mission_target_pub_topic_name, 1,
+        defaults::GZSITL_MISSION_TARGET_POSE_PUB_FREQ_HZ);
 
     this->vehicle_pose_pub = this->node->Advertise<msgs::Pose>(
         "~/" + this->model->GetName() + "/" + vehicle_pub_topic_name, 1,

gzsitl/gzsitl_plugin.hh

@@ -24,7 +24,7 @@
 
 #include "mavlink_vehicles.hh"
 
-#define DEFAULT_PERM_TARGET_POSE_PUB_TOPIC_NAME "target_pose"
+#define DEFAULT_MISSION_TARGET_POSE_PUB_TOPIC_NAME "target_pose"
 #define DEFAULT_VEHICLE_POSE_PUB_TOPIC_NAME "vehicle_pose"
 
 using namespace ignition::math;
@@ -60,9 +60,9 @@ class GAZEBO_VISIBLE GZSitlPlugin : public ModelPlugin
                               mavlink_vehicles::local_pos local_position);
 
     // Target and Target Override
-    bool perm_target_exists = false;
-    Pose3d perm_target_pose = Pose3d::Zero;
-    Pose3d perm_target_pose_prev = Pose3d::Zero;
+    bool mission_target_exists = false;
+    Pose3d mission_target_pose = Pose3d::Zero;
+    Pose3d mission_target_pose_prev = Pose3d::Zero;
 
     // Mavlink
     std::shared_ptr<mavlink_vehicles::mav_vehicle> mav;
@@ -77,18 +77,16 @@ class GAZEBO_VISIBLE GZSitlPlugin : public ModelPlugin
 
     // Gazebo Simulation
     physics::ModelPtr model;
-    std::string perm_target_name;
-    std::string perm_target_vis_name;
-    physics::ModelPtr perm_target;
-    physics::ModelPtr perm_target_vis;
+    std::string mission_target_name;
+    physics::ModelPtr mission_target;
     event::ConnectionPtr update_connection;
 
     // Gazebo Communication
-    std::string perm_target_pub_topic_name =
-        DEFAULT_PERM_TARGET_POSE_PUB_TOPIC_NAME;
+    std::string mission_target_pub_topic_name =
+        DEFAULT_MISSION_TARGET_POSE_PUB_TOPIC_NAME;
     std::string vehicle_pub_topic_name = DEFAULT_VEHICLE_POSE_PUB_TOPIC_NAME;
     transport::NodePtr node;
-    transport::PublisherPtr perm_target_pose_pub;
+    transport::PublisherPtr mission_target_pose_pub;
     transport::PublisherPtr vehicle_pose_pub;
 };
 }

models/gzsitl_perm_targ_vis/model.config → models/gzsitl_mission_target/model.config

@@ -1,11 +1,14 @@
 <?xml version="1.0"?>
 <model>
-  <name>gzsitl_perm_targ_vis</name>
+  <name>gzsitl_mission_target</name>
   <version>1.0</version>
   <sdf version="1.6">model.sdf</sdf>
   <author>
     <name/>
     <email/>
   </author>
-  <description/>
+  <description>
+      Mission target for Gazebo Sitl
+  </description>
 </model>
+

models/gzsitl_perm_targ_ctrl/model.sdf → models/gzsitl_mission_target/model.sdf

@@ -1,6 +1,6 @@
 <?xml version="1.0"?>
 <sdf version="1.6">
-  <model name="gzsitl_perm_targ_ctrl">
+  <model name="gzsitl_mission_target">
     <link name="link">
       <pose frame="">0 0 0 0 -0 0</pose>
       <inertial>
@@ -41,11 +41,6 @@
         </material>
         <cast_shadows>1</cast_shadows>
         <transparency>0.5</transparency>
-        <plugin name="gzsitl_visflags_plugin" filename="libgzsitl_visflags_plugin.so">
-          <gz_visibility_all>false</gz_visibility_all>
-          <gz_visibility_gui>true</gz_visibility_gui>
-          <gz_visibility_sel>true</gz_visibility_sel>
-        </plugin>
       </visual>
     </link>
     <static>1</static>

models/gzsitl_quadcopter/model.sdf

@@ -7,9 +7,8 @@
     </include>
     <!-- Attach the plugin to this model -->
     <plugin name="gzsitl_drone_control" filename="libgzsitl_plugin.so">
-      <perm_target_ctrl_name>gzsitl_perm_targ_ctrl</perm_target_ctrl_name>
-      <perm_target_vis_name>gzsitl_perm_targ_vis</perm_target_vis_name>
-      <perm_target_topic_name>target_pose</perm_target_topic_name>
+      <mission_target_name>gzsitl_mission_target</mission_target_name>
+      <mission_target_topic_name>target_pose</mission_target_topic_name>
       <vehicle_topic_name>vehicle_pose</vehicle_topic_name>
     </plugin>
     <!-- Configuration of the model -->

world/gzsitl_drone_target.world

@@ -20,9 +20,9 @@
       <pose>0 0 0 0 0 0</pose>
     </model>
     <!-- Gzsitl Target -->
-    <model name="gzsitl_perm_targ_ctrl">
+    <model name="gzsitl_mission_target">
       <include>
-        <uri>model://gzsitl_perm_targ_ctrl</uri>
+        <uri>model://gzsitl_mission_target</uri>
       </include>
       <static>true</static>
       <pose>0 0 1.5 0 0 0</pose>