aah98
/
XTDrone
forked from xtdrone/XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Push robocup map, add cars and terrorists

This commit is contained in:
QiuMingZS 2020-09-09 20:45:11 +08:00
parent b1ee747ef1
commit 4ae99e5621
14 changed files with 7808 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
actor_related/Actor.proto Normal file
View File

@ -0,0 +1,9 @@
syntax = "proto2";
package mav_msgs.msgs;
import "vector3d.proto";
message Actor
{
required gazebo.msgs.Vector3d actor_pose = 1;
}

256
actor_related/ActorPlugin.cpp Normal file
View File

@ -0,0 +1,256 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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 <functional>
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "ActorPlugin.h"
#include "common.h"
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin)
#define WALKING_ANIMATION "walking"
int flag1 = 0;
int count_flag1 = 0;
/////////////////////////////////////////////////
ActorPlugin::~ActorPlugin()
{
update_connection_->~Connection();
}
/////////////////////////////////////////////////
void ActorPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->sdf = _sdf;
this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
this->world = this->actor->GetWorld();
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
std::bind(&ActorPlugin::OnUpdate, this, std::placeholders::_1)));
this->Reset();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[ActorPlugin] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&ActorPlugin::OnUpdate, this, _1));
//Subscribe the command actor pose
cmd_pose_sub_ = node_handle_->Subscribe<mav_msgs::msgs::CmdActor>("cmd_actor_pose", &ActorPlugin::CmdPoseCallback, this);
//Publish the command actor pose
cmd_pose_pub_ = node_handle_->Advertise<mav_msgs::msgs::CmdActor>("cmd_actor_pose", 1);
// FIXME: Commented out to prevent warnings about queue limit reached.
actor_pose_pub_ = node_handle_->Advertise<mav_msgs::msgs::Actor>("actor_pose", 1);
}
void ActorPlugin::CmdPoseCallback(CmdActorPtr &cmd_msg)
{
target[0] = cmd_msg->cmd_actor_pose_x();
target[1] = cmd_msg->cmd_actor_pose_y();
target[2] = cmd_msg->cmd_actor_pose_z();
//target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl;
}
/////////////////////////////////////////////////
void ActorPlugin::Reset()
{
this->lastUpdate = 0;
/*
if (this->sdf && this->sdf->HasElement("target"))
this->target = this->sdf->Get<ignition::math::Vector3d>("target");
else
*/
this->target = ignition::math::Vector3d(0, 0, 1.0191);
auto skelAnims = this->actor->SkeletonAnimations();
if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
{
gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
}
else
{
// Create custom trajectory
this->trajectoryInfo.reset(new physics::TrajectoryInfo());
this->trajectoryInfo->type = WALKING_ANIMATION;
this->trajectoryInfo->duration = 1.0;
this->actor->SetCustomTrajectory(this->trajectoryInfo);
}
}
/////////////////////////////////////////////////
void ActorPlugin::ChooseNewTarget()
{
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
{
double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
- newTarget).Length();
if (dist < 2.0)
{
newTarget = this->target;
break;
}
}
}
this->target = newTarget;
}
/////////////////////////////////////////////////
void ActorPlugin::OnUpdate(const common::UpdateInfo &_info)
{
this->velocity = 0.8;
// Time delta
double dt = (_info.simTime - this->lastUpdate).Double();
ignition::math::Pose3d pose = this->actor->WorldPose();
ignition::math::Vector3d pos = this->target - pose.Pos();
ignition::math::Vector3d rpy = pose.Rot().Euler();
double distance = pos.Length();
// Choose a new target position if the actor has reached its current target.
if (distance < 0.01)
{
// FIXME: Commented out to prevent swerve after actor reached its target.
//this->ChooseNewTarget();
pos = this->target - pose.Pos();
}
// Choose a suitable velocity at different distance.
if (distance > 1.0)
{
this->velocity = velocity / distance;
}
else if (distance <= 1.0)
{
this->velocity = velocity;
}
// Compute the yaw orientation.
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
yaw.Normalize();
// Rotate in place, instead of jumping.
if (std::abs(yaw.Radian()) > IGN_DTOR(10))
{
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian()*0.001);
}
else
{
pose.Pos() += pos * this->velocity * dt;
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
}
if (flag1 == 0)
{
pose.Pos().X(0);
pose.Pos().Y(0);
pose.Pos().Z(1.0191);
cmd_pose_msg.set_cmd_actor_pose_x(0);
cmd_pose_msg.set_cmd_actor_pose_y(0);
count_flag1 = 0;
}
else
{
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);
}
// Distance traveled is used to coordinate motion with the walking
// animation
double distanceTraveled = (pose.Pos() -
this->actor->WorldPose().Pos()).Length();
this->actor->SetWorldPose(pose, false, false);
this->actor->SetScriptTime(this->actor->ScriptTime() +
(distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
gazebo::msgs::Vector3d* actor_pose = new gazebo::msgs::Vector3d();
actor_pose->set_x(pose.Pos().X());
actor_pose->set_y(pose.Pos().Y());
actor_pose->set_z(pose.Pos().Z());
// FIXME: Commented out to prevent warnings about queue limit reached.
pose_msg.set_allocated_actor_pose(actor_pose);
actor_pose_pub_->Publish(pose_msg);
if ((abs(target[0]-pose.Pos().X())<0.1)&&(abs(target[1]-pose.Pos().Y())<0.1))
{
if (count_flag1 == 0)
{
flag1 ++;
}
count_flag1 = 1;
if (flag1 ==5)
flag1 = 1;
}
if (flag1 == 1)
{
cmd_pose_msg.set_cmd_actor_pose_x(10);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag1 = 0;
}
if (flag1 == 2)
{
cmd_pose_msg.set_cmd_actor_pose_x(10);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag1 = 0;
}
if (flag1 == 3)
{
cmd_pose_msg.set_cmd_actor_pose_x(-10);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag1 = 0;
}
if (flag1 == 4)
{
cmd_pose_msg.set_cmd_actor_pose_x(-10);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag1 = 0;
}
cmd_pose_msg.set_cmd_actor_pose_z(1.0191);
cmd_pose_pub_->Publish(cmd_pose_msg);
//std::cout << "[XTDrone_Actor_Plugin]: Publish topic actor_pose_pub"<< std::endl;
//std::cout << "Target_Position: " << target[0] << "," << target[1] << "," << target[2] << endl;
//std::cout << "Actor_Position: " << dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << endl;
}

136
actor_related/ActorPlugin.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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.
*
*/
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN_HH_
#include <string>
#include <vector>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/util/system.hh"
#include "gazebo/msgs/msgs.hh"
#include <gazebo/gazebo.hh>
#include "Actor.pb.h"
#include "CmdActor.pb.h"
namespace gazebo
{
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static const std::string kDefaultLinkName = "base_link";
typedef const boost::shared_ptr<const mav_msgs::msgs::CmdActor> CmdActorPtr;
class GZ_PLUGIN_VISIBLE ActorPlugin : public ModelPlugin
{
/// \brief Constructor
public: ActorPlugin()
: ModelPlugin(),
namespace_(kDefaultNamespace),
frame_id_(kDefaultFrameId),
link_name_(kDefaultLinkName),
node_handle_(NULL) {}
virtual ~ActorPlugin();
/// \brief Load the actor plugin.
/// \param[in] _model Pointer to the parent model.
/// \param[in] _sdf Pointer to the plugin's SDF elements.
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
// Documentation Inherited.
public: virtual void Reset();
/// \brief Function that is called every update cycle.
/// \param[in] _info Timing information
private: void OnUpdate(const common::UpdateInfo &_info);
/// \brief Helper function to choose a new target location
private: void ChooseNewTarget();
/// \brief Helper function to avoid obstacles. This implements a very
/// simple vector-field algorithm.
/// \param[in] _pos Direction vector that should be adjusted according
/// to nearby obstacles.
//private: void HandleObstacles(ignition::math::Vector3d &_pos);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::ModelPtr model_;
physics::LinkPtr link_;
std::string namespace_;
std::string frame_id_;
std::string link_name_;
transport::NodePtr node_handle_;
transport::PublisherPtr actor_pose_pub_;
transport::SubscriberPtr cmd_pose_sub_;
transport::PublisherPtr cmd_pose_pub_;
mav_msgs::msgs::Actor pose_msg;
mav_msgs::msgs::CmdActor cmd_pose_msg;
/// \callback function of command actor pose
void CmdPoseCallback(CmdActorPtr &cmd_msg);
/// \brief Pointer to the parent actor.
private: physics::ActorPtr actor;
/// \brief Pointer to the world, for convenience.
private: physics::WorldPtr world;
/// \brief Pointer to the sdf element.
private: sdf::ElementPtr sdf;
/// \brief Velocity of the actor
private: ignition::math::Vector3d velocity;
/// \brief List of connections
private: std::vector<event::ConnectionPtr> connections;
/// \brief Current target location
private: ignition::math::Vector3d target;
/// \brief Target location weight (used for vector field)
//private: double targetWeight = 1.0;
/// \brief Obstacle weight (used for vector field)
//private: double obstacleWeight = 1.0;
/// \brief Time scaling factor. Used to coordinate translational motion
/// with the actor's walking animation.
//private: double animationFactor = 1.0;
/// \brief Time of the last update.
private: common::Time lastUpdate;
/// \brief List of models to ignore. Used for vector field
private: std::vector<std::string> ignoreModels;
/// \brief Custom trajectory info.
private: physics::TrajectoryInfoPtr trajectoryInfo;
};
}
#endif

257
actor_related/ActorPlugin2.cpp Normal file
View File

@ -0,0 +1,257 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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 <functional>
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "ActorPlugin2.h"
#include "common.h"
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin2)
#define WALKING_ANIMATION "walking2"
int flag2 = 0;
int count_flag2 = 0;
/////////////////////////////////////////////////
ActorPlugin2::~ActorPlugin2()
{
update_connection_->~Connection();
}
/////////////////////////////////////////////////
void ActorPlugin2::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->sdf = _sdf;
this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
this->world = this->actor->GetWorld();
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
std::bind(&ActorPlugin2::OnUpdate, this, std::placeholders::_1)));
this->Reset();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[ActorPlugin2] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&ActorPlugin2::OnUpdate, this, _1));
//Subscribe the command actor pose
cmd_pose_sub_2 = node_handle_->Subscribe<mav_msgs::msgs::CmdActor>("cmd_actor_pose2", &ActorPlugin2::CmdPoseCallback, this);
//Publish the command actor pose
cmd_pose_pub_2 = node_handle_->Advertise<mav_msgs::msgs::CmdActor>("cmd_actor_pose2", 1);
// FIXME: Commented out to prevent warnings about queue limit reached.
actor_pose_pub_2 = node_handle_->Advertise<mav_msgs::msgs::Actor>("actor_pose2", 1);
}
void ActorPlugin2::CmdPoseCallback(CmdActorPtr &cmd_msg)
{
target[0] = cmd_msg->cmd_actor_pose_x();
target[1] = cmd_msg->cmd_actor_pose_y();
target[2] = cmd_msg->cmd_actor_pose_z();
//target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl;
}
/////////////////////////////////////////////////
void ActorPlugin2::Reset()
{
this->lastUpdate = 0;
/*
if (this->sdf && this->sdf->HasElement("target"))
this->target = this->sdf->Get<ignition::math::Vector3d>("target");
else
*/
this->target = ignition::math::Vector3d(0, 0, 1.0191);
auto skelAnims = this->actor->SkeletonAnimations();
if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
{
gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
}
else
{
// Create custom trajectory
this->trajectoryInfo.reset(new physics::TrajectoryInfo());
this->trajectoryInfo->type = WALKING_ANIMATION;
this->trajectoryInfo->duration = 1.0;
this->actor->SetCustomTrajectory(this->trajectoryInfo);
}
}
/////////////////////////////////////////////////
void ActorPlugin2::ChooseNewTarget()
{
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
{
double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
- newTarget).Length();
if (dist < 2.0)
{
newTarget = this->target;
break;
}
}
}
this->target = newTarget;
}
/////////////////////////////////////////////////
void ActorPlugin2::OnUpdate(const common::UpdateInfo &_info)
{
this->velocity = 0.8;
// Time delta
double dt = (_info.simTime - this->lastUpdate).Double();
ignition::math::Pose3d pose = this->actor->WorldPose();
ignition::math::Vector3d pos = this->target - pose.Pos();
ignition::math::Vector3d rpy = pose.Rot().Euler();
double distance = pos.Length();
// Choose a new target position if the actor has reached its current target.
if (distance < 0.01)
{
// FIXME: Commented out to prevent swerve after actor reached its target.
//this->ChooseNewTarget();
pos = this->target - pose.Pos();
}
// Choose a suitable velocity at different distance.
if (distance > 1.0)
{
this->velocity = velocity / distance;
}
else if (distance <= 1.0)
{
this->velocity = velocity;
}
// Compute the yaw orientation.
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
yaw.Normalize();
// Rotate in place, instead of jumping.
if (std::abs(yaw.Radian()) > IGN_DTOR(10))
{
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian()*0.001);
}
else
{
pose.Pos() += pos * this->velocity * dt;
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
}
if (flag2 == 0)
{
pose.Pos().X(-30);
pose.Pos().Y(-30);
pose.Pos().Z(1.0191);
cmd_pose_msg.set_cmd_actor_pose_x(-30);
cmd_pose_msg.set_cmd_actor_pose_y(-30);
count_flag2 = 0;
}
else
{
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);
}
// Distance traveled is used to coordinate motion with the walking
// animation
double distanceTraveled = (pose.Pos() -
this->actor->WorldPose().Pos()).Length();
//std::cout << "this->actor->WorldPose().Pos():" << this->actor->WorldPose().Pos().X() << ";" << this->actor->WorldPose().Pos().Y();
this->actor->SetWorldPose(pose, false, false);
this->actor->SetScriptTime(this->actor->ScriptTime() + (distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
gazebo::msgs::Vector3d* actor_pose = new gazebo::msgs::Vector3d();
actor_pose->set_x(pose.Pos().X());
actor_pose->set_y(pose.Pos().Y());
actor_pose->set_z(pose.Pos().Z());
// FIXME: Commented out to prevent warnings about queue limit reached.
pose_msg.set_allocated_actor_pose(actor_pose);
actor_pose_pub_2->Publish(pose_msg);
if ((abs(target[0]-pose.Pos().X())<0.1)&&(abs(target[1]-pose.Pos().Y())<0.1))
{
if (count_flag2 == 0)
{
flag2 ++;
}
count_flag2 = 1;
if (flag2 ==5)
flag2 = 1;
}
if (flag2 == 1)
{
cmd_pose_msg.set_cmd_actor_pose_x(-45);
cmd_pose_msg.set_cmd_actor_pose_y(-45);
count_flag2 = 0;
}
if (flag2 == 2)
{
cmd_pose_msg.set_cmd_actor_pose_x(-45);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag2 = 0;
}
if (flag2 == 3)
{
cmd_pose_msg.set_cmd_actor_pose_x(-15);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag2 = 0;
}
if (flag2 == 4)
{
cmd_pose_msg.set_cmd_actor_pose_x(-15);
cmd_pose_msg.set_cmd_actor_pose_y(-45);
count_flag2 = 0;
}
cmd_pose_msg.set_cmd_actor_pose_z(1.0191);
cmd_pose_pub_2->Publish(cmd_pose_msg);
//std::cout << "[XTDrone_Actor_Plugin2]: Publish topic actor_pose_pub2"<< std::endl;
//std::cout << "Target_Position2: " << target[0] << "," << target[1] << "," << target[2] << endl;
//std::cout << "Actor_Position2: " << dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << endl;
}

136
actor_related/ActorPlugin2.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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.
*
*/
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN2_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN2_HH_
#include <string>
#include <vector>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/util/system.hh"
#include "gazebo/msgs/msgs.hh"
#include <gazebo/gazebo.hh>
#include "Actor.pb.h"
#include "CmdActor.pb.h"
namespace gazebo
{
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static const std::string kDefaultLinkName = "base_link";
typedef const boost::shared_ptr<const mav_msgs::msgs::CmdActor> CmdActorPtr;
class GZ_PLUGIN_VISIBLE ActorPlugin2 : public ModelPlugin
{
/// \brief Constructor
public: ActorPlugin2()
: ModelPlugin(),
namespace_(kDefaultNamespace),
frame_id_(kDefaultFrameId),
link_name_(kDefaultLinkName),
node_handle_(NULL) {}
virtual ~ActorPlugin2();
/// \brief Load the actor plugin.
/// \param[in] _model Pointer to the parent model.
/// \param[in] _sdf Pointer to the plugin's SDF elements.
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
// Documentation Inherited.
public: virtual void Reset();
/// \brief Function that is called every update cycle.
/// \param[in] _info Timing information
private: void OnUpdate(const common::UpdateInfo &_info);
/// \brief Helper function to choose a new target location
private: void ChooseNewTarget();
/// \brief Helper function to avoid obstacles. This implements a very
/// simple vector-field algorithm.
/// \param[in] _pos Direction vector that should be adjusted according
/// to nearby obstacles.
//private: void HandleObstacles(ignition::math::Vector3d &_pos);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::ModelPtr model_;
physics::LinkPtr link_;
std::string namespace_;
std::string frame_id_;
std::string link_name_;
transport::NodePtr node_handle_;
transport::PublisherPtr actor_pose_pub_2;
transport::SubscriberPtr cmd_pose_sub_2;
transport::PublisherPtr cmd_pose_pub_2;
mav_msgs::msgs::Actor pose_msg;
mav_msgs::msgs::CmdActor cmd_pose_msg;
/// \callback function of command actor pose
void CmdPoseCallback(CmdActorPtr &cmd_msg);
/// \brief Pointer to the parent actor.
private: physics::ActorPtr actor;
/// \brief Pointer to the world, for convenience.
private: physics::WorldPtr world;
/// \brief Pointer to the sdf element.
private: sdf::ElementPtr sdf;
/// \brief Velocity of the actor
private: ignition::math::Vector3d velocity;
/// \brief List of connections
private: std::vector<event::ConnectionPtr> connections;
/// \brief Current target location
private: ignition::math::Vector3d target;
/// \brief Target location weight (used for vector field)
//private: double targetWeight = 1.0;
/// \brief Obstacle weight (used for vector field)
//private: double obstacleWeight = 1.0;
/// \brief Time scaling factor. Used to coordinate translational motion
/// with the actor's walking animation.
//private: double animationFactor = 1.0;
/// \brief Time of the last update.
private: common::Time lastUpdate;
/// \brief List of models to ignore. Used for vector field
private: std::vector<std::string> ignoreModels;
/// \brief Custom trajectory info.
private: physics::TrajectoryInfoPtr trajectoryInfo;
};
}
#endif

258
actor_related/ActorPlugin3.cpp Normal file
View File

@ -0,0 +1,258 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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 <functional>
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "ActorPlugin3.h"
#include "common.h"
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin3)
#define WALKING_ANIMATION "walking3"
int flag3 = 0;
int count_flag3 = 0;
/////////////////////////////////////////////////
ActorPlugin3::~ActorPlugin3()
{
update_connection_->~Connection();
}
/////////////////////////////////////////////////
void ActorPlugin3::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->sdf = _sdf;
this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
this->world = this->actor->GetWorld();
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
std::bind(&ActorPlugin3::OnUpdate, this, std::placeholders::_1)));
this->Reset();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[ActorPlugin3] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&ActorPlugin3::OnUpdate, this, _1));
//Subscribe the command actor pose
cmd_pose_sub_3 = node_handle_->Subscribe<mav_msgs::msgs::CmdActor>("cmd_actor_pose3", &ActorPlugin3::CmdPoseCallback, this);
//Publish the command actor pose
cmd_pose_pub_3 = node_handle_->Advertise<mav_msgs::msgs::CmdActor>("cmd_actor_pose3", 1);
// FIXME: Commented out to prevent warnings about queue limit reached.
actor_pose_pub_3 = node_handle_->Advertise<mav_msgs::msgs::Actor>("actor_pose3", 1);
}
void ActorPlugin3::CmdPoseCallback(CmdActorPtr &cmd_msg)
{
target[0] = cmd_msg->cmd_actor_pose_x();
target[1] = cmd_msg->cmd_actor_pose_y();
target[2] = cmd_msg->cmd_actor_pose_z();
//target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl;
}
/////////////////////////////////////////////////
void ActorPlugin3::Reset()
{
this->lastUpdate = 0;
/*
if (this->sdf && this->sdf->HasElement("target"))
this->target = this->sdf->Get<ignition::math::Vector3d>("target");
else
*/
this->target = ignition::math::Vector3d(0, 0, 1.0191);
auto skelAnims = this->actor->SkeletonAnimations();
if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
{
gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
}
else
{
// Create custom trajectory
this->trajectoryInfo.reset(new physics::TrajectoryInfo());
this->trajectoryInfo->type = WALKING_ANIMATION;
this->trajectoryInfo->duration = 1.0;
this->actor->SetCustomTrajectory(this->trajectoryInfo);
}
}
/////////////////////////////////////////////////
void ActorPlugin3::ChooseNewTarget()
{
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
{
double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
- newTarget).Length();
if (dist < 2.0)
{
newTarget = this->target;
break;
}
}
}
this->target = newTarget;
}
/////////////////////////////////////////////////
void ActorPlugin3::OnUpdate(const common::UpdateInfo &_info)
{
this->velocity = 0.8;
// Time delta
double dt = (_info.simTime - this->lastUpdate).Double();
ignition::math::Pose3d pose = this->actor->WorldPose();
ignition::math::Vector3d pos = this->target - pose.Pos();
ignition::math::Vector3d rpy = pose.Rot().Euler();
double distance = pos.Length();
// Choose a new target position if the actor has reached its current target.
if (distance < 0.01)
{
// FIXME: Commented out to prevent swerve after actor reached its target.
//this->ChooseNewTarget();
pos = this->target - pose.Pos();
}
// Choose a suitable velocity at different distance.
if (distance > 1.0)
{
this->velocity = velocity / distance;
}
else if (distance <= 1.0)
{
this->velocity = velocity;
}
// Compute the yaw orientation.
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
yaw.Normalize();
// Rotate in place, instead of jumping.
if (std::abs(yaw.Radian()) > IGN_DTOR(10))
{
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian()*0.001);
}
else
{
pose.Pos() += pos * this->velocity * dt;
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
}
if (flag3 == 0)
{
pose.Pos().X(-30);
pose.Pos().Y(30);
pose.Pos().Z(1.0191);
cmd_pose_msg.set_cmd_actor_pose_x(-30);
cmd_pose_msg.set_cmd_actor_pose_y(30);
count_flag3 = 0;
}
else
{
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);
}
// Distance traveled is used to coordinate motion with the walking
// animation
double distanceTraveled = (pose.Pos() -
this->actor->WorldPose().Pos()).Length();
//std::cout << "this->actor->WorldPose().Pos():" << this->actor->WorldPose().Pos().X() << ";" << this->actor->WorldPose().Pos().Y();
this->actor->SetWorldPose(pose, false, false);
this->actor->SetScriptTime(this->actor->ScriptTime() + (distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
gazebo::msgs::Vector3d* actor_pose = new gazebo::msgs::Vector3d();
actor_pose->set_x(pose.Pos().X());
actor_pose->set_y(pose.Pos().Y());
actor_pose->set_z(pose.Pos().Z());
// FIXME: Commented out to prevent warnings about queue limit reached.
pose_msg.set_allocated_actor_pose(actor_pose);
actor_pose_pub_3->Publish(pose_msg);
if ((abs(target[0]-pose.Pos().X())<0.1)&&(abs(target[1]-pose.Pos().Y())<0.1))
{
if (count_flag3 == 0)
{
flag3 ++;
}
count_flag3 = 1;
if (flag3 ==5)
flag3 = 1;
}
if (flag3 == 1)
{
cmd_pose_msg.set_cmd_actor_pose_x(-15);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag3 = 0;
}
if (flag3 == 2)
{
cmd_pose_msg.set_cmd_actor_pose_x(-15);
cmd_pose_msg.set_cmd_actor_pose_y(45);
count_flag3 = 0;
}
if (flag3 == 3)
{
cmd_pose_msg.set_cmd_actor_pose_x(-45);
cmd_pose_msg.set_cmd_actor_pose_y(45);
count_flag3 = 0;
}
if (flag3 == 4)
{
cmd_pose_msg.set_cmd_actor_pose_x(-45);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag3 = 0;
}
cmd_pose_msg.set_cmd_actor_pose_z(1.0191);
cmd_pose_pub_3->Publish(cmd_pose_msg);
//std::cout << "[XTDrone_Actor_Plugin3]: Publish topic actor_pose_pub3"<< std::endl;
//std::cout << "Target_Position3: " << target[0] << "," << target[1] << "," << target[2] << endl;
//std::cout << "Actor_Position3: " << dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << endl;
}

136
actor_related/ActorPlugin3.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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.
*
*/
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN3_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN3_HH_
#include <string>
#include <vector>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/util/system.hh"
#include "gazebo/msgs/msgs.hh"
#include <gazebo/gazebo.hh>
#include "Actor.pb.h"
#include "CmdActor.pb.h"
namespace gazebo
{
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static const std::string kDefaultLinkName = "base_link";
typedef const boost::shared_ptr<const mav_msgs::msgs::CmdActor> CmdActorPtr;
class GZ_PLUGIN_VISIBLE ActorPlugin3 : public ModelPlugin
{
/// \brief Constructor
public: ActorPlugin3()
: ModelPlugin(),
namespace_(kDefaultNamespace),
frame_id_(kDefaultFrameId),
link_name_(kDefaultLinkName),
node_handle_(NULL) {}
virtual ~ActorPlugin3();
/// \brief Load the actor plugin.
/// \param[in] _model Pointer to the parent model.
/// \param[in] _sdf Pointer to the plugin's SDF elements.
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
// Documentation Inherited.
public: virtual void Reset();
/// \brief Function that is called every update cycle.
/// \param[in] _info Timing information
private: void OnUpdate(const common::UpdateInfo &_info);
/// \brief Helper function to choose a new target location
private: void ChooseNewTarget();
/// \brief Helper function to avoid obstacles. This implements a very
/// simple vector-field algorithm.
/// \param[in] _pos Direction vector that should be adjusted according
/// to nearby obstacles.
//private: void HandleObstacles(ignition::math::Vector3d &_pos);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::ModelPtr model_;
physics::LinkPtr link_;
std::string namespace_;
std::string frame_id_;
std::string link_name_;
transport::NodePtr node_handle_;
transport::PublisherPtr actor_pose_pub_3;
transport::SubscriberPtr cmd_pose_sub_3;
transport::PublisherPtr cmd_pose_pub_3;
mav_msgs::msgs::Actor pose_msg;
mav_msgs::msgs::CmdActor cmd_pose_msg;
/// \callback function of command actor pose
void CmdPoseCallback(CmdActorPtr &cmd_msg);
/// \brief Pointer to the parent actor.
private: physics::ActorPtr actor;
/// \brief Pointer to the world, for convenience.
private: physics::WorldPtr world;
/// \brief Pointer to the sdf element.
private: sdf::ElementPtr sdf;
/// \brief Velocity of the actor
private: ignition::math::Vector3d velocity;
/// \brief List of connections
private: std::vector<event::ConnectionPtr> connections;
/// \brief Current target location
private: ignition::math::Vector3d target;
/// \brief Target location weight (used for vector field)
//private: double targetWeight = 1.0;
/// \brief Obstacle weight (used for vector field)
//private: double obstacleWeight = 1.0;
/// \brief Time scaling factor. Used to coordinate translational motion
/// with the actor's walking animation.
//private: double animationFactor = 1.0;
/// \brief Time of the last update.
private: common::Time lastUpdate;
/// \brief List of models to ignore. Used for vector field
private: std::vector<std::string> ignoreModels;
/// \brief Custom trajectory info.
private: physics::TrajectoryInfoPtr trajectoryInfo;
};
}
#endif

256
actor_related/ActorPlugin4.cpp Normal file
View File

@ -0,0 +1,256 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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 <functional>
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "ActorPlugin4.h"
#include "common.h"
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin4)
#define WALKING_ANIMATION "walking4"
int flag4 = 0;
int count_flag4 = 0;
/////////////////////////////////////////////////
ActorPlugin4::~ActorPlugin4()
{
update_connection_->~Connection();
}
/////////////////////////////////////////////////
void ActorPlugin4::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->sdf = _sdf;
this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
this->world = this->actor->GetWorld();
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
std::bind(&ActorPlugin4::OnUpdate, this, std::placeholders::_1)));
this->Reset();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[ActorPlugin4] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&ActorPlugin4::OnUpdate, this, _1));
//Subscribe the command actor pose
cmd_pose_sub_4 = node_handle_->Subscribe<mav_msgs::msgs::CmdActor>("cmd_actor_pose4", &ActorPlugin4::CmdPoseCallback, this);
//Publish the command actor pose
cmd_pose_pub_4 = node_handle_->Advertise<mav_msgs::msgs::CmdActor>("cmd_actor_pose4", 1);
// FIXME: Commented out to prevent warnings about queue limit reached.
actor_pose_pub_4 = node_handle_->Advertise<mav_msgs::msgs::Actor>("actor_pose4", 1);
}
void ActorPlugin4::CmdPoseCallback(CmdActorPtr &cmd_msg)
{
target[0] = cmd_msg->cmd_actor_pose_x();
target[1] = cmd_msg->cmd_actor_pose_y();
target[2] = cmd_msg->cmd_actor_pose_z();
//target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl;
}
/////////////////////////////////////////////////
void ActorPlugin4::Reset()
{
this->lastUpdate = 0;
/*
if (this->sdf && this->sdf->HasElement("target"))
this->target = this->sdf->Get<ignition::math::Vector3d>("target");
else
*/
this->target = ignition::math::Vector3d(0, 0, 1.0191);
auto skelAnims = this->actor->SkeletonAnimations();
if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
{
gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
}
else
{
// Create custom trajectory
this->trajectoryInfo.reset(new physics::TrajectoryInfo());
this->trajectoryInfo->type = WALKING_ANIMATION;
this->trajectoryInfo->duration = 1.0;
this->actor->SetCustomTrajectory(this->trajectoryInfo);
}
}
/////////////////////////////////////////////////
void ActorPlugin4::ChooseNewTarget()
{
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
{
double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
- newTarget).Length();
if (dist < 2.0)
{
newTarget = this->target;
break;
}
}
}
this->target = newTarget;
}
/////////////////////////////////////////////////
void ActorPlugin4::OnUpdate(const common::UpdateInfo &_info)
{
this->velocity = 0.8;
// Time delta
double dt = (_info.simTime - this->lastUpdate).Double();
ignition::math::Pose3d pose = this->actor->WorldPose();
ignition::math::Vector3d pos = this->target - pose.Pos();
ignition::math::Vector3d rpy = pose.Rot().Euler();
double distance = pos.Length();
// Choose a new target position if the actor has reached its current target.
if (distance < 0.01)
{
// FIXME: Commented out to prevent swerve after actor reached its target.
//this->ChooseNewTarget();
pos = this->target - pose.Pos();
}
// Choose a suitable velocity at different distance.
if (distance > 1.0)
{
this->velocity = velocity / distance;
}
else if (distance <= 1.0)
{
this->velocity = velocity;
}
// Compute the yaw orientation.
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
yaw.Normalize();
// Rotate in place, instead of jumping.
if (std::abs(yaw.Radian()) > IGN_DTOR(10))
{
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian()*0.001);
}
else
{
pose.Pos() += pos * this->velocity * dt;
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
}
if (flag4 == 0)
{
pose.Pos().X(80);
pose.Pos().Y(-30);
pose.Pos().Z(1.0191);
cmd_pose_msg.set_cmd_actor_pose_x(80);
cmd_pose_msg.set_cmd_actor_pose_y(-30);
count_flag4 = 0;
}
else
{
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);
}
// Distance traveled is used to coordinate motion with the walking
// animation
double distanceTraveled = (pose.Pos() -
this->actor->WorldPose().Pos()).Length();
//std::cout << "this->actor->WorldPose().Pos():" << this->actor->WorldPose().Pos().X() << ";" << this->actor->WorldPose().Pos().Y();
this->actor->SetWorldPose(pose, false, false);
this->actor->SetScriptTime(this->actor->ScriptTime() + (distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
gazebo::msgs::Vector3d* actor_pose = new gazebo::msgs::Vector3d();
actor_pose->set_x(pose.Pos().X());
actor_pose->set_y(pose.Pos().Y());
actor_pose->set_z(pose.Pos().Z());
// FIXME: Commented out to prevent warnings about queue limit reached.
pose_msg.set_allocated_actor_pose(actor_pose);
actor_pose_pub_4->Publish(pose_msg);
if ((abs(target[0]-pose.Pos().X())<0.1)&&(abs(target[1]-pose.Pos().Y())<0.1))
{
if (count_flag4 == 0)
{
flag4 ++;
}
count_flag4 = 1;
if (flag4 ==5)
flag4 = 1;
}
if (flag4 == 1)
{
cmd_pose_msg.set_cmd_actor_pose_x(65);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag4 = 0;
}
if (flag4 == 2)
{
cmd_pose_msg.set_cmd_actor_pose_x(65);
cmd_pose_msg.set_cmd_actor_pose_y(-45);
count_flag4 = 0;
}
if (flag4 == 3)
{
cmd_pose_msg.set_cmd_actor_pose_x(95);
cmd_pose_msg.set_cmd_actor_pose_y(-45);
count_flag4 = 0;
}
if (flag4 == 4)
{
cmd_pose_msg.set_cmd_actor_pose_x(95);
cmd_pose_msg.set_cmd_actor_pose_y(-15);
count_flag4 = 0;
}
cmd_pose_msg.set_cmd_actor_pose_z(1.0191);
cmd_pose_pub_4->Publish(cmd_pose_msg);
//std::cout << "[XTDrone_Actor_Plugin4]: Publish topic actor_pose_pub4"<< std::endl;
//std::cout << "Target_Position4: " << target[0] << "," << target[1] << "," << target[2] << endl;
//std::cout << "Actor_Position4: " << dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << endl;
}

136
actor_related/ActorPlugin4.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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.
*
*/
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN4_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN4_HH_
#include <string>
#include <vector>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/util/system.hh"
#include "gazebo/msgs/msgs.hh"
#include <gazebo/gazebo.hh>
#include "Actor.pb.h"
#include "CmdActor.pb.h"
namespace gazebo
{
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static const std::string kDefaultLinkName = "base_link";
typedef const boost::shared_ptr<const mav_msgs::msgs::CmdActor> CmdActorPtr;
class GZ_PLUGIN_VISIBLE ActorPlugin4 : public ModelPlugin
{
/// \brief Constructor
public: ActorPlugin4()
: ModelPlugin(),
namespace_(kDefaultNamespace),
frame_id_(kDefaultFrameId),
link_name_(kDefaultLinkName),
node_handle_(NULL) {}
virtual ~ActorPlugin4();
/// \brief Load the actor plugin.
/// \param[in] _model Pointer to the parent model.
/// \param[in] _sdf Pointer to the plugin's SDF elements.
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
// Documentation Inherited.
public: virtual void Reset();
/// \brief Function that is called every update cycle.
/// \param[in] _info Timing information
private: void OnUpdate(const common::UpdateInfo &_info);
/// \brief Helper function to choose a new target location
private: void ChooseNewTarget();
/// \brief Helper function to avoid obstacles. This implements a very
/// simple vector-field algorithm.
/// \param[in] _pos Direction vector that should be adjusted according
/// to nearby obstacles.
//private: void HandleObstacles(ignition::math::Vector3d &_pos);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::ModelPtr model_;
physics::LinkPtr link_;
std::string namespace_;
std::string frame_id_;
std::string link_name_;
transport::NodePtr node_handle_;
transport::PublisherPtr actor_pose_pub_4;
transport::SubscriberPtr cmd_pose_sub_4;
transport::PublisherPtr cmd_pose_pub_4;
mav_msgs::msgs::Actor pose_msg;
mav_msgs::msgs::CmdActor cmd_pose_msg;
/// \callback function of command actor pose
void CmdPoseCallback(CmdActorPtr &cmd_msg);
/// \brief Pointer to the parent actor.
private: physics::ActorPtr actor;
/// \brief Pointer to the world, for convenience.
private: physics::WorldPtr world;
/// \brief Pointer to the sdf element.
private: sdf::ElementPtr sdf;
/// \brief Velocity of the actor
private: ignition::math::Vector3d velocity;
/// \brief List of connections
private: std::vector<event::ConnectionPtr> connections;
/// \brief Current target location
private: ignition::math::Vector3d target;
/// \brief Target location weight (used for vector field)
//private: double targetWeight = 1.0;
/// \brief Obstacle weight (used for vector field)
//private: double obstacleWeight = 1.0;
/// \brief Time scaling factor. Used to coordinate translational motion
/// with the actor's walking animation.
//private: double animationFactor = 1.0;
/// \brief Time of the last update.
private: common::Time lastUpdate;
/// \brief List of models to ignore. Used for vector field
private: std::vector<std::string> ignoreModels;
/// \brief Custom trajectory info.
private: physics::TrajectoryInfoPtr trajectoryInfo;
};
}
#endif

256
actor_related/ActorPlugin5.cpp Normal file
View File

@ -0,0 +1,256 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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 <functional>
#include <ignition/math.hh>
#include "gazebo/physics/physics.hh"
#include "ActorPlugin5.h"
#include "common.h"
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(ActorPlugin5)
#define WALKING_ANIMATION "walking5"
int flag5 = 0;
int count_flag5 = 0;
/////////////////////////////////////////////////
ActorPlugin5::~ActorPlugin5()
{
update_connection_->~Connection();
}
/////////////////////////////////////////////////
void ActorPlugin5::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
this->sdf = _sdf;
this->actor = boost::dynamic_pointer_cast<physics::Actor>(_model);
this->world = this->actor->GetWorld();
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
std::bind(&ActorPlugin5::OnUpdate, this, std::placeholders::_1)));
this->Reset();
if (_sdf->HasElement("robotNamespace"))
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
else
gzerr << "[ActorPlugin5] Please specify a robotNamespace.\n";
node_handle_ = transport::NodePtr(new transport::Node());
node_handle_->Init(namespace_);
// Listen to the update event. This event is broadcast every
// simulation iteration.
update_connection_ = event::Events::ConnectWorldUpdateBegin(boost::bind(&ActorPlugin5::OnUpdate, this, _1));
//Subscribe the command actor pose
cmd_pose_sub_5 = node_handle_->Subscribe<mav_msgs::msgs::CmdActor>("cmd_actor_pose5", &ActorPlugin5::CmdPoseCallback, this);
//Publish the command actor pose
cmd_pose_pub_5 = node_handle_->Advertise<mav_msgs::msgs::CmdActor>("cmd_actor_pose5", 1);
// FIXME: Commented out to prevent warnings about queue limit reached.
actor_pose_pub_5 = node_handle_->Advertise<mav_msgs::msgs::Actor>("actor_pose5", 1);
}
void ActorPlugin5::CmdPoseCallback(CmdActorPtr &cmd_msg)
{
target[0] = cmd_msg->cmd_actor_pose_x();
target[1] = cmd_msg->cmd_actor_pose_y();
target[2] = cmd_msg->cmd_actor_pose_z();
//target = ignition::math::Vector3d(10, 10, 1.0191);
//std::cout << "I'm here!" << endl;
}
/////////////////////////////////////////////////
void ActorPlugin5::Reset()
{
this->lastUpdate = 0;
/*
if (this->sdf && this->sdf->HasElement("target"))
this->target = this->sdf->Get<ignition::math::Vector3d>("target");
else
*/
this->target = ignition::math::Vector3d(0, 0, 1.0191);
auto skelAnims = this->actor->SkeletonAnimations();
if (skelAnims.find(WALKING_ANIMATION) == skelAnims.end())
{
gzerr << "Skeleton animation " << WALKING_ANIMATION << " not found.\n";
}
else
{
// Create custom trajectory
this->trajectoryInfo.reset(new physics::TrajectoryInfo());
this->trajectoryInfo->type = WALKING_ANIMATION;
this->trajectoryInfo->duration = 1.0;
this->actor->SetCustomTrajectory(this->trajectoryInfo);
}
}
/////////////////////////////////////////////////
void ActorPlugin5::ChooseNewTarget()
{
ignition::math::Vector3d newTarget(this->target);
while ((newTarget - this->target).Length() < 2.0)
{
newTarget.X(ignition::math::Rand::DblUniform(-50, 100));
newTarget.Y(ignition::math::Rand::DblUniform(-50, 50));
for (unsigned int i = 0; i < this->world->ModelCount(); ++i)
{
double dist = (this->world->ModelByIndex(i)->WorldPose().Pos()
- newTarget).Length();
if (dist < 2.0)
{
newTarget = this->target;
break;
}
}
}
this->target = newTarget;
}
/////////////////////////////////////////////////
void ActorPlugin5::OnUpdate(const common::UpdateInfo &_info)
{
this->velocity = 0.8;
// Time delta
double dt = (_info.simTime - this->lastUpdate).Double();
ignition::math::Pose3d pose = this->actor->WorldPose();
ignition::math::Vector3d pos = this->target - pose.Pos();
ignition::math::Vector3d rpy = pose.Rot().Euler();
double distance = pos.Length();
// Choose a new target position if the actor has reached its current target.
if (distance < 0.01)
{
// FIXME: Commented out to prevent swerve after actor reached its target.
//this->ChooseNewTarget();
pos = this->target - pose.Pos();
}
// Choose a suitable velocity at different distance.
if (distance > 1.0)
{
this->velocity = velocity / distance;
}
else if (distance <= 1.0)
{
this->velocity = velocity;
}
// Compute the yaw orientation.
ignition::math::Angle yaw = atan2(pos.Y(), pos.X()) + 1.5707 - rpy.Z();
yaw.Normalize();
// Rotate in place, instead of jumping.
if (std::abs(yaw.Radian()) > IGN_DTOR(10))
{
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z() + yaw.Radian()*0.001);
}
else
{
pose.Pos() += pos * this->velocity * dt;
pose.Rot() = ignition::math::Quaterniond(1.5707, 0, rpy.Z()+yaw.Radian());
}
if (flag5 == 0)
{
pose.Pos().X(80);
pose.Pos().Y(30);
pose.Pos().Z(1.0191);
cmd_pose_msg.set_cmd_actor_pose_x(80);
cmd_pose_msg.set_cmd_actor_pose_y(30);
count_flag5 = 0;
}
else
{
// Make sure the actor stays within bounds
pose.Pos().X(std::max(-50.0, std::min(100.0, pose.Pos().X())));
pose.Pos().Y(std::max(-50.0, std::min(50.0, pose.Pos().Y())));
pose.Pos().Z(1.0191);
}
// Distance traveled is used to coordinate motion with the walking
// animation
double distanceTraveled = (pose.Pos() -
this->actor->WorldPose().Pos()).Length();
//std::cout << "this->actor->WorldPose().Pos():" << this->actor->WorldPose().Pos().X() << ";" << this->actor->WorldPose().Pos().Y();
this->actor->SetWorldPose(pose, false, false);
this->actor->SetScriptTime(this->actor->ScriptTime() + (distanceTraveled * 5.0));
this->lastUpdate = _info.simTime;
gazebo::msgs::Vector3d* actor_pose = new gazebo::msgs::Vector3d();
actor_pose->set_x(pose.Pos().X());
actor_pose->set_y(pose.Pos().Y());
actor_pose->set_z(pose.Pos().Z());
// FIXME: Commented out to prevent warnings about queue limit reached.
pose_msg.set_allocated_actor_pose(actor_pose);
actor_pose_pub_5->Publish(pose_msg);
if ((abs(target[0]-pose.Pos().X())<0.1)&&(abs(target[1]-pose.Pos().Y())<0.1))
{
if (count_flag5 == 0)
{
flag5 ++;
}
count_flag5 = 1;
if (flag5 ==5)
flag5 = 1;
}
if (flag5 == 1)
{
cmd_pose_msg.set_cmd_actor_pose_x(95);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag5 = 0;
}
if (flag5 == 2)
{
cmd_pose_msg.set_cmd_actor_pose_x(95);
cmd_pose_msg.set_cmd_actor_pose_y(45);
count_flag5 = 0;
}
if (flag5 == 3)
{
cmd_pose_msg.set_cmd_actor_pose_x(65);
cmd_pose_msg.set_cmd_actor_pose_y(45);
count_flag5 = 0;
}
if (flag5 == 4)
{
cmd_pose_msg.set_cmd_actor_pose_x(65);
cmd_pose_msg.set_cmd_actor_pose_y(15);
count_flag5 = 0;
}
cmd_pose_msg.set_cmd_actor_pose_z(1.0191);
cmd_pose_pub_5->Publish(cmd_pose_msg);
//std::cout << "[XTDrone_Actor_Plugin5]: Publish topic actor_pose_pub5"<< std::endl;
//std::cout << "Target_Position5: " << target[0] << "," << target[1] << "," << target[2] << endl;
//std::cout << "Actor_Position5: " << dec << pose.Pos().X() << "," << pose.Pos().Y() << "," << pose.Pos().Z() << endl;
}

136
actor_related/ActorPlugin5.h Normal file
View File

@ -0,0 +1,136 @@
/*
* Copyright (C) 2016 Open Source Robotics Foundation
*
* 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.
*
*/
#ifndef GAZEBO_PLUGINS_ACTORPLUGIN5_HH_
#define GAZEBO_PLUGINS_ACTORPLUGIN5_HH_
#include <string>
#include <vector>
#include <random>
#include <gazebo/common/common.hh>
#include <gazebo/common/Plugin.hh>
#include <gazebo/physics/physics.hh>
#include "gazebo/util/system.hh"
#include "gazebo/msgs/msgs.hh"
#include <gazebo/gazebo.hh>
#include "Actor.pb.h"
#include "CmdActor.pb.h"
namespace gazebo
{
// Default values
static const std::string kDefaultNamespace = "";
static const std::string kDefaultFrameId = "world";
static const std::string kDefaultLinkName = "base_link";
typedef const boost::shared_ptr<const mav_msgs::msgs::CmdActor> CmdActorPtr;
class GZ_PLUGIN_VISIBLE ActorPlugin5 : public ModelPlugin
{
/// \brief Constructor
public: ActorPlugin5()
: ModelPlugin(),
namespace_(kDefaultNamespace),
frame_id_(kDefaultFrameId),
link_name_(kDefaultLinkName),
node_handle_(NULL) {}
virtual ~ActorPlugin5();
/// \brief Load the actor plugin.
/// \param[in] _model Pointer to the parent model.
/// \param[in] _sdf Pointer to the plugin's SDF elements.
public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
// Documentation Inherited.
public: virtual void Reset();
/// \brief Function that is called every update cycle.
/// \param[in] _info Timing information
private: void OnUpdate(const common::UpdateInfo &_info);
/// \brief Helper function to choose a new target location
private: void ChooseNewTarget();
/// \brief Helper function to avoid obstacles. This implements a very
/// simple vector-field algorithm.
/// \param[in] _pos Direction vector that should be adjusted according
/// to nearby obstacles.
//private: void HandleObstacles(ignition::math::Vector3d &_pos);
private:
/// \brief Pointer to the update event connection.
event::ConnectionPtr update_connection_;
physics::ModelPtr model_;
physics::LinkPtr link_;
std::string namespace_;
std::string frame_id_;
std::string link_name_;
transport::NodePtr node_handle_;
transport::PublisherPtr actor_pose_pub_5;
transport::SubscriberPtr cmd_pose_sub_5;
transport::PublisherPtr cmd_pose_pub_5;
mav_msgs::msgs::Actor pose_msg;
mav_msgs::msgs::CmdActor cmd_pose_msg;
/// \callback function of command actor pose
void CmdPoseCallback(CmdActorPtr &cmd_msg);
/// \brief Pointer to the parent actor.
private: physics::ActorPtr actor;
/// \brief Pointer to the world, for convenience.
private: physics::WorldPtr world;
/// \brief Pointer to the sdf element.
private: sdf::ElementPtr sdf;
/// \brief Velocity of the actor
private: ignition::math::Vector3d velocity;
/// \brief List of connections
private: std::vector<event::ConnectionPtr> connections;
/// \brief Current target location
private: ignition::math::Vector3d target;
/// \brief Target location weight (used for vector field)
//private: double targetWeight = 1.0;
/// \brief Obstacle weight (used for vector field)
//private: double obstacleWeight = 1.0;
/// \brief Time scaling factor. Used to coordinate translational motion
/// with the actor's walking animation.
//private: double animationFactor = 1.0;
/// \brief Time of the last update.
private: common::Time lastUpdate;
/// \brief List of models to ignore. Used for vector field
private: std::vector<std::string> ignoreModels;
/// \brief Custom trajectory info.
private: physics::TrajectoryInfoPtr trajectoryInfo;
};
}
#endif

507
actor_related/CMakeLists.txt Normal file
View File

@ -0,0 +1,507 @@
cmake_minimum_required(VERSION 3.5 FATAL_ERROR)
cmake_policy(SET CMP0042 NEW)
cmake_policy(SET CMP0048 NEW)
cmake_policy(SET CMP0054 NEW)
if (NOT CMAKE_INSTALL_PREFIX)
set(CMAKE_INSTALL_PREFIX "/usr" CACHE STRING "install prefix" FORCE)
endif()
message(STATUS "install-prefix: ${CMAKE_INSTALL_PREFIX}")
project(mavlink_sitl_gazebo VERSION 1.0.0)
include(GNUInstallDirs)
#######################
## Find Dependencies ##
#######################
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
option(BUILD_GSTREAMER_PLUGIN "enable gstreamer plugin" ON)
option(BUILD_ROS_INTERFACE "Enable building ROS dependent plugins" OFF)
option(SEND_VISION_ESTIMATION_DATA "Send Mavlink VISION_POSITION_ESTIMATE msgs" OFF)
option(SEND_ODOMETRY_DATA "Send Mavlink ODOMETRY msgs" OFF)
# Set c++11 or higher
include(EnableC++XX)
## System dependencies are found with CMake's conventions
find_package(Boost 1.58 REQUIRED COMPONENTS system thread filesystem)
find_package(gazebo REQUIRED)
find_package(PkgConfig REQUIRED)
find_program(px4 REQUIRED)
# Note: If using catkin, Python 2 is found since it points
# to the Python libs installed with the ROS distro
if (NOT CATKIN_DEVEL_PREFIX)
find_package(PythonInterp 3 REQUIRED)
else()
find_package(PythonInterp REQUIRED)
endif()
find_package(OpenCV REQUIRED)
find_package(TinyXML REQUIRED)
if (BUILD_GSTREAMER_PLUGIN)
set(GStreamer_FIND_VERSION "1.0")
find_package(GStreamer REQUIRED)
if (GSTREAMER_FOUND)
if("${GAZEBO_VERSION}" VERSION_LESS "8.0")
find_package (Qt4)
include (${QT_USE_FILE})
else()
# In order to find Qt5 in macOS, the Qt5 path needs to be added to the CMake prefix path.
if(APPLE)
execute_process(COMMAND brew --prefix qt5
ERROR_QUIET
OUTPUT_VARIABLE QT5_PREFIX_PATH
OUTPUT_STRIP_TRAILING_WHITESPACE
)
list(APPEND CMAKE_PREFIX_PATH "${QT5_PREFIX_PATH}/lib/cmake")
endif()
find_package(Qt5 COMPONENTS Core Widgets REQUIRED)
endif()
endif()
endif()
pkg_check_modules(OGRE OGRE)
if("${GAZEBO_VERSION}" VERSION_LESS "8.0")
include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS})
else()
include_directories(SYSTEM ${GAZEBO_INCLUDE_DIRS} ${Qt5Core_INCLUDE_DIRS})
endif()
link_directories(${GAZEBO_LIBRARY_DIRS})
add_subdirectory( external/OpticalFlow OpticalFlow )
set( OpticalFlow_LIBS "OpticalFlow" )
# for ROS subscribers and publishers
if (BUILD_ROS_INTERFACE)
find_package(geometry_msgs REQUIRED)
find_package(roscpp REQUIRED)
find_package(sensor_msgs REQUIRED)
endif()
# find MAVLink
find_package(MAVLink)
# see if catkin was invoked to build this
if (CATKIN_DEVEL_PREFIX)
message(STATUS "catkin ENABLED")
find_package(catkin REQUIRED)
if (catkin_FOUND)
catkin_package()
else()
message(FATAL_ERROR "catkin not found")
endif()
else()
message(STATUS "catkin DISABLED")
endif()
# XXX this approach is extremely error prone
# it would be preferable to either depend on the
# compiled headers from Gazebo directly
# or to have something entirely independent.
#
set(PROTOBUF_IMPORT_DIRS "")
foreach(ITR ${GAZEBO_INCLUDE_DIRS})
if(ITR MATCHES ".*gazebo-[0-9.]+$")
set(PROTOBUF_IMPORT_DIRS "${ITR}/gazebo/msgs/proto")
endif()
endforeach()
# PROTOBUF_IMPORT_DIRS has to be set before
# find_package is called
find_package(Protobuf REQUIRED)
pkg_check_modules(PROTOBUF protobuf)
if ("${PROTOBUF_VERSION}" VERSION_LESS "2.5.0")
message(FATAL_ERROR "protobuf version: ${PROTOBUF_VERSION} not compatible, must be >= 2.5.0")
endif()
if("${GAZEBO_VERSION}" VERSION_LESS "6.0")
message(FATAL_ERROR "You need at least Gazebo 6.0. Your version: ${GAZEBO_VERSION}")
else()
message(STATUS "Gazebo version: ${GAZEBO_VERSION}")
endif()
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
# Fallback to cmake_modules
find_package(Eigen QUIET)
if(NOT EIGEN_FOUND)
pkg_check_modules(EIGEN3 REQUIRED eigen3)
else()
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})
endif()
else()
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()
###########
## Build ##
###########
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99 -Wno-deprecated-declarations -Wno-address-of-packed-member")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-deprecated-declarations -Wno-address-of-packed-member")
set(GAZEBO_MSG_INCLUDE_DIRS)
foreach(ITR ${GAZEBO_INCLUDE_DIRS})
if(ITR MATCHES ".*gazebo-[0-9.]+$")
set(GAZEBO_MSG_INCLUDE_DIRS "${ITR}/gazebo/msgs")
endif()
endforeach()
include_directories(
include
${Boost_INCLUDE_DIR}
${CMAKE_CURRENT_BINARY_DIR}
${EIGEN3_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}/eigen3 # Workaround for Eigen3
${GAZEBO_INCLUDE_DIRS}
${GAZEBO_MSG_INCLUDE_DIRS}
${MAVLINK_INCLUDE_DIRS}
${OGRE_INCLUDE_DIRS}
${OGRE_INCLUDE_DIRS}/Paging # Workaround for "fatal error: OgrePagedWorldSection.h: No such file or directory"
${OpenCV_INCLUDE_DIRS}
${OpticalFlow_INCLUDE_DIRS}
${TinyXML_INCLUDE_DIRS}
)
if (GSTREAMER_FOUND)
include_directories(
${GSTREAMER_INCLUDE_DIRS}
)
endif()
link_libraries(
${Boost_SYSTEM_LIBRARY_RELEASE}
${Boost_THREAD_LIBRARY_RELEASE}
${Boost_TIMER_LIBRARY_RELEASE}
${GAZEBO_LIBRARIES}
${OpenCV_LIBRARIES}
${PROTOBUF_LIBRARY}
)
if (GSTREAMER_FOUND)
link_libraries(
${GSTREAMER_LIBRARIES}
glib-2.0
gobject-2.0
)
endif()
link_directories(
${GAZEBO_LIBRARY_DIRS}
${CMAKE_CURRENT_BINARY_DIR}
${OGRE_LIBRARY_DIRS}
)
#--------------------------#
# Generation of SDF models #
#--------------------------#
set(enable_mavlink_interface "true")
set(enable_ground_truth "false")
set(enable_logging "false")
set(enable_camera "false")
set(enable_wind "false")
set(rotors_description_dir "${CMAKE_CURRENT_SOURCE_DIR}/models/rotors_description")
set(scripts_dir "${CMAKE_CURRENT_SOURCE_DIR}/scripts")
# set the vision estimation to be sent if set by the CMake option SEND_VISION_ESTIMATION_DATA
set(send_vision_estimation "false")
if (SEND_VISION_ESTIMATION_DATA)
set(send_vision_estimation "true")
endif()
# if SEND_ODOMETRY_DATA option is set, then full odometry data is sent instead of
# only the visual pose estimate
set(send_odometry "false")
if (SEND_ODOMETRY_DATA)
set(send_odometry "true")
set(send_vision_estimation "false")
endif()
add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/models/iris/iris.sdf
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
COMMAND rm -f ${CMAKE_CURRENT_SOURCE_DIR}/models/iris/iris.sdf
COMMAND ${PYTHON_EXECUTABLE} ${scripts_dir}/xacro.py -o ${rotors_description_dir}/urdf/iris_base.urdf ${rotors_description_dir}/urdf/iris_base.xacro enable_mavlink_interface:=${enable_mavlink_interface} enable_ground_truth:=${enable_ground_truth} enable_wind:=${enable_wind} enable_logging:=${enable_logging} rotors_description_dir:=${rotors_description_dir} send_vision_estimation:=${send_vision_estimation} send_odometry:=${send_odometry}
COMMAND gz sdf -p ${rotors_description_dir}/urdf/iris_base.urdf >> ${CMAKE_CURRENT_SOURCE_DIR}/models/iris/iris.sdf
COMMAND rm -f ${rotors_description_dir}/urdf/iris_base.urdf
DEPENDS ${rotors_description_dir}/urdf/iris.xacro
DEPENDS ${rotors_description_dir}/urdf/iris_base.xacro
DEPENDS ${rotors_description_dir}/urdf/component_snippets.xacro
)
add_custom_target(sdf ALL DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/models/iris/iris.sdf)
#-----------#
# Functions #
#-----------#
function(glob_generate target file_glob)
file(GLOB_RECURSE glob_files RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} ${file_glob})
set(gen_files)
foreach(glob_file ${glob_files})
string(REGEX REPLACE "\\.[^.]*$" "" file_name ${glob_file})
string(REGEX MATCH "[^.]*$" file_ext ${glob_file})
get_filename_component(file_dir ${glob_file} DIRECTORY)
set(in_file ${CMAKE_CURRENT_SOURCE_DIR}/${glob_file})
file(MAKE_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${file_dir})
set(out_file ${CMAKE_CURRENT_SOURCE_DIR}/${file_name})
string(REGEX REPLACE ".sdf" "-gen.sdf" out_file ${out_file})
if (${file_ext} STREQUAL "jinja")
add_custom_command(OUTPUT ${out_file}
COMMAND ${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py
${in_file} ${CMAKE_CURRENT_SOURCE_DIR}
DEPENDS ${in_file}
VERBATIM
)
list(APPEND gen_files_${target} ${out_file})
endif()
endforeach()
add_custom_target(${target} ALL DEPENDS ${gen_files_${target}})
endfunction()
glob_generate(models_gen ${CMAKE_CURRENT_SOURCE_DIR}/models/*.jinja)
#--------------------#
# Message Generation #
#--------------------#
set(mav_msgs
msgs/CommandMotorSpeed.proto
msgs/MotorSpeed.proto
msgs/Actor.proto
msgs/CmdActor.proto
)
set(nav_msgs msgs/Odometry.proto)
set(physics_msgs msgs/Wind.proto)
set(std_msgs msgs/Int32.proto)
set(sensor_msgs
msgs/Imu.proto
msgs/IRLock.proto
msgs/Float.proto
msgs/Groundtruth.proto
msgs/Range.proto
msgs/SITLGps.proto
msgs/OpticalFlow.proto
msgs/MagneticField.proto
msgs/Pressure.proto
)
PROTOBUF_GENERATE_CPP(MAV_PROTO_SRCS MAV_PROTO_HDRS ${mav_msgs})
PROTOBUF_GENERATE_CPP(NAV_PROTO_SRCS NAV_PROTO_HDRS ${nav_msgs})
PROTOBUF_GENERATE_CPP(PHY_PROTO_SRCS PHY_PROTO_HDRS ${physics_msgs})
PROTOBUF_GENERATE_CPP(STD_PROTO_SRCS STD_PROTO_HDRS ${std_msgs})
PROTOBUF_GENERATE_CPP(SEN_PROTO_SRCS SEN_PROTO_HDRS ${sensor_msgs})
add_library(mav_msgs SHARED ${MAV_PROTO_SRCS})
add_library(nav_msgs SHARED ${NAV_PROTO_SRCS})
add_library(physics_msgs SHARED ${PHY_PROTO_SRCS})
add_library(std_msgs SHARED ${STD_PROTO_SRCS})
add_library(sensor_msgs SHARED ${SEN_PROTO_SRCS})
#---------#
# Plugins #
#---------#
link_libraries(mav_msgs nav_msgs std_msgs sensor_msgs)
link_libraries(physics_msgs)
add_library(gazebo_geotagged_images_plugin SHARED src/gazebo_geotagged_images_plugin.cpp)
add_library(gazebo_gps_plugin SHARED src/gazebo_gps_plugin.cpp)
add_library(gazebo_irlock_plugin SHARED src/gazebo_irlock_plugin.cpp)
add_library(gazebo_lidar_plugin SHARED src/gazebo_lidar_plugin.cpp)
add_library(gazebo_opticalflow_mockup_plugin SHARED src/gazebo_opticalflow_mockup_plugin.cpp)
add_library(gazebo_opticalflow_plugin SHARED src/gazebo_opticalflow_plugin.cpp)
add_library(gazebo_sonar_plugin SHARED src/gazebo_sonar_plugin.cpp)
add_library(gazebo_uuv_plugin SHARED src/gazebo_uuv_plugin.cpp)
add_library(gazebo_vision_plugin SHARED src/gazebo_vision_plugin.cpp)
add_library(gazebo_controller_interface SHARED src/gazebo_controller_interface.cpp)
add_library(gazebo_gimbal_controller_plugin SHARED src/gazebo_gimbal_controller_plugin.cpp)
add_library(gazebo_imu_plugin SHARED src/gazebo_imu_plugin.cpp)
add_library(gazebo_mavlink_interface SHARED src/gazebo_mavlink_interface.cpp )
add_library(gazebo_motor_model SHARED src/gazebo_motor_model.cpp)
add_library(gazebo_multirotor_base_plugin SHARED src/gazebo_multirotor_base_plugin.cpp)
add_library(gazebo_wind_plugin SHARED src/gazebo_wind_plugin.cpp)
add_library(gazebo_magnetometer_plugin SHARED src/gazebo_magnetometer_plugin.cpp src/geo_mag_declination.cpp)
add_library(gazebo_barometer_plugin SHARED src/gazebo_barometer_plugin.cpp)
add_library(gazebo_catapult_plugin SHARED src/gazebo_catapult_plugin.cpp)
add_library(gazebo_usv_dynamics_plugin SHARED src/gazebo_usv_dynamics_plugin.cpp)
add_library(gazebo_parachute_plugin SHARED src/gazebo_parachute_plugin.cpp)
add_library(ActorPlugin SHARED src/ActorPlugin.cpp)
add_library(ActorPlugin2 SHARED src/ActorPlugin2.cpp)
add_library(ActorPlugin3 SHARED src/ActorPlugin3.cpp)
add_library(ActorPlugin4 SHARED src/ActorPlugin4.cpp)
add_library(ActorPlugin5 SHARED src/ActorPlugin5.cpp)
set(plugins
gazebo_geotagged_images_plugin
gazebo_gps_plugin
gazebo_irlock_plugin
gazebo_lidar_plugin
gazebo_opticalflow_mockup_plugin
gazebo_opticalflow_plugin
gazebo_sonar_plugin
gazebo_uuv_plugin
gazebo_vision_plugin
gazebo_controller_interface
gazebo_gimbal_controller_plugin
gazebo_imu_plugin
gazebo_mavlink_interface
gazebo_motor_model
gazebo_multirotor_base_plugin
gazebo_wind_plugin
gazebo_magnetometer_plugin
gazebo_barometer_plugin
gazebo_catapult_plugin
gazebo_usv_dynamics_plugin
gazebo_parachute_plugin
ActorPlugin
ActorPlugin2
ActorPlugin3
ActorPlugin4
ActorPlugin5
)
foreach(plugin ${plugins})
target_link_libraries(${plugin} ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${TinyXML_LIBRARIES})
endforeach()
target_link_libraries(gazebo_opticalflow_plugin ${OpticalFlow_LIBS})
# If BUILD_ROS_INTERFACE set to ON, build plugins that have ROS dependencies
# Current plugins that can be used with ROS interface: gazebo_motor_failure_plugin
if (BUILD_ROS_INTERFACE)
add_library(gazebo_motor_failure_plugin SHARED src/gazebo_motor_failure_plugin.cpp)
target_link_libraries(gazebo_motor_failure_plugin ${GAZEBO_libraries} ${roscpp_LIBRARIES})
list(APPEND plugins gazebo_motor_failure_plugin)
message(STATUS "adding gazebo_motor_failure_plugin to build")
include_directories(
include
${geometry_msgs_INCLUDE_DIRS}
${sensor_msgs_INCLUDE_DIRS}
)
target_link_libraries(gazebo_motor_failure_plugin
${catkin_LIBRARIES}
${roscpp_LIBRARIES}
${GAZEBO_libraries}
)
endif()
if (GSTREAMER_FOUND)
add_library(gazebo_gst_camera_plugin SHARED src/gazebo_gst_camera_plugin.cpp)
set(plugins
${plugins}
gazebo_gst_camera_plugin
)
message(STATUS "Found GStreamer: adding gst_camera_plugin")
if("${GAZEBO_VERSION}" VERSION_LESS "8.0")
QT4_WRAP_CPP(headers_MOC include/gazebo_video_stream_widget.h)
add_library(gazebo_video_stream_widget SHARED ${headers_MOC} src/gazebo_video_stream_widget.cpp)
target_link_libraries(gazebo_video_stream_widget ${GAZEBO_LIBRARIES} ${QT_LIBRARIES} ${PROTOBUF_LIBRARIES})
set(plugins
${plugins}
gazebo_video_stream_widget
)
message(STATUS "Found GStreamer: adding gst_video_stream_widget")
else()
QT5_WRAP_CPP(headers_MOC include/gazebo_video_stream_widget.h)
add_library(gazebo_video_stream_widget SHARED ${headers_MOC} src/gazebo_video_stream_widget.cpp)
target_link_libraries(gazebo_video_stream_widget ${GAZEBO_LIBRARIES} ${Qt5Core_LIBRARIES} ${Qt5Widgets_LIBRARIES} ${PROTOBUF_LIBRARIES} ${Qt5Test_LIBRARIES})
set(plugins
${plugins}
gazebo_video_stream_widget
)
message(STATUS "Found GStreamer: adding gst_video_stream_widget")
endif()
endif()
# Linux is not consistent with plugin availability, even on Gazebo 7
#if("${GAZEBO_VERSION}" VERSION_LESS "7.0")
add_library(LiftDragPlugin SHARED src/liftdrag_plugin/liftdrag_plugin.cpp)
list(APPEND plugins LiftDragPlugin)
#endif()
foreach(plugin ${plugins})
add_dependencies(${plugin} mav_msgs nav_msgs std_msgs sensor_msgs)
add_dependencies(${plugin} physics_msgs)
endforeach()
# Configure the setup script
if (catkin_FOUND)
catkin_add_env_hooks(50_sitl_gazebo_setup
DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/cmake
SHELLS sh)
endif()
################
## Unit Tests ##
################
include(UnitTests)
add_subdirectory(unit_tests)
#############
## Install ##
#############
set(PLUGIN_PATH ${CMAKE_INSTALL_LIBDIR}/${PROJECT_NAME}/plugins)
set(MODEL_PATH ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME}/models)
set(RESOURCE_PATH ${CMAKE_INSTALL_DATADIR}/${PROJECT_NAME})
file(REMOVE_RECURSE ${PROJECT_SOURCE_DIR}/models/.DS_Store)
file(GLOB models_list LIST_DIRECTORIES true ${PROJECT_SOURCE_DIR}/models/*)
file(REMOVE_RECURSE ${PROJECT_SOURCE_DIR}/worlds/.DS_Store)
file(GLOB worlds_list LIST_DIRECTORIES true ${PROJECT_SOURCE_DIR}/worlds/*)
install(TARGETS ${plugins} mav_msgs nav_msgs std_msgs sensor_msgs DESTINATION ${PLUGIN_PATH})
install(DIRECTORY ${models_list} DESTINATION ${MODEL_PATH})
install(FILES ${worlds_list} DESTINATION ${RESOURCE_PATH}/worlds)
configure_file(src/setup.sh.in "${CMAKE_CURRENT_BINARY_DIR}/setup.sh" @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/setup.sh DESTINATION ${RESOURCE_PATH})
install(FILES ${PROJECT_SOURCE_DIR}/package.xml DESTINATION ${RESOURCE_PATH})
#############
## Testing ##
#############
# TODO
###############
## Packaging ##
###############
set(CPACK_PACKAGE_NAME ${PROJECT_NAME}-${GAZEBO_MAJOR_VERSION})
set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
set(CPACK_PACKAGE_CONTACT pxusers@googlegroups.com)
set(DEBIAN_PACKAGE_DEPENDS "")
set(RPM_PACKAGE_DEPENDS "")
set(CPACK_DEBIAN_PACKAGE_DEPENDS ${DEBIAN_PACKAGE_DEPENDS})
set(CPACK_DEBIAN_PACKAGE_SECTION "devel")
set(CPACK_DEBIAN_PACKAGE_PRIORITY "optional")
set(CPACK_DEBIAN_PACKAGE_SHLIBDEPS ON)
set(CPACK_DEBIAN_PACKAGE_DESCRIPTION "gazebo plugins for px4 sitl.")
set(CPACK_RPM_PACKAGE_REQUIRES "${DEBIAN_PACKAGE_DEPENDS}")
set(CPACK_RPM_PACKAGE_DESCRIPTION "Gazebo plugins for px4 sitl.")
set(CPACK_PACKAGE_FILE_NAME "${PROJECT_NAME}-${GAZEBO_MAJOR_VERSION}-${PROJECT_VERSION}")
set(CPACK_SOURCE_PACKAGE_FILE_NAME "${PROJECT_NAME}-${GAZEBO_MAJOR_VERSION}-${PROJECT_VERSION}")
include(CPack)

12
actor_related/CmdActor.proto Normal file
View File

@ -0,0 +1,12 @@
syntax = "proto2";
package mav_msgs.msgs;
//import "vector3d.proto";
message CmdActor
{
required double cmd_actor_pose_x = 1;
required double cmd_actor_pose_y = 2;
required double cmd_actor_pose_z = 3;
//required gazebo.msgs.Vector3d cmd_actor_pose = 1;
}

5317
sitl_config/worlds/robcup.world Normal file
View File

File diff suppressed because it is too large Load Diff