Skip to content
This repository has been archived by the owner on Aug 5, 2022. It is now read-only.

Try to make gazebo-sitl compatible with Parrot Sphinx Gazebo 7 (libignition-math2.so.2.2.2) #52

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@ include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})

# Find ignition-math
find_package(ignition-math3 REQUIRED)
find_package(ignition-math2 REQUIRED)

# ignition-math Include and Link directories
include_directories(${IGNITION-MATH_INCLUDE_DIRS})
Expand Down
20 changes: 18 additions & 2 deletions gzsitl/gzsitl_plugin.cc
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,11 @@ void GZSitlPlugin::OnUpdate()
this->vehicle_pose_pub->Publish(msgs::Convert(vehicle_pose));

// Get pointer to the permanent target control model if exists
# if GAZEBO_MAJOR_VERSION >= 7
this->perm_target = model->GetWorld()->GetModel(perm_target_name);
# else
this->perm_target = model->GetWorld()->ModelByName(perm_target_name);
# endif
this->perm_target_exists = (bool)this->perm_target;

// Wait for mission target
Expand All @@ -121,16 +125,24 @@ void GZSitlPlugin::OnUpdate()
}

// Retrieve and publish current permanent target pose if target exists
this->perm_target_pose = this->perm_target->WorldPose();
this->perm_target_pose_pub->Publish(msgs::Convert(this->perm_target_pose));
# if GAZEBO_MAJOR_VERSION >= 7
// this->perm_target_pose = this->perm_target->GetWorldPose();
# else
// this->perm_target_pose = this->perm_target->WorldPose();
# endif
// 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 =
# if GAZEBO_MAJOR_VERSION >= 7
model->GetWorld()->GetModel(perm_target_vis_name)) {
# else
model->GetWorld()->ModelByName(perm_target_vis_name)) {
# endif
this->perm_target_vis->SetWorldPose(Pose3d(perm_targ_pos.y,
perm_targ_pos.x, -perm_targ_pos.z, 0, 0, 0));
}
Expand Down Expand Up @@ -249,7 +261,11 @@ void GZSitlPlugin::Load(physics::ModelPtr m, sdf::ElementPtr sdf)

// Setup Publishers
this->node = transport::NodePtr(new transport::Node());
# if GAZEBO_MAJOR_VERSION >= 7
this->node->Init(this->model->GetWorld()->GetName());
# else
this->node->Init(this->model->GetWorld()->Name());
#endif

this->perm_target_pose_pub = this->node->Advertise<msgs::Pose>(
"~/" + this->model->GetName() + "/" + perm_target_pub_topic_name, 1,
Expand Down