diff --git a/gzsitl/CMakeLists.txt b/gzsitl/CMakeLists.txt index e03ca46..3774d67 100644 --- a/gzsitl/CMakeLists.txt +++ b/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}) diff --git a/gzsitl/gzsitl_plugin.cc b/gzsitl/gzsitl_plugin.cc index 6a88bdb..3ddad4b 100644 --- a/gzsitl/gzsitl_plugin.cc +++ b/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("perm_target_ctrl_name"); - perm_target_vis_name = sdf->Get("perm_target_vis_name"); + // Get name for the mission target + mission_target_name = sdf->Get("mission_target_name"); // Get topic names - if (sdf->HasElement("perm_target_topic_name")) { - perm_target_pub_topic_name = - sdf->Get("perm_target_topic_name"); + if (sdf->HasElement("mission_target_topic_name")) { + mission_target_pub_topic_name = + sdf->Get("mission_target_topic_name"); } if (sdf->HasElement("vehicle_topic_name")) { vehicle_pub_topic_name = sdf->Get("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( - "~/" + 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( + "~/" + this->model->GetName() + "/" + mission_target_pub_topic_name, 1, + defaults::GZSITL_MISSION_TARGET_POSE_PUB_FREQ_HZ); this->vehicle_pose_pub = this->node->Advertise( "~/" + this->model->GetName() + "/" + vehicle_pub_topic_name, 1, diff --git a/gzsitl/gzsitl_plugin.hh b/gzsitl/gzsitl_plugin.hh index 41f029f..d5bf140 100644 --- a/gzsitl/gzsitl_plugin.hh +++ b/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 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; }; } diff --git a/gzsitl/gzsitl_visflags_plugin.cc b/gzsitl/gzsitl_visflags_plugin.cc deleted file mode 100644 index 6cf35cd..0000000 --- a/gzsitl/gzsitl_visflags_plugin.cc +++ /dev/null @@ -1,72 +0,0 @@ -/* -// Copyright (c) 2016 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -*/ - -#include "gzsitl_visflags_plugin.hh" - -using namespace gazebo; - -// Register this plugin with the simulator -GZ_REGISTER_VISUAL_PLUGIN(GZSitlVisibilityFlagsPlugin) - -void GZSitlVisibilityFlagsPlugin::Load(rendering::VisualPtr _parent, - sdf::ElementPtr _sdf) -{ - // Store the pointer to the model - this->visual = _parent; - - // Store pointer to the inner parameters list - sdf::ElementPtr plugin_params = _sdf; - - // This block is needed because of an inconsistency with the _sdf argument - // on different calls to Load on Gazebo VisualPlugins - if (_sdf->HasElement("sdf")) { - plugin_params = _sdf->GetElement("sdf"); - } - - bool params_found = false; - - // Find visibility flags - bool vis_all = false; - if (plugin_params->HasElement("gz_visibility_all")) { - vis_all = plugin_params->Get("gz_visibility_all"); - params_found = true; - } - - bool vis_gui = false; - if (plugin_params->HasElement("gz_visibility_gui")) { - vis_gui = plugin_params->Get("gz_visibility_gui"); - params_found = true; - } - - bool vis_sel = false; - if (plugin_params->HasElement("gz_visibility_sel")) { - vis_sel = plugin_params->Get("gz_visibility_sel"); - params_found = true; - } - - // Don't change anything if flags have not been found - if (!params_found) { - return; - } - - int visflags = (vis_all * GZ_VISIBILITY_ALL) | - (vis_sel * GZ_VISIBILITY_SELECTABLE) | - (vis_gui * GZ_VISIBILITY_GUI); - - // Set visibility flags - this->visual->SetVisibilityFlags(visflags); -} - diff --git a/gzsitl/gzsitl_visflags_plugin.hh b/gzsitl/gzsitl_visflags_plugin.hh deleted file mode 100644 index 91d3e10..0000000 --- a/gzsitl/gzsitl_visflags_plugin.hh +++ /dev/null @@ -1,34 +0,0 @@ -/* -// Copyright (c) 2016 Intel Corporation -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -*/ - -#pragma once - -#include -#include -#include - -namespace gazebo -{ -class GZSitlVisibilityFlagsPlugin : public VisualPlugin -{ - public: - void Load(rendering::VisualPtr _parent, sdf::ElementPtr _sdf); - - private: - rendering::VisualPtr visual; -}; -} - diff --git a/models/gzsitl_perm_targ_vis/model.config b/models/gzsitl_mission_target/model.config similarity index 57% rename from models/gzsitl_perm_targ_vis/model.config rename to models/gzsitl_mission_target/model.config index 221a25a..ad12a3f 100644 --- a/models/gzsitl_perm_targ_vis/model.config +++ b/models/gzsitl_mission_target/model.config @@ -1,11 +1,14 @@ - gzsitl_perm_targ_vis + gzsitl_mission_target 1.0 model.sdf - + + Mission target for Gazebo Sitl + + diff --git a/models/gzsitl_perm_targ_ctrl/model.sdf b/models/gzsitl_mission_target/model.sdf similarity index 81% rename from models/gzsitl_perm_targ_ctrl/model.sdf rename to models/gzsitl_mission_target/model.sdf index a5d7d97..3bbc6b9 100644 --- a/models/gzsitl_perm_targ_ctrl/model.sdf +++ b/models/gzsitl_mission_target/model.sdf @@ -1,6 +1,6 @@ - + 0 0 0 0 -0 0 @@ -41,11 +41,6 @@ 1 0.5 - - false - true - true - 1 diff --git a/models/gzsitl_perm_targ_ctrl/model.config b/models/gzsitl_perm_targ_ctrl/model.config deleted file mode 100644 index dca0459..0000000 --- a/models/gzsitl_perm_targ_ctrl/model.config +++ /dev/null @@ -1,11 +0,0 @@ - - - gzsitl_perm_targ_ctrl - 1.0 - model.sdf - - - - - - diff --git a/models/gzsitl_perm_targ_vis/model.sdf b/models/gzsitl_perm_targ_vis/model.sdf deleted file mode 100644 index d34bc89..0000000 --- a/models/gzsitl_perm_targ_vis/model.sdf +++ /dev/null @@ -1,54 +0,0 @@ - - - - - 0 0 0 0 -0 0 - - 0.12017 - - 0.00292634 - 0 - 0 - 0.00292634 - 0 - 0.00292634 - - 0 0 0 0 -0 0 - - 0 - 0 - 1 - - 0 0 0 0 -0 0 - - - 0.18 - - - - 1 - - 0.3 0.3 0.3 1 - 0.7 0.7 0.7 1 - 0.01 0.01 0.01 1 - 0 0 0 1 - - __default__ - - - 1 - 0.2 - - false - true - false - - - - 1 - 1 - - diff --git a/models/gzsitl_quadcopter/model.sdf b/models/gzsitl_quadcopter/model.sdf index fbef490..f3df52d 100644 --- a/models/gzsitl_quadcopter/model.sdf +++ b/models/gzsitl_quadcopter/model.sdf @@ -7,9 +7,8 @@ - gzsitl_perm_targ_ctrl - gzsitl_perm_targ_vis - target_pose + gzsitl_mission_target + target_pose vehicle_pose diff --git a/world/gzsitl_drone_target.world b/world/gzsitl_drone_target.world index 7d53704..7fef484 100644 --- a/world/gzsitl_drone_target.world +++ b/world/gzsitl_drone_target.world @@ -20,9 +20,9 @@ 0 0 0 0 0 0 - + - model://gzsitl_perm_targ_ctrl + model://gzsitl_mission_target true 0 0 1.5 0 0 0