p80426359
/
pxmlw6n2f
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

1. xml_configuration branch merged 

2. sdformat updated with distribution
3. hector test project (ROS)
This commit is contained in:
zhangshuai 2019-04-04 13:51:08 +08:00
parent a57eba035e
commit 18b2f5223e
1433 changed files with 366860 additions and 205 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Gazebo_Distributed/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
build
.vscode

1
Gazebo_Distributed/gazebo/CMakeLists.txt
View File

@ -131,5 +131,6 @@ set(headers
gazebo.hh
Master.hh
Server.hh
countTime.hh
)
gz_install_includes("" ${headers})

13
Gazebo_Distributed/gazebo/countTime.hh Normal file
View File

@ -0,0 +1,13 @@
#ifndef _COUNT_TIME_HH_
#define _COUNT_TIME_HH_
#ifndef _WIN32 //Added by zenglei@2018-12-04
#include <sys/time.h>
#endif
#ifndef USE_COUNT_TIME
#define USE_COUNT_TIME
#endif
#endif

4
Gazebo_Distributed/gazebo/physics/CMakeLists.txt
View File

@ -85,6 +85,8 @@ set (sources ${sources}
World.cc
WorldState.cc
TcpCommunication.cc #zhangshuai 2019.03.19
Distribution.cc #zhangshuai 2019.03.28
GazeboID.cc #zhangshuai 2019.03.28
)
set (headers
@ -139,6 +141,8 @@ set (headers
World.hh
WorldState.hh
TcpCommunication.hh #zhangshuai 2019.03.19
Distribution.hh #zhangshuai 2019.03.28
GazeboID.hh #zhangshuai 2019.03.28
)
set (physics_headers "" CACHE INTERNAL "physics headers" FORCE)

125
Gazebo_Distributed/gazebo/physics/Distribution.cc Normal file
View File

@ -0,0 +1,125 @@
/*
* Copyright (C) 2019 AIRC 01
*
* 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
* distribution under the License is distribution 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.
*
*/
/* Desc: Base class for distribution simualtion
* Author: Zhang Shuai
* Date: 28 March 2019
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
// #include <tbb/parallel_for.h>
// #include <tbb/blocked_range.h>
// #include <float.h>
#include <boost/bind.hpp>
#include <boost/function.hpp>
// #include <boost/thread/recursive_mutex.hpp>
// #include <sstream>
// #include "gazebo/util/OpenAL.hh"
// #include "gazebo/common/KeyFrame.hh"
// #include "gazebo/common/Animation.hh"
// #include "gazebo/common/Plugin.hh"
// #include "gazebo/common/Events.hh"
// #include "gazebo/common/Exception.hh"
// #include "gazebo/common/Console.hh"
// #include "gazebo/common/CommonTypes.hh"
// #include "gazebo/physics/Gripper.hh"
// #include "gazebo/physics/Joint.hh"
// #include "gazebo/physics/JointController.hh"
// #include "gazebo/physics/Link.hh"
// #include "gazebo/physics/World.hh"
// #include "gazebo/physics/PhysicsEngine.hh"
// #include "gazebo/physics/Model.hh"
// #include "gazebo/physics/Contact.hh"
// #include "gazebo/transport/Node.hh"
#include "gazebo/physics/Distribution.hh"
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
Distribution::Distribution()
{
}
//////////////////////////////////////////////////
Distribution::~Distribution()
{
this->Fini();
}
//////////////////////////////////////////////////
void Distribution::Load(sdf::ElementPtr _sdf)
{
if (_sdf)
this->sdf = _sdf;
if (this->sdf->HasElement("gazebo_id"))
{
sdf::ElementPtr gazeboIdElem = this->sdf->GetElement("gazebo_id");
while (gazeboIdElem)
{
GazeboIDPtr GazeboID(new physics::GazeboID());
GazeboID->Load(gazeboIdElem);
this->gazeboIDs.push_back(GazeboID);
gazeboIdElem = gazeboIdElem->GetNextElement("gazebo_id");
}
}
}
//////////////////////////////////////////////////
unsigned int Distribution::GetGazeboID(unsigned int _i)
{
return gazeboIDs[_i]->GetGazeboID();
}
//////////////////////////////////////////////////
GazeboIDPtr Distribution::GetGazeboIDPtr(unsigned int _gazeboID)
{
GazeboIDPtr result;
for (unsigned int i = 0; i < this->GetGazeboCount(); i++)
{
if (this->GetGazeboID(i) == _gazeboID)
{
result = gazeboIDs[i];
break;
}
}
return result;
}
//////////////////////////////////////////////////
unsigned int Distribution::GetGazeboCount()
{
return gazeboIDs.size();
}
//////////////////////////////////////////////////
void Distribution::Fini()
{
this->gazeboIDs.clear();
this->sdf.reset();
}

99
Gazebo_Distributed/gazebo/physics/Distribution.hh Normal file
View File

@ -0,0 +1,99 @@
/*
* Copyright (C) 2019 AIRC 01
*
* 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
* distribution under the License is distribution 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.
*
*/
/* Desc: Base class for distribution simualtion
* Author: Zhang Shuai
* Date: 28 March 2019
*/
#ifndef _DISTRIBUTION_HH_
#define _DISTRIBUTION_HH_
#include <string>
// #include <map>
#include <vector>
#include <boost/function.hpp>
// #include <boost/thread/recursive_mutex.hpp>
#include <sdf/sdf.hh>
// #include "gazebo/common/CommonTypes.hh"
#include "gazebo/physics/PhysicsTypes.hh"
// #include "gazebo/physics/ModelState.hh"
// #include "gazebo/physics/Entity.hh"
#include "gazebo/util/system.hh"
#include "gazebo/physics/GazeboID.hh"
namespace gazebo
{
namespace physics
{
/// \class Distribution Distribution.hh physics/physics.hh
/// \brief Distribution is used to store information about the distribution simulation.
class GZ_PHYSICS_VISIBLE Distribution
{
/// \brief Constructor.
/// \param[in] _parent Parent object.
public:
explicit Distribution();
/// \brief Destructor.
public:
virtual ~Distribution();
/// \brief Load the Distribution.
/// \param[in] _sdf SDF parameters to load from.
public:
void Load(sdf::ElementPtr _sdf);
/// \brief Get the gazebo ID of the specified gazebo.
/// \param[in] _i Number of the gazebo ID to get.
/// \return the gazebo ID of the specified gazebo.
public:
unsigned int GetGazeboID(unsigned int _i);
/// \brief Get the gazebo ID pointer of the specified gazebo.
/// \param[in] _i Number of the gazebo ID to get.
/// \return the gazebo ID pointer of the specified gazebo.
public:
GazeboIDPtr GetGazeboIDPtr(unsigned int _gazeboID);
/// \brief Get the number of Gazebos this Distribution has.
/// \return Number of Gazebos associated with this Distribution.
public:
unsigned int GetGazeboCount();
/// \brief Finialize the Distribution
public:
void Fini();
/// \brief The Distribution's current SDF description.
private:
sdf::ElementPtr sdf;
// /// \brief Gazebos' ID in this distribution gazebo.
// private:
// std::vector<unsigned int> gazeboIDs;
/// \brief Gazebos' ID in this distribution gazebo.
private:
GazeboID_V gazeboIDs;
};
} // namespace physics
} // namespace gazebo
#endif

137
Gazebo_Distributed/gazebo/physics/GazeboID.cc Normal file
View File

@ -0,0 +1,137 @@
/*
* Copyright (C) 2019 AIRC 01
*
* 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.
*
*/
/* Desc: Base class for distributed gazebo
* Author: Zhang Shuai
* Date: 28 March 2019
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
// #include <tbb/parallel_for.h>
// #include <tbb/blocked_range.h>
// #include <float.h>
// #include <boost/bind.hpp>
// #include <boost/function.hpp>
// #include <boost/thread/recursive_mutex.hpp>
// #include <sstream>
// #include "gazebo/util/OpenAL.hh"
// #include "gazebo/common/KeyFrame.hh"
// #include "gazebo/common/Animation.hh"
// #include "gazebo/common/Plugin.hh"
// #include "gazebo/common/Events.hh"
// #include "gazebo/common/Exception.hh"
// #include "gazebo/common/Console.hh"
// #include "gazebo/common/CommonTypes.hh"
// #include "gazebo/physics/Gripper.hh"
// #include "gazebo/physics/Joint.hh"
// #include "gazebo/physics/JointController.hh"
// #include "gazebo/physics/Link.hh"
// #include "gazebo/physics/World.hh"
// #include "gazebo/physics/PhysicsEngine.hh"
// #include "gazebo/physics/Model.hh"
// #include "gazebo/physics/Contact.hh"
// #include "gazebo/transport/Node.hh"
#include "gazebo/physics/GazeboID.hh"
using namespace gazebo;
using namespace physics;
//////////////////////////////////////////////////
GazeboID::GazeboID()
{
}
//////////////////////////////////////////////////
GazeboID::~GazeboID()
{
this->Fini();
}
//////////////////////////////////////////////////
void GazeboID::Load(sdf::ElementPtr _sdf)
{
if (_sdf)
this->sdf = _sdf;
if (this->sdf->HasAttribute("num"))
this->gazeboID = this->sdf->Get<unsigned int>("num");
if (this->sdf->HasAttribute("ip"))
this->ip = this->sdf->Get<std::string>("ip");
// if (this->sdf->HasElement("ip"))
// {
// sdf::ElementPtr gazeboIdElem = this->sdf->GetElement("ip");
// ip = gazeboIdElem->Get<std::string>();
// }
// if (this->sdf->HasElement("port"))
// {
// sdf::ElementPtr gazeboPortElem = this->sdf->GetElement("port");
// port = gazeboPortElem->Get<int>();
// }
if (this->sdf->HasElement("model_name"))
{
sdf::ElementPtr modelNameElem = this->sdf->GetElement("model_name");
while (modelNameElem)
{
modelNames.push_back(modelNameElem->Get<std::string>());
modelNameElem = modelNameElem->GetNextElement("model_name");
}
}
}
//////////////////////////////////////////////////
unsigned int GazeboID::GetGazeboID()
{
return gazeboID;
}
//////////////////////////////////////////////////
std::string GazeboID::GetGazeboIp()
{
return ip;
}
//////////////////////////////////////////////////
unsigned int GazeboID::GetModelCount()
{
return modelNames.size();
}
//////////////////////////////////////////////////
std::string GazeboID::GetModelName(unsigned int _i)
{
return modelNames[_i];
}
//////////////////////////////////////////////////
void GazeboID::Fini()
{
this->modelNames.clear();
this->sdf.reset();
}

109
Gazebo_Distributed/gazebo/physics/GazeboID.hh Normal file
View File

@ -0,0 +1,109 @@
/*
* Copyright (C) 2019 AIRC 01
*
* 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.
*
*/
/* Desc: Base class for distributed gazebo
* Author: Zhang Shuai
* Date: 28 March 2019
*/
#ifndef _GAZEBOID_HH_
#define _GAZEBOID_HH_
#include <string>
// #include <map>
#include <vector>
#include <boost/function.hpp>
// #include <boost/thread/recursive_mutex.hpp>
#include <sdf/sdf.hh>
// #include "gazebo/common/CommonTypes.hh"
#include "gazebo/physics/PhysicsTypes.hh"
// #include "gazebo/physics/ModelState.hh"
// #include "gazebo/physics/Entity.hh"
#include "gazebo/util/system.hh"
namespace gazebo
{
namespace physics
{
/// \class Distribution Distribution.hh physics/physics.hh
/// \brief Distribution is used to store information about the distributed simulation.
class GZ_PHYSICS_VISIBLE GazeboID
{
/// \brief Constructor.
/// \param[in] _parent Parent object.
public:
explicit GazeboID();
/// \brief Destructor.
public:
virtual ~GazeboID();
/// \brief Load the Distribution.
/// \param[in] _sdf SDF parameters to load from.
public:
void Load(sdf::ElementPtr _sdf);
/// \brief Get the gazebo ID of this Distribution.
/// \return the gazebo ID of this Distribution.
public:
unsigned int GetGazeboID();
/// \brief Get the gazebo ip of this Distribution.
/// \return the gazebo ip of this Distribution.
public:
std::string GetGazeboIp();
/// \brief Get the number of models this Distribution has.
/// \return Number of models associated with this Distribution.
public:
unsigned int GetModelCount();
/// \brief Get the name of the specified model.
/// \param[in] _i Number of the model name to get.
/// \return the name of the specified model.
public:
std::string GetModelName(unsigned int _i);
/// \brief Finialize the gazeboID
public:
void Fini();
/// \brief The Distribution's current SDF description.
private:
sdf::ElementPtr sdf;
/// \brief idex of distributed gazebo.
private:
unsigned int gazeboID;
/// \brief ip of distributed gazebo.
private:
std::string ip;
/// \brief port of distributed gazebo.
private:
int port;
/// \brief Models name in this gazebo
private:
std::vector<std::string> modelNames;
};
} // namespace physics
} // namespace gazebo
#endif

184
Gazebo_Distributed/gazebo/physics/Link.cc
View File

@ -16,9 +16,9 @@
*/
#ifdef _WIN32
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
// Ensure that Winsock2.h is included before Windows.h, which can get
// pulled in by anybody (e.g., Boost).
#include <Winsock2.h>
#endif
#include <boost/algorithm/string.hpp>
@ -85,8 +85,7 @@ void Link::Load(sdf::ElementPtr _sdf)
{
this->SetSelfCollide(this->GetModel()->GetSelfCollide());
}
this->sdf->GetElement("self_collide")->GetValue()->SetUpdateFunc(
std::bind(&Link::GetSelfCollide, this));
this->sdf->GetElement("self_collide")->GetValue()->SetUpdateFunc(std::bind(&Link::GetSelfCollide, this));
// Parse visuals from SDF
this->ParseVisuals();
@ -103,45 +102,43 @@ void Link::Load(sdf::ElementPtr _sdf)
}
}
//zhangshuai
std::string bebop_0 = "bebop_0";
std::cout << "bebop_0:" << bebop_0 << std::endl; //zhangshuai
std::cout << "model_name:" << this->GetModel()->GetName() << std::endl; //zhangshuai
// if (std::strcmp(this->GetModel()->GetName(), bebop_0) == 0)
if (this->GetModel()->GetName() == bebop_0)
// Modified by zhangshuai 2019.04.02 ----Begin
// Judge if the model to load is simulated in this gazebo
for (unsigned int i = 0; i < this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelCount(); i++)
{
//zhangshuai
if (this->sdf->HasElement("sensor"))
{
sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor");
while (sensorElem)
if (this->GetModel()->GetName() == this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelName(i))
{
/// \todo This if statement is a hack to prevent Links from creating
/// a force torque sensor. We should make this more generic.
if (sensorElem->Get<std::string>("type") == "force_torque")
// Original part
if (this->sdf->HasElement("sensor"))
{
gzerr << "A link cannot load a [" <<
sensorElem->Get<std::string>("type") << "] sensor.\n";
}
else if (sensorElem->Get<std::string>("type") != "__default__")
{
// This must match the implementation in Sensors::GetScopedName
std::string sensorName = this->GetScopedName(true) + "::" +
sensorElem->Get<std::string>("name");
sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor");
while (sensorElem)
{
/// \todo This if statement is a hack to prevent Links from creating
/// a force torque sensor. We should make this more generic.
if (sensorElem->Get<std::string>("type") == "force_torque")
{
gzerr << "A link cannot load a [" << sensorElem->Get<std::string>("type") << "] sensor.\n";
}
else if (sensorElem->Get<std::string>("type") != "__default__")
{
// This must match the implementation in Sensors::GetScopedName
std::string sensorName = this->GetScopedName(true) + "::" +
sensorElem->Get<std::string>("name");
// Tell the sensor library to create a sensor.
event::Events::createSensor(sensorElem,
this->GetWorld()->GetName(), this->GetScopedName(), this->GetId());
// Tell the sensor library to create a sensor.
event::Events::createSensor(sensorElem,
this->GetWorld()->GetName(), this->GetScopedName(), this->GetId());
this->sensors.push_back(sensorName);
this->sensors.push_back(sensorName);
}
sensorElem = sensorElem->GetNextElement("sensor");
}
}
sensorElem = sensorElem->GetNextElement("sensor");
// Original part
}
}
//zhangshuai
}
//zhangshuai
// Modified by zhangshuai 2019.04.02 ----End
// Load the lights
if (this->sdf->HasElement("light"))
@ -187,16 +184,16 @@ void Link::Load(sdf::ElementPtr _sdf)
if (!collisionNames.empty())
{
for (std::vector<std::string>::iterator iter = collisionNames.begin();
iter != collisionNames.end(); ++iter)
iter != collisionNames.end(); ++iter)
{
(*iter) = this->GetScopedName() + "::" + (*iter);
}
std::string topic =
this->world->GetPhysicsEngine()->GetContactManager()->CreateFilter(
this->GetScopedName() + "/audio_collision", collisionNames);
this->world->GetPhysicsEngine()->GetContactManager()->CreateFilter(
this->GetScopedName() + "/audio_collision", collisionNames);
this->audioContactsSub = this->node->Subscribe(topic,
&Link::OnCollision, this);
&Link::OnCollision, this);
}
}
@ -249,7 +246,7 @@ void Link::Init()
}
if ((*iter)->HasType(Base::LIGHT))
{
LightPtr light= boost::static_pointer_cast<Light>(*iter);
LightPtr light = boost::static_pointer_cast<Light>(*iter);
// TODO add to lights var?
// this->lights.push_back(light);
light->Init();
@ -290,7 +287,7 @@ void Link::Fini()
for (auto iter : this->visuals)
{
auto deleteMsg = msgs::CreateRequest("entity_delete",
std::to_string(iter.second.id()));
std::to_string(iter.second.id()));
this->requestPub->Publish(*deleteMsg, true);
msgs::Visual msg;
@ -373,8 +370,7 @@ void Link::UpdateParameters(sdf::ElementPtr _sdf)
this->sdf->GetElement("gravity")->GetValue()->SetUpdateFunc(
boost::bind(&Link::GetGravityMode, this));
this->sdf->GetElement("kinematic")->GetValue()->SetUpdateFunc(
boost::bind(&Link::GetKinematic, this));
this->sdf->GetElement("kinematic")->GetValue()->SetUpdateFunc(boost::bind(&Link::GetKinematic, this));
if (this->sdf->Get<bool>("gravity") != this->GetGravityMode())
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
@ -442,13 +438,13 @@ void Link::SetCollideMode(const std::string &_mode)
if (_mode == "all")
{
categoryBits = GZ_ALL_COLLIDE;
collideBits = GZ_ALL_COLLIDE;
categoryBits = GZ_ALL_COLLIDE;
collideBits = GZ_ALL_COLLIDE;
}
else if (_mode == "none")
{
categoryBits = GZ_NONE_COLLIDE;
collideBits = GZ_NONE_COLLIDE;
categoryBits = GZ_NONE_COLLIDE;
collideBits = GZ_NONE_COLLIDE;
}
else if (_mode == "sensors")
{
@ -514,7 +510,8 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
// Update all the audio sources
for (std::vector<util::OpenALSourcePtr>::iterator iter =
this->audioSources.begin(); iter != this->audioSources.end(); ++iter)
this->audioSources.begin();
iter != this->audioSources.end(); ++iter)
{
(*iter)->SetPose(this->GetWorldPose().Ign());
(*iter)->SetVelocity(this->GetWorldLinearVel().Ign());
@ -522,7 +519,7 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
#endif
// FIXME: race condition on factory-based model loading!!!!!
/*if (this->GetEnabled() != this->enabled)
/*if (this->GetEnabled() != this->enabled)
{
this->enabled = this->GetEnabled();
this->enabledSignal(this->enabled);
@ -567,8 +564,8 @@ Link_V Link::GetChildJointsLinks() const
{
Link_V links;
for (std::vector<JointPtr>::const_iterator iter = this->childJoints.begin();
iter != this->childJoints.end();
++iter)
iter != this->childJoints.end();
++iter)
{
if ((*iter)->GetChild())
links.push_back((*iter)->GetChild());
@ -581,8 +578,8 @@ Link_V Link::GetParentJointsLinks() const
{
Link_V links;
for (std::vector<JointPtr>::const_iterator iter = this->parentJoints.begin();
iter != this->parentJoints.end();
++iter)
iter != this->parentJoints.end();
++iter)
{
if ((*iter)->GetParent())
links.push_back((*iter)->GetParent());
@ -595,13 +592,13 @@ void Link::LoadCollision(sdf::ElementPtr _sdf)
{
CollisionPtr collision;
std::string geomType =
_sdf->GetElement("geometry")->GetFirstElement()->GetName();
_sdf->GetElement("geometry")->GetFirstElement()->GetName();
if (geomType == "heightmap" || geomType == "map")
this->SetStatic(true);
collision = this->GetWorld()->GetPhysicsEngine()->CreateCollision(geomType,
boost::static_pointer_cast<Link>(shared_from_this()));
boost::static_pointer_cast<Link>(shared_from_this()));
if (!collision)
gzthrow("Unknown Collisionetry Type[" + geomType + "]");
@ -683,7 +680,7 @@ math::Vector3 Link::GetRelativeLinearVel() const
math::Vector3 Link::GetRelativeAngularVel() const
{
return this->GetWorldPose().rot.RotateVectorReverse(
this->GetWorldAngularVel());
this->GetWorldAngularVel());
}
//////////////////////////////////////////////////
@ -702,7 +699,7 @@ math::Vector3 Link::GetWorldLinearAccel() const
math::Vector3 Link::GetRelativeAngularAccel() const
{
return this->GetWorldPose().rot.RotateVectorReverse(
this->GetWorldAngularAccel());
this->GetWorldAngularAccel());
}
//////////////////////////////////////////////////
@ -713,8 +710,7 @@ math::Vector3 Link::GetWorldAngularAccel() const
// L: angular momentum of CoG in world frame
// w: angular velocity in world frame
// return I^-1 * (T - w x L)
return this->GetWorldInertiaMatrix().Inverse() * (this->GetWorldTorque()
- this->GetWorldAngularVel().Cross(this->GetWorldAngularMomentum()));
return this->GetWorldInertiaMatrix().Inverse() * (this->GetWorldTorque() - this->GetWorldAngularVel().Cross(this->GetWorldAngularMomentum()));
}
//////////////////////////////////////////////////
@ -770,7 +766,7 @@ bool Link::SetSelected(bool _s)
}
//////////////////////////////////////////////////
void Link::SetInertial(const InertialPtr &/*_inertial*/)
void Link::SetInertial(const InertialPtr & /*_inertial*/)
{
gzwarn << "Link::SetMass is empty\n";
}
@ -813,8 +809,8 @@ void Link::AddChildJoint(JointPtr _joint)
void Link::RemoveParentJoint(const std::string &_jointName)
{
for (std::vector<JointPtr>::iterator iter = this->parentJoints.begin();
iter != this->parentJoints.end();
++iter)
iter != this->parentJoints.end();
++iter)
{
/// @todo: can we assume there are no repeats?
if ((*iter)->GetName() == _jointName)
@ -829,8 +825,8 @@ void Link::RemoveParentJoint(const std::string &_jointName)
void Link::RemoveChildJoint(const std::string &_jointName)
{
for (std::vector<JointPtr>::iterator iter = this->childJoints.begin();
iter != this->childJoints.end();
++iter)
iter != this->childJoints.end();
++iter)
{
/// @todo: can we assume there are no repeats?
if ((*iter)->GetName() == _jointName)
@ -870,7 +866,7 @@ void Link::FillMsg(msgs::Link &_msg)
_msg.mutable_inertial()->set_iyz(this->inertial->GetIYZ());
_msg.mutable_inertial()->set_izz(this->inertial->GetIZZ());
msgs::Set(_msg.mutable_inertial()->mutable_pose(),
this->inertial->GetPose().Ign());
this->inertial->GetPose().Ign());
for (auto &child : this->children)
{
@ -901,7 +897,7 @@ void Link::FillMsg(msgs::Link &_msg)
this->UpdateVisualMsg();
for (Visuals_M::iterator iter = this->visuals.begin();
iter != this->visuals.end(); ++iter)
iter != this->visuals.end(); ++iter)
{
msgs::Visual *vis = _msg.add_visual();
vis->CopyFrom(iter->second);
@ -976,7 +972,7 @@ void Link::ProcessMsg(const msgs::Link &_msg)
if (coll)
coll->ProcessMsg(_msg.collision(i));
}
if (_msg.collision_size()>0)
if (_msg.collision_size() > 0)
this->UpdateSurface();
}
@ -1015,8 +1011,8 @@ void Link::DetachStaticModel(const std::string &_modelName)
{
if (this->attachedModels[i]->GetName() == _modelName)
{
this->attachedModels.erase(this->attachedModels.begin()+i);
this->attachedModelsOffset.erase(this->attachedModelsOffset.begin()+i);
this->attachedModels.erase(this->attachedModels.begin() + i);
this->attachedModelsOffset.erase(this->attachedModelsOffset.begin() + i);
break;
}
}
@ -1085,7 +1081,7 @@ double Link::GetAngularDamping() const
}
/////////////////////////////////////////////////
void Link::SetKinematic(const bool &/*_kinematic*/)
void Link::SetKinematic(const bool & /*_kinematic*/)
{
}
@ -1109,8 +1105,8 @@ void Link::SetPublishData(bool _enable)
std::string topic = "~/" + this->GetScopedName();
this->dataPub = this->node->Advertise<msgs::LinkData>(topic);
this->connections.push_back(
event::Events::ConnectWorldUpdateEnd(
boost::bind(&Link::PublishData, this)));
event::Events::ConnectWorldUpdateEnd(
boost::bind(&Link::PublishData, this)));
}
else
{
@ -1128,9 +1124,9 @@ void Link::PublishData()
msgs::Set(this->linkDataMsg.mutable_time(), this->world->GetSimTime());
linkDataMsg.set_name(this->GetScopedName());
msgs::Set(this->linkDataMsg.mutable_linear_velocity(),
this->GetWorldLinearVel().Ign());
this->GetWorldLinearVel().Ign());
msgs::Set(this->linkDataMsg.mutable_angular_velocity(),
this->GetWorldAngularVel().Ign());
this->GetWorldAngularVel().Ign());
this->dataPub->Publish(this->linkDataMsg);
}
}
@ -1227,7 +1223,7 @@ bool Link::SetVisualPose(const uint32_t _id,
while (visualElem)
{
std::string visName = linkName + "::" +
visualElem->Get<std::string>("name");
visualElem->Get<std::string>("name");
// update visual msg if it exists
if (msg.name() == visName)
@ -1266,12 +1262,13 @@ void Link::OnCollision(ConstContactsPtr &_msg)
GZ_ASSERT(pos1 != std::string::npos, "Invalid collision name");
GZ_ASSERT(pos2 != std::string::npos, "Invalid collision name");
collisionName1 = collisionName1.substr(pos1+2);
collisionName2 = collisionName2.substr(pos2+2);
collisionName1 = collisionName1.substr(pos1 + 2);
collisionName2 = collisionName2.substr(pos2 + 2);
#ifdef HAVE_OPENAL
for (std::vector<util::OpenALSourcePtr>::iterator iter =
this->audioSources.begin(); iter != this->audioSources.end(); ++iter)
this->audioSources.begin();
iter != this->audioSources.end(); ++iter)
{
if ((*iter)->HasCollisionName(collisionName1) ||
(*iter)->HasCollisionName(collisionName2))
@ -1351,7 +1348,7 @@ void Link::UpdateVisualGeomSDF(const math::Vector3 &_scale)
math::Vector3 size =
geomElem->GetElement("box")->Get<math::Vector3>("size");
geomElem->GetElement("box")->GetElement("size")->Set(
_scale/this->scale*size);
_scale / this->scale * size);
}
else if (geomElem->HasElement("sphere"))
{
@ -1359,9 +1356,9 @@ void Link::UpdateVisualGeomSDF(const math::Vector3 &_scale)
double radius = geomElem->GetElement("sphere")->Get<double>("radius");
double newRadius = std::max(_scale.z, std::max(_scale.x, _scale.y));
double oldRadius = std::max(this->scale.Z(),
std::max(this->scale.X(), this->scale.Y()));
std::max(this->scale.X(), this->scale.Y()));
geomElem->GetElement("sphere")->GetElement("radius")->Set(
newRadius/oldRadius*radius);
newRadius / oldRadius * radius);
}
else if (geomElem->HasElement("cylinder"))
{
@ -1371,10 +1368,8 @@ void Link::UpdateVisualGeomSDF(const math::Vector3 &_scale)
double oldRadius = std::max(this->scale.X(), this->scale.Y());
double length = geomElem->GetElement("cylinder")->Get<double>("length");
geomElem->GetElement("cylinder")->GetElement("radius")->Set(
newRadius/oldRadius*radius);
geomElem->GetElement("cylinder")->GetElement("length")->Set(
_scale.z/this->scale.Z()*length);
geomElem->GetElement("cylinder")->GetElement("radius")->Set(newRadius / oldRadius * radius);
geomElem->GetElement("cylinder")->GetElement("length")->Set(_scale.z / this->scale.Z() * length);
}
else if (geomElem->HasElement("mesh"))
geomElem->GetElement("mesh")->GetElement("scale")->Set(_scale);
@ -1402,7 +1397,7 @@ void Link::UpdateVisualMsg()
for (auto &iter : this->visuals)
{
std::string visName = linkName + "::" +
visualElem->Get<std::string>("name");
visualElem->Get<std::string>("name");
if (iter.second.name() == visName)
{
iter.second.mutable_geometry()->CopyFrom(msg.geometry());
@ -1424,7 +1419,7 @@ void Link::UpdateVisualMsg()
auto iter = this->visuals.find(msg.id());
if (iter != this->visuals.end())
gzthrow(std::string("Duplicate visual name[")+msg.name()+"]\n");
gzthrow(std::string("Duplicate visual name[") + msg.name() + "]\n");
this->visuals[msg.id()] = msg;
}
@ -1479,15 +1474,14 @@ double Link::GetWorldEnergy() const
void Link::MoveFrame(const math::Pose &_worldReferenceFrameSrc,
const math::Pose &_worldReferenceFrameDst)
{
math::Pose targetWorldPose = (this->GetWorldPose() - _worldReferenceFrameSrc)
+ _worldReferenceFrameDst;
math::Pose targetWorldPose = (this->GetWorldPose() - _worldReferenceFrameSrc) + _worldReferenceFrameDst;
this->SetWorldPose(targetWorldPose);
this->SetWorldTwist(math::Vector3(0, 0, 0), math::Vector3(0, 0, 0));
}
/////////////////////////////////////////////////
bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
Link_V &_connectedLinks, bool _fistLink)
Link_V &_connectedLinks, bool _fistLink)
{
// debug
// std::string pn;
@ -1507,7 +1501,7 @@ bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
// loop through all joints where this link is a parent link of the joint
for (Link_V::iterator li = childLinks.begin();
li != childLinks.end(); ++li)
li != childLinks.end(); ++li)
{
// gzerr << "debug: checking " << (*li)->GetName() << "\n";
@ -1536,7 +1530,7 @@ bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
// if it returns false, it looped back to parent, mark flag and break
// from current for-loop.
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
_connectedLinks))
_connectedLinks))
{
// one of the recursed link is the parent link
return false;
@ -1553,7 +1547,7 @@ bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
// loop through all joints where this link is a parent link of the joint
for (Link_V::iterator li = parentLinks.begin();
li != parentLinks.end(); ++li)
li != parentLinks.end(); ++li)
{
// check child link of each child joint recursively
if ((*li).get() == _originalParentLink.get())
@ -1586,7 +1580,7 @@ bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
// if it returns false, it looped back to parent, mark flag and break
// from current for-loop.
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
_connectedLinks))
_connectedLinks))
{
// one of the recursed link is the parent link
return false;

22
Gazebo_Distributed/gazebo/physics/PhysicsTypes.hh
View File

@ -66,6 +66,10 @@ namespace gazebo
class LightState;
class LinkState;
class JointState;
// Added by zhangshuai 2019.03.28 ----Begin
class GazeboID;
class Distribution;
// Added by zhangshuai 2019.03.28 ----End
/// \def BasePtr
/// \brief Boost shared pointer to a Base object
@ -235,6 +239,24 @@ namespace gazebo
/// \brief Map of joint state
typedef std::map<std::string, JointState> JointState_M;
// Added by zhangshuai 2019.03.28 ----Begin
/// \def GazeboIDPtr
/// \brief Boost shared pointer to a GazeboID object
typedef boost::shared_ptr<GazeboID> GazeboIDPtr;
/// \def DistributionPtr
/// \brief Boost shared pointer to a Distribution object
typedef boost::shared_ptr<Distribution> DistributionPtr;
/// \def GazeboID_V
/// \brief Vector of GazeboIDPtr
typedef std::vector<GazeboIDPtr> GazeboID_V;
/// \def Distribution_V
/// \brief Vector of DistributionPtr
typedef std::vector<DistributionPtr> Distribution_V;
// Added by zhangshuai 2019.03.28 ----End
#ifndef GZ_COLLIDE_BITS
/// \def GZ_ALL_COLLIDE

399
Gazebo_Distributed/gazebo/physics/World.cc
View File

@ -84,6 +84,10 @@
#include "gazebo/physics/ContactManager.hh"
#include "gazebo/physics/Population.hh"
// Added by zhangshuai 2019.04.03 for count time ----Begin
#include "gazebo/countTime.hh"
// Added by zhangshuai 2019.04.03 for count time ----End
using namespace gazebo;
using namespace physics;
@ -91,6 +95,22 @@ using namespace physics;
/// This will be replaced with a class member variable in Gazebo 3.0
bool g_clearModels;
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
double before_worldUpdateBeginTime = 0;
double worldUpdateBeginTime = 0;
double modelUpdateFuncTime = 0;
double updateCollisionTime = 0;
double loggingTime = 0;
double before_updatePhysicsTime = 0;
double updatePhysicsTime = 0;
double before_tcpTime = 0;
double tcpTime = 0;
double after_tcpTime = 0;
double wholeUpdataTime = 0;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
class ModelUpdate_TBB
{
public:
@ -253,15 +273,30 @@ void World::Load(sdf::ElementPtr _sdf)
"~/factory/light");
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
if (this->dataPtr->sdf->HasElement("distributed"))
if (this->dataPtr->sdf->HasElement("distribution"))
{
sdf::ElementPtr eventElem = this->dataPtr->sdf->GetElement("distributed");
type_ = eventElem->Get<int>("type");
ip = eventElem->Get<std::string>("ip");
port = eventElem->Get<int>("port");
flag = eventElem->Get<int>("flag");
simula_entity_num = eventElem->Get<int>("simula_entity_num");
shadow_entity_num = eventElem->Get<int>("shadow_entity_num");
sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution");
gazeboLocalID = distributionElem->Get<unsigned int>("gazebo_local_ID");
port = distributionElem->Get<int>("port");
flag = distributionElem->Get<int>("flag");
std::cout << "================= gazebo_local_ID: " << gazeboLocalID << "\t Port: " << port << "\t Flag: " << flag << " =================" << std::endl;
DistributionPtr distribution_tmp(new physics::Distribution());
this->distribution = distribution_tmp;
this->distribution->Load(distributionElem);
std::cout << "================= gazebo_counts: " << this->distribution->GetGazeboCount() << " =================" << std::endl;
std::cout << "================= gazebo_ID: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetGazeboID() << " =================" << std::endl;
std::cout << "================= gazebo_ip: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetGazeboIp() << " =================" << std::endl;
std::cout << "================= model_counts: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount() << " =================" << std::endl;
// std::cout << "================= the second model name: " << this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(1) << " =================" << std::endl;
// ip = eventElem->Get<std::string>("ip");
// simula_entity_num = eventElem->Get<int>("simula_entity_num");
// shadow_entity_num = eventElem->Get<int>("shadow_entity_num");
}
//Added by zhangshuai based on zenglei 2019.03.19 ----End
@ -419,18 +454,16 @@ void World::Run(unsigned int _iterations)
this->dataPtr->stopIterations = _iterations;
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
std::cout << "Server IP: " << ip << "\t Server Port: " << port << "\t Flag: " << flag << std::endl;
std::cout << "simula_entity_num: " << simula_entity_num << "\t shadow_entity_num: " << shadow_entity_num << std::endl;
if (1 == flag)
{
if (0 == type_)
if (0 == this->gazeboLocalID)
{
gazeboZero.SetPort(port);
gazeboZero.StartServer();
}
else if (1 == type_)
else if (1 == this->gazeboLocalID)
{
gazeboOne.SetIp(ip);
gazeboOne.SetIp(this->distribution->GetGazeboIDPtr(0)->GetGazeboIp());
gazeboOne.SetPort(port);
gazeboOne.ConnectServer();
}
@ -519,11 +552,11 @@ void World::RunLoop()
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
if (1 == flag)
{
if (0 == type_)
if (0 == this->gazeboLocalID)
{
gazeboZero.Close();
}
else if (1 == type_)
else if (1 == this->gazeboLocalID)
{
gazeboOne.Close();
}
@ -720,6 +753,18 @@ void World::Step(unsigned int _steps)
//////////////////////////////////////////////////
void World::Update()
{
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
struct timeval tv;
double cur_time;
double start_time;
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
start_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
DIAG_TIMER_START("World::Update");
if (this->dataPtr->needsReset)
@ -737,20 +782,77 @@ void World::Update()
this->dataPtr->updateInfo.simTime = this->GetSimTime();
this->dataPtr->updateInfo.realTime = this->GetRealTime();
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
before_worldUpdateBeginTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
event::Events::worldUpdateBegin(this->dataPtr->updateInfo);
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
worldUpdateBeginTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
DIAG_TIMER_LAP("World::Update", "Events::worldUpdateBegin");
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Update all the models
(*this.*dataPtr->modelUpdateFunc)();
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
modelUpdateFuncTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
DIAG_TIMER_LAP("World::Update", "Model::Update");
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// This must be called before PhysicsEngine::UpdatePhysics.
this->dataPtr->physicsEngine->UpdateCollision();
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
updateCollisionTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
DIAG_TIMER_LAP("World::Update", "PhysicsEngine::UpdateCollision");
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Wait for logging to finish, if it's running.
if (util::LogRecord::Instance()->Running())
{
@ -766,13 +868,41 @@ void World::Update()
}
}
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
loggingTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Give clients a possibility to react to collisions before the physics
// gets updated.
this->dataPtr->updateInfo.realTime = this->GetRealTime();
event::Events::beforePhysicsUpdate(this->dataPtr->updateInfo);
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
before_updatePhysicsTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
DIAG_TIMER_LAP("World::Update", "Events::beforePhysicsUpdate");
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Update the physics engine
if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
{
@ -800,102 +930,133 @@ void World::Update()
DIAG_TIMER_LAP("World::Update", "SetWorldPose(dirtyPoses)");
}
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
updatePhysicsTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
if (1 == flag)
{
// if (this->dataPtr->iterations % 10 == 0)
// Modified by zhangshuai 2019.04.02 ----Begin
{
unsigned int i = 0;
for (unsigned int k = 0; k < simula_entity_num; k++)
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++)
{
std::stringstream ss;
ss << k;
std::string ks = ss.str();
std::string tmp_model_name = "bebop_" + ks;
// std::cout << "model_name:" << tmp_model_name << std::endl;
for (i = 0; i < this->GetModel(tmp_model_name)->GetLink("base_link")->GetName().length(); i++)
infor[k].link_name[i] = this->GetModel(tmp_model_name)->GetLink("base_link")->GetName()[i];
infor[k].link_name[i] = '\0';
infor[k].ID = this->GetModel(tmp_model_name)->GetLink("base_link")->GetId();
infor[k].link_pose = this->GetModel(tmp_model_name)->GetLink("base_link")->GetWorldPose();
// std::cout << "link_name:" << this->GetModel("bebop_0")->GetLink("base_link")->GetName() << std::endl;
// std::cout << "ID:" << this->GetModel("bebop_0")->GetLink("base_link")->GetId() << std::endl;
// std::cout << "link_pos_x:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().pos.x << std::endl;
// std::cout << "link_pos_y:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().pos.y << std::endl;
// std::cout << "link_pos_z:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().pos.z << std::endl;
// std::cout << "link_rot_x:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().x << std::endl;
// std::cout << "link_rot_y:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().y << std::endl;
// std::cout << "link_rot_z:" << this->GetModel("bebop_0")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().z << std::endl;
// std::cout << "link_name(infor):" << infor[k].link_name << std::endl;
// std::cout << "ID(infor):" << infor[k].ID << std::endl;
// std::cout << "link_pos_x(infor):" << infor[k].link_pose.pos.x << std::endl;
// std::cout << "link_pos_y(infor):" << infor[k].link_pose.pos.y << std::endl;
// std::cout << "link_pos_z(infor):" << infor[k].link_pose.pos.z << std::endl;
// std::cout << "link_rot_x(infor):" << infor[k].link_pose.rot.GetAsEuler().x << std::endl;
// std::cout << "link_rot_y(infor):" << infor[k].link_pose.rot.GetAsEuler().y << std::endl;
// std::cout << "link_rot_z(infor):" << infor[k].link_pose.rot.GetAsEuler().z << std::endl;
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i);
unsigned int j = 0;
for (j = 0; j < this->GetModel(tmp_model_name)->GetLink("base_link")->GetName().length(); j++)
infor[i].link_name[j] = this->GetModel(tmp_model_name)->GetLink("base_link")->GetName()[j];
infor[i].link_name[j] = '\0';
infor[i].ID = this->GetModel(tmp_model_name)->GetLink("base_link")->GetId();
infor[i].link_pose = this->GetModel(tmp_model_name)->GetLink("base_link")->GetWorldPose();
}
CommunicationData temp[50];
if (0 == type_)
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
before_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
if (0 == this->gazeboLocalID)
{
if (!gazeboZero.sendInfor(infor, simula_entity_num))
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
if (!gazeboZero.sendInfor(infor, this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount()))
std::cout << "Send Infor fail!" << std::endl;
if (!gazeboZero.recvInfor(temp, shadow_entity_num))
if (!gazeboZero.recvInfor(temp, this->distribution->GetGazeboIDPtr(1)->GetModelCount()))
std::cout << "Rcceive Infor fail!" << std::endl;
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(1)->GetModelCount(); i++)
{
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(1)->GetModelName(i);
this->GetModel(tmp_model_name)->GetLink("base_link")->SetWorldPose(temp[i].link_pose);
}
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
}
else if (1 == type_)
else if (1 == this->gazeboLocalID)
{
if (!gazeboOne.recvInfor(temp, shadow_entity_num))
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
if (!gazeboOne.recvInfor(temp, this->distribution->GetGazeboIDPtr(0)->GetModelCount()))
std::cout << "Rcceive Infor fail!" << std::endl;
if (!gazeboOne.sendInfor(infor, simula_entity_num))
if (!gazeboOne.sendInfor(infor, this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount()))
std::cout << "Send Infor fail!" << std::endl;
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
cur_time = (double)tv.tv_sec + (double)tv.tv_usec / 1.e6;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(0)->GetModelCount(); i++)
{
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(0)->GetModelName(i);
this->GetModel(tmp_model_name)->GetLink("base_link")->SetWorldPose(temp[i].link_pose);
}
// Added by zhangshuai 2019.04.03 for count time ----Begin
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
after_tcpTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - cur_time;
#endif
// Added by zhangshuai 2019.04.03 for count time ----End
}
for (unsigned int k = 0; k < shadow_entity_num; k++)
{
std::stringstream ss;
ss << (k + shadow_entity_num);
std::string ks = ss.str();
std::string tmp_model_name = "bebop_" + ks;
// std::cout << "model_name:" << tmp_model_name << std::endl;
// std::cout << "link_name(infor):" << temp[k].link_name << std::endl;
// std::cout << "ID(infor):" << temp[k].ID << std::endl;
// std::cout << "link_pos_x(infor):" << temp[k].link_pose.pos.x << std::endl;
// std::cout << "link_pos_y(infor):" << temp[k].link_pose.pos.y << std::endl;
// std::cout << "link_pos_z(infor):" << temp[k].link_pose.pos.z << std::endl;
// std::cout << "link_rot_x(infor):" << temp[k].link_pose.rot.GetAsEuler().x << std::endl;
// std::cout << "link_rot_y(infor):" << temp[k].link_pose.rot.GetAsEuler().y << std::endl;
// std::cout << "link_rot_z(infor):" << temp[k].link_pose.rot.GetAsEuler().z << std::endl;
this->GetModel(tmp_model_name)->GetLink("base_link")->SetWorldPose(temp[k].link_pose);
}
// std::cout << "link_name(infor):" << infor[0].link_name << std::endl;
// std::cout << "ID(infor):" << infor[0].ID << std::endl;
// std::cout << "link_pos_x(infor):" << temp[0].link_pose.pos.x << std::endl;
// std::cout << "link_pos_y(infor):" << temp[0].link_pose.pos.y << std::endl;
// std::cout << "link_pos_z(infor):" << temp[0].link_pose.pos.z << std::endl;
// std::cout << "link_rot_x(infor):" << temp[0].link_pose.rot.GetAsEuler().x << std::endl;
// std::cout << "link_rot_y(infor):" << temp[0].link_pose.rot.GetAsEuler().y << std::endl;
// std::cout << "link_rot_z(infor):" << temp[0].link_pose.rot.GetAsEuler().z << std::endl;
// std::cout << "link_name:" << this->GetModel("bebop_0")->GetLink("base_link")->GetName() << std::endl;
// std::cout << "ID:" << this->GetModel("bebop_0")->GetLink("base_link")->GetId() << std::endl;
// std::cout << "link_pos_x:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().pos.x << std::endl;
// std::cout << "link_pos_y:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().pos.y << std::endl;
// std::cout << "link_pos_z:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().pos.z << std::endl;
// std::cout << "link_rot_x:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().x << std::endl;
// std::cout << "link_rot_y:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().y << std::endl;
// std::cout << "link_rot_z:" << this->GetModel("bebop_1")->GetLink("base_link")->GetWorldPose().rot.GetAsEuler().z << std::endl;
}
// Modified by zhangshuai 2019.04.02 ----End
}
//Added by zhangshuai based on zenglei 2019.03.19 ----End
// Added by zhangshuai based on zenglei 2019.03.19 ----End
// Only update state information if logging data.
if (util::LogRecord::Instance()->Running())
@ -910,6 +1071,28 @@ void World::Update()
event::Events::worldUpdateEnd();
DIAG_TIMER_STOP("World::Update");
#ifdef USE_COUNT_TIME
gettimeofday(&tv, NULL);
wholeUpdataTime += (double)tv.tv_sec + (double)tv.tv_usec / 1.e6 - start_time;
#endif
if (this->dataPtr->iterations == 100000)
{
std::cout << "===== average before_worldUpdateBeginTime:\t" << before_worldUpdateBeginTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average worldUpdateBeginTime:\t" << worldUpdateBeginTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average modelUpdateFuncTime:\t" << modelUpdateFuncTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average updateCollisionTime:\t" << updateCollisionTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average loggingTime:\t" << loggingTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average before_updatePhysicsTime:\t" << before_updatePhysicsTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average updatePhysicsTime:\t" << updatePhysicsTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average before_tcpTime:\t" << before_tcpTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average tcpTime:\t" << tcpTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average after_tcpTime:\t" << after_tcpTime / 100.0 << "\tms =====" << std::endl;
double wholeAddTime = before_worldUpdateBeginTime + worldUpdateBeginTime + modelUpdateFuncTime + updateCollisionTime + loggingTime + before_updatePhysicsTime + updatePhysicsTime + before_tcpTime + tcpTime + after_tcpTime;
std::cout << "===== average wholeAddTime:\t" << wholeAddTime / 100.0 << "\tms =====" << std::endl;
std::cout << "===== average wholeUpdateTime:\t" << wholeUpdataTime / 100.0 << "\tms =====" << std::endl;
}
}
//////////////////////////////////////////////////
@ -2189,7 +2372,18 @@ void World::ProcessFactoryMsgs()
if (model != nullptr)
{
model->Init();
// model->LoadPlugins();
// Modified by zhangshuai 2019.04.02 ----Begin
// Judge if the model to load is simulated in this gazebo
for (unsigned int i = 0; i < this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelCount(); i++)
{
if (model->GetName() == this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i))
{
// Original part
model->LoadPlugins();
// Original part
}
}
// Modified by zhangshuai 2019.04.02 ----End
}
}
catch (...)
@ -3095,11 +3289,18 @@ std::string World::UniqueModelName(const std::string &_name)
return result;
}
// Added by zhangshuai 2019.04.02 ----Begin
//////////////////////////////////////////////////
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
/// add for Tcp communication
// unsigned int World::GetLengthOfCommunicationData()
// {
// return sizeof(infor);
// }
// Added by zhangshuai based on zenglei 2019.03.19 ----End
/// add for read the element of "distribution"
DistributionPtr World::GetDistribution()
{
return this->distribution;
}
//////////////////////////////////////////////////
/// add for read the element of "gazeboLocalID"
unsigned int World::GetGazeboLocalID()
{
return this->gazeboLocalID;
}
// Added by zhangshuai 2019.04.02 ----End

40
Gazebo_Distributed/gazebo/physics/World.hh
View File

@ -52,6 +52,10 @@
#include "gazebo/physics/TcpCommunication.hh"
// Added by zhangshuai based on zenglei 2019.03.19 ----End
// Added by zhangshuai 2019.04.01 ----Begin
#include "gazebo/physics/Distribution.hh"
// Added by zhangshuai 2019.04.01 ----End
namespace gazebo
{
namespace physics
@ -716,11 +720,11 @@ private:
private:
TcpServer gazeboZero;
private:
std::string ip;
// private:
// std::string ip;
private:
int type_; //0:server:gazeboZero; 1:client:gazeboOne;
// private:
// int type_; //0:server:gazeboZero; 1:client:gazeboOne;
private:
int port;
@ -728,18 +732,32 @@ private:
private:
int flag;
private:
unsigned int simula_entity_num;
// private:
// unsigned int simula_entity_num;
private:
unsigned int shadow_entity_num;
// private:
// unsigned int shadow_entity_num;
private:
CommunicationData infor[50];
// public:
// unsigned int GetLengthOfCommunicationData();
// Added by zhangshuai based on zenglei 2019.03.19 ----End
// Added by zhangshuai 2019.04.01 ----Begin
/// \brief the .
private:
unsigned int gazeboLocalID;
/// \brief the .
private:
// Distribution_V distributions;
DistributionPtr distribution;
public:
DistributionPtr GetDistribution();
public:
unsigned int GetGazeboLocalID();
// Added by zhangshuai 2019.04.01 ----End
};
/// \}
} // namespace physics

1
Gazebo_Hector_Test/.catkin_workspace Normal file
View File

@ -0,0 +1 @@
# This file currently only serves to mark the location of a catkin workspace for tool integration

2
Gazebo_Hector_Test/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
build
devel

361
Gazebo_Hector_Test/Distributed_Gazebo Executable file
View File

@ -0,0 +1,361 @@
#!/bin/bash
#########################################################################
# Author: zhangshuai #
# Date: 2019.03.21 #
# Description: Automated generation of launch and world #
# Usage: ./Distributed_Gazebo configuration_file #
#########################################################################
configuration_file=$1
world="WORLD:"
node="GAZEBO_NODE:"
ip="GAZEBO_IP:"
port="SOCKET_PORT:"
hector="HECTOR_NAME:"
world_name=0
idex=(0 0)
id0=0
id1=1
node_0_hector_count=0
node_1_hector_count=0
gazebo_idex_count=0
# gazebo_count
# gazebo_id
# local_hector_
gazebo_param_0=(0 0)
gazebo_param_1=(0 0)
hector_node_0=(0)
hector_node_1=(0)
echo "begin"
while read line
do
if [[ $line =~ $world ]]
then
temp=${line#*\"}
temp=${temp%\"*}
world_name=$temp
echo ${temp}
fi
if [[ $line =~ $node ]]
then
temp=${line#*\"}
temp=${temp%\"*}
idex[gazebo_idex_count]=$temp
echo ${temp}
echo ${idex[gazebo_idex_count]}
echo "gazebo_idex_count:$gazebo_idex_count"
let gazebo_idex_count++
fi
if [[ $line =~ $ip ]]
then
temp=${line#*\"}
temp=${temp%\"*}
if [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
then
gazebo_param_0[0]=$temp
# echo "gazebo_param_0[0]:${gazebo_param_0[0]}"
fi
if [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
then
gazebo_param_1[0]=$temp
# echo "gazebo_param_1[0]:${gazebo_param_1[0]}"
fi
echo ${temp}
fi
if [[ $line =~ $port ]]
then
temp=${line#*\"}
temp=${temp%\"*}
if [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
then
gazebo_param_0[1]=$temp
fi
if [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
then
gazebo_param_1[1]=$temp
fi
echo ${temp}
fi
if [[ $line =~ $hector ]]
then
temp=${line#*\"}
temp=${temp%\"*}
if [ "${idex[gazebo_idex_count-1]}" == "$id0" ]
then
hector_node_0[node_0_hector_count]=$temp
let node_0_hector_count++
fi
if [ "${idex[gazebo_idex_count-1]}" == "$id1" ]
then
hector_node_1[node_1_hector_count]=$temp
let node_1_hector_count++
fi
echo ${temp}
fi
done < $configuration_file
echo "end"
# echo ${gazebo_param_0[0]}
# echo ${gazebo_param_0[1]}
# echo ${gazebo_param_1[0]}
# echo ${gazebo_param_1[1]}
# echo ${hector_node_0[0]}
# echo ${hector_node_0[1]}
# echo ${hector_node_1[0]}
# echo ${hector_node_1[1]}
echo "node0:${node_0_hector_count}"
echo "node1:${node_1_hector_count}"
file="var/log/messages"
echo "${file#*/}"
echo "${file##*/}"
# 开始写文件
launch_file=(0 0)
world_file=(0 0)
# node_0的launch文件
launch_file[0]="src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_node_0_${node_0_hector_count}_${node_1_hector_count}.launch"
if [ -f ${launch_file[0]} ]
then
`rm -rf ${launch_file[0]}`
fi
`touch ${launch_file[0]}`
# node_1的launch文件
launch_file[1]="src/hector_quadrotor/hector_quadrotor_gazebo/launch/hector_node_1_${node_0_hector_count}_${node_1_hector_count}.launch"
if [ -f ${launch_file[1]} ]
then
`rm -rf ${launch_file[1]}`
fi
`touch ${launch_file[1]}`
# node_0的world文件
world_file[0]="src/hector_quadrotor/hector_quadrotor_gazebo/worlds/${world_name}_node_0.world"
if [ -f ${world_file[0]} ]
then
`rm -rf ${world_file[0]}`
fi
`touch ${world_file[0]}`
# node_1的world文件
world_file[1]="src/hector_quadrotor/hector_quadrotor_gazebo/worlds/${world_name}_node_1.world"
if [ -f ${world_file[1]} ]
then
`rm -rf ${world_file[1]}`
fi
`touch ${world_file[1]}`
# 写world文件
for((i=0;i<$gazebo_idex_count;i++))
do
echo "<?xml version=\"1.0\" ?>" >> ${world_file[i]}
echo "<sdf version=\"1.4\">" >> ${world_file[i]}
echo " <world name=\"default\">" >> ${world_file[i]}
echo "" >> ${world_file[i]}
echo " <!-- A global light source -->" >> ${world_file[i]}
echo " <include>" >> ${world_file[i]}
echo " <uri>model://sun</uri>" >> ${world_file[i]}
echo " </include>" >> ${world_file[i]}
echo "" >> ${world_file[i]}
echo " <include>" >> ${world_file[i]}
echo " <uri>model://${world_name}</uri>" >> ${world_file[i]}
echo " <pose>0 0 0 0 0 0</pose>" >> ${world_file[i]}
echo " </include>" >> ${world_file[i]}
echo "" >> ${world_file[i]}
echo " <scene>" >> ${world_file[i]}
echo " <ambient>0.68 0.68 0.68 1.0</ambient>" >> ${world_file[i]}
echo " <sky>" >> ${world_file[i]}
echo " <sunrise/>" >> ${world_file[i]}
echo " <clouds>" >> ${world_file[i]}
echo " <speed>0</speed>" >> ${world_file[i]}
echo " </clouds>" >> ${world_file[i]}
echo " </sky>" >> ${world_file[i]}
echo " </scene>" >> ${world_file[i]}
echo "" >> ${world_file[i]}
tmp_int=$i
if [ "$tmp_int" == "$id0" ]
then
echo " <distributed type=$i ip=\"${gazebo_param_1[0]}\" port=${gazebo_param_1[1]} simula_entity_num=${node_0_hector_count} shadow_entity_num=${node_1_hector_count} flag=1 />" >> ${world_file[i]}
fi
if [ "$tmp_int" == "$id1" ]
then
echo " <distributed type=$i ip=\"${gazebo_param_0[0]}\" port=${gazebo_param_0[1]} simula_entity_num=${node_1_hector_count} shadow_entity_num=${node_0_hector_count} flag=1 />" >> ${world_file[i]}
fi
echo "" >> ${world_file[i]}
echo " <physics type="ode" name="ode">" >> ${world_file[i]}
echo " <real_time_update_rate>0</real_time_update_rate>" >> ${world_file[i]}
echo " <max_step_size>0.001</max_step_size>" >> ${world_file[i]}
echo " <real_time_factor>0</real_time_factor>" >> ${world_file[i]}
echo " </physics>" >> ${world_file[i]}
echo "" >> ${world_file[i]}
echo " </world>" >> ${world_file[i]}
echo "</sdf>" >> ${world_file[i]}
done
# 写launch文件
for((i=0;i<$gazebo_idex_count;i++))
do
echo "<!-- This is a launch file that runs the bare minimum requirements to get -->" >> ${launch_file[i]}
echo "<!-- gazebo running for a fixed-wing aircraft -->" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
echo "<launch>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
echo " <include file=\"\$(find gazebo_ros)/launch/empty_world.launch\">" >> ${launch_file[i]}
echo " <arg name=\"paused\" value=\"true\"/>" >> ${launch_file[i]}
echo " <arg name=\"gui\" value=\"true\"/>" >> ${launch_file[i]}
echo " <arg name=\"verbose\" value=\"false\"/>" >> ${launch_file[i]}
echo " <arg name=\"debug\" value=\"false\"/>" >> ${launch_file[i]}
echo " <arg name=\"world_name\" value=\"\$(find hector_quadrotor_gazebo)/worlds/${world_file[i]##*/}\"/>" >> ${launch_file[i]}
echo " </include>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
x=0
y=0
t_x=0
t_y=0
# tmp0=$node_0_hector_count+$node_1_hector_count
# tmp1=$node_1_hector_count
tmp_int=$i
if [ "$tmp_int" == "$id0" ]
then
while [ $x -lt $(($node_0_hector_count + $node_1_hector_count)) ]
do
for((j=0;j<$node_0_hector_count;j++))
do
if [ $y -eq 10 ]
then
let t_y=0
let t_x=t_x+5
fi
echo " <group ns=\"${hector_node_0[j]}\">" >> ${launch_file[i]}
echo " <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch\">" >> ${launch_file[i]}
echo " <arg name=\"name\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro\"/>" >> ${launch_file[i]}
echo " <arg name=\"tf_prefix\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
echo " <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
echo " <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
echo " </include>" >> ${launch_file[i]}
echo " </group>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
let t_y=t_y+5
let y++;
let x++;
done
for((j=0;j<$node_1_hector_count;j++))
do
if [ $y -eq 10 ]
then
let t_y=0
let t_x=t_x+5
fi
echo " <group ns=\"${hector_node_1[j]}\">" >> ${launch_file[i]}
echo " <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor_shadow.launch\">" >> ${launch_file[i]}
echo " <arg name=\"name\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_shadow.gazebo.xacro\"/>" >> ${launch_file[i]}
echo " <arg name=\"tf_prefix\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
echo " <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
echo " <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
echo " </include>" >> ${launch_file[i]}
echo " </group>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
let t_y=t_y+5
let y++;
let x++;
done
done
fi
if [ "$tmp_int" == "$id1" ]
then
while [ $x -lt 4 ]
do
for((j=0;j<$node_0_hector_count;j++))
do
if [ $y -eq 10 ]
then
let t_y=0
let t_x=t_x+5
fi
echo " <group ns=\"${hector_node_0[j]}\">" >> ${launch_file[i]}
echo " <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor_shadow.launch\">" >> ${launch_file[i]}
echo " <arg name=\"name\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_shadow.gazebo.xacro\"/>" >> ${launch_file[i]}
echo " <arg name=\"tf_prefix\" value=\"${hector_node_0[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
echo " <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
echo " <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
echo " </include>" >> ${launch_file[i]}
echo " </group>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
let t_y=t_y+5
let y++;
let x++;
done
for((j=0;j<$node_1_hector_count;j++))
do
if [ $y -eq 10 ]
then
let t_y=0
let t_x=t_x+5
fi
echo " <group ns=\"${hector_node_1[j]}\">" >> ${launch_file[i]}
echo " <include file=\"\$(find hector_quadrotor_gazebo)/launch/spawn_quadrotor.launch\">" >> ${launch_file[i]}
echo " <arg name=\"name\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"model\" value=\"\$(find hector_quadrotor_description)/urdf/quadrotor_with_downward_cam.gazebo.xacro\"/>" >> ${launch_file[i]}
echo " <arg name=\"tf_prefix\" value=\"${hector_node_1[j]}\" />" >> ${launch_file[i]}
echo " <arg name=\"x\" value=\"$t_x\" />" >> ${launch_file[i]}
echo " <arg name=\"y\" value=\"$t_y\" />" >> ${launch_file[i]}
echo " <arg name=\"z\" value=\"0.186\" />" >> ${launch_file[i]}
echo " </include>" >> ${launch_file[i]}
echo " </group>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
let t_y=t_y+5
let y++;
let x++;
done
done
fi
echo " <node name=\"simple_takeoff\" pkg=\"hector\" type=\"hector_simple_takeoff\"/>" >> ${launch_file[i]}
echo "" >> ${launch_file[i]}
echo "</launch>" >> ${launch_file[i]}
done

42
Gazebo_Hector_Test/configuration_file Normal file
View File

@ -0,0 +1,42 @@
WORLD:"kunming_airport"
GAZEBO_NODE:"0"
GAZEBO_IP:"192.168.1.7"
SOCKET_PORT:"8888"
HECTOR_NAME:"bebop_0"
HECTOR_NAME:"bebop_1"
HECTOR_NAME:"bebop_2"
HECTOR_NAME:"bebop_3"
HECTOR_NAME:"bebop_4"
HECTOR_NAME:"bebop_5"
HECTOR_NAME:"bebop_6"
HECTOR_NAME:"bebop_7"
HECTOR_NAME:"bebop_8"
HECTOR_NAME:"bebop_9"
HECTOR_NAME:"bebop_10"
HECTOR_NAME:"bebop_11"
HECTOR_NAME:"bebop_12"
HECTOR_NAME:"bebop_13"
HECTOR_NAME:"bebop_14"
GAZEBO_NODE:"1"
GAZEBO_IP:"192.168.1.231"
SOCKET_PORT:"8888"
HECTOR_NAME:"bebop_15"
HECTOR_NAME:"bebop_16"
HECTOR_NAME:"bebop_17"
HECTOR_NAME:"bebop_18"
HECTOR_NAME:"bebop_19"
HECTOR_NAME:"bebop_20"
HECTOR_NAME:"bebop_21"
HECTOR_NAME:"bebop_22"
HECTOR_NAME:"bebop_23"
HECTOR_NAME:"bebop_24"
HECTOR_NAME:"bebop_25"
HECTOR_NAME:"bebop_26"
HECTOR_NAME:"bebop_27"
HECTOR_NAME:"bebop_28"
HECTOR_NAME:"bebop_29"

1
Gazebo_Hector_Test/src/CMakeLists.txt Symbolic link
View File

@ -0,0 +1 @@
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake

23
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/README.md Normal file
View File

@ -0,0 +1,23 @@
# ROSplane
This is intended to eventually be a fully-featured fixed-wing autopilot for ROS using ROSflight as the hardware I/O or fcu_sim as the simulator. It is built according to the method published in [Small Unmanned Aircraft: Theory and Practice](http://uavbook.byu.edu/doku.php) by Beard and McLain, so as to allow anyone to easily understand, modify and use the code. This framework is inherently modular and extensively documented so as to aid the user in understanding and extending for personal use.
The package is intended to be used with fcu\_io or fcu\_sim, for hardware with a naze32 or spracingf3 (or derivatives) or simulation, respectively.
It is a single ROS package, with several nodes.
# - Estimator
The estimator is a standard ekf, as defined mostly in the way in the reference above. It has a attitude filter and a possition filter for gps smoothing. We are probably going to release a full state filter at some point. We are estimating position, velocity, and attitude. The state is then published in the rosplane_msgs/msg/State.msg.
# - Controller
Implements a nested PID controller according to the revefereance above. Requires State and Controller_Commands messages to be published. Altitude is controlled in a longitudinal state machine including take-off, climb, desend, and hold zones. Controller can be tuned using rqt_reconfigure. Again a full state controller is in the works and will probably be on a seperate branch.
# - Path Follower
Gets the aircraft onto a Current_Path using vector fields. K_path and K_orbit gains can also be tuned using rqt_reconfigure.
# - Path Manager
Receives Waypoint messages and creates a path (straight line, fillet, or Dubins) to acheive all the waypoints.

165
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/CMakeLists.txt Normal file
View File

@ -0,0 +1,165 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector)
set(CMAKE_CXX_STANDARD 11)
if (NOT CMAKE_BUILD_TYPE)
# Options: Debug, Release, MinSizeRel, RelWithDebInfo
message(STATUS "No build type selected, default to Release")
set(CMAKE_BUILD_TYPE "Release")
endif()
set(CMAKE_CXX_FLAGS "-fopenmp")
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
rosflight_msgs
hector_msgs
dynamic_reconfigure
sensor_msgs
tf
)
find_package(Eigen3 REQUIRED)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
# Generate dynamic reconfigure parameters in the 'cfg' folder
generate_dynamic_reconfigure_options(
cfg/Follower.cfg
cfg/Controller.cfg
)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES
CATKIN_DEPENDS roscpp rospy rosflight_msgs hector_msgs
# DEPENDS system_lib
)
include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
## Declare a C++ executable
#add_executable(hector_controller
# src/controller_base.cpp
# src/controller_example.cpp)
#add_dependencies(hector_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
#target_link_libraries(hector_controller ${catkin_LIBRARIES})
#add_executable(hector_controller99
# src/controller_base99.cpp
# src/controller_example99.cpp)
#add_dependencies(hector_controller99 ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
#target_link_libraries(hector_controller99 ${catkin_LIBRARIES})
add_executable(hector_pseudo_controller
# src/controller_base.cpp
# src/controller_example.cpp)
src/pseudo_controller_base.cpp
src/pseudo_controller_example.cpp)
add_dependencies(hector_pseudo_controller ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries(hector_pseudo_controller ${catkin_LIBRARIES})
## Declare a C++ executable
#add_executable(hector_estimator
# src/estimator_base.cpp
# src/estimator_example.cpp)
#add_dependencies(hector_estimator ${catkin_EXPORTED_TARGETS})
#target_link_libraries(hector_estimator ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_estimator_statesub
src/estimator_base_statesub.cpp
)
add_dependencies(hector_estimator_statesub ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_estimator_statesub ${catkin_LIBRARIES})
## Declare a C++ executable
#add_executable(hector_estimator_updata_statesub
# src/estimator_base_updata_statesub.cpp
# )
#add_dependencies(hector_estimator_updata_statesub ${catkin_EXPORTED_TARGETS})
#target_link_libraries(hector_estimator_updata_statesub ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_estimator_99pub
src/estimator_base_99pub.cpp
)
add_dependencies(hector_estimator_99pub ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_estimator_99pub ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_path_follower
src/path_follower_example.cpp
src/path_follower_base.cpp)
add_dependencies(hector_path_follower ${catkin_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg)
target_link_libraries(hector_path_follower ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_path_manager
src/path_manager_base.cpp
src/path_manager_example.cpp)
add_dependencies(hector_path_manager ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_path_manager ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_path_planner
src/path_planner.cpp)
add_dependencies(hector_path_planner ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_path_planner ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_path_point_transfer
src/path_point_transfer.cpp)
add_dependencies(hector_path_point_transfer ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_path_point_transfer ${catkin_LIBRARIES})
## Declare a C++ executable
add_executable(hector_simple_takeoff
src/simple_takeoff.cpp)
add_dependencies(hector_simple_takeoff ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_simple_takeoff ${catkin_LIBRARIES})
# ## Declare a C++ executable
# add_executable(udp_send_image
# src/udp_send_image.cpp
# )
# add_dependencies(udp_send_image
# ${catkin_EXPORTED_TARGETS}
# )
# target_link_libraries(udp_send_image
# ${catkin_LIBRARIES}
# ${OpenCV_LIBRARIES}
# )
# ## Declare a C++ executable
# add_executable(udp_recv_image
# src/udp_recv_image.cpp
# )
# add_dependencies(udp_recv_image
# ${catkin_EXPORTED_TARGETS}
# )
# target_link_libraries(udp_recv_image
# ${catkin_LIBRARIES}
# ${OpenCV_LIBRARIES}
# )
# ## Declare a C++ executable
# add_executable(udp_send_state
# src/udp_send_state.cpp)
# add_dependencies(udp_send_state ${catkin_EXPORTED_TARGETS})
# target_link_libraries(udp_send_state ${catkin_LIBRARIES})

58
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/cfg/Controller.cfg Executable file
View File

@ -0,0 +1,58 @@
#!/usr/bin/env python
PACKAGE = "hector"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# trim
trim = gen.add_group("Trim")
trim.add("TRIM_E", double_t, 0, "Elevator trim", 0, -1, 1)
trim.add("TRIM_A", double_t, 0, "Aileron trim", 0, -1, 1)
trim.add("TRIM_R", double_t, 0, "Rudder trim", 0, -1, 1)
trim.add("TRIM_T", double_t, 0, "Throttle trim", 0.6, 0, 1)
# course hold
course = gen.add_group("Course")
course.add("COURSE_KP", double_t, 0, "Course proportional gain", 0.7329, 0, 2)
course.add("COURSE_KD", double_t, 0, "Course derivative gain", 0, -1, 0)
course.add("COURSE_KI", double_t, 0, "Course integral gain", 0.0, 0, 0.2)
# roll hold
roll = gen.add_group("Roll")
roll.add("ROLL_KP", double_t, 0, "Roll proportional gain", 1.17, 0, 3)
roll.add("ROLL_KD", double_t, 0, "Roll derivative gain", -0.13, -1, 0)
roll.add("ROLL_KI", double_t, 0, "Roll integral gain", 0, 0, 0.2)
# pitch hold
pitch = gen.add_group("Pitch")
pitch.add("PITCH_KP", double_t, 0, "Pitch proportional gain", 1.0, 0, 3)
pitch.add("PITCH_KD", double_t, 0, "Pitch derivative gain", -0.17, -0.4, 0)
pitch.add("PITCH_KI", double_t, 0, "Pitch integral gain", 0, 0, 0.2)
pitch.add("PITCH_FF", double_t, 0, "Pitch feed forward value", 0, -1, 1)
# airspeed with pitch hold
as_pitch = gen.add_group("Airspeed with Pitch")
as_pitch.add("AS_PITCH_KP", double_t, 0, "Airspeed with pitch proportional gain", -0.0713, 0, 0.2)
as_pitch.add("AS_PITCH_KD", double_t, 0, "Airspeed with pitch derivative gain", -0.0635, -0.2, 0)
as_pitch.add("AS_PITCH_KI", double_t, 0, "Airspeed with pitch integral gain", 0, 0, 0.2)
# airspeed with throttle hold
as_thr = gen.add_group("Airspeed with Throttle")
as_thr.add("AS_THR_KP", double_t, 0, "Airspeed with throttle proportional gain", 3.2, 0, 10)
as_thr.add("AS_THR_KD", double_t, 0, "Airspeed with throttle derivative gain", 0, -5, 0)
as_thr.add("AS_THR_KI", double_t, 0, "Airspeed with throttle integral gain", 1.0, 0, 10)
# altitude hold
alt = gen.add_group("Altitude")
alt.add("ALT_KP", double_t, 0, "Altitude proportional gain", 0.045, 0, 0.1)
alt.add("ALT_KD", double_t, 0, "Altitude derivative gain", 0, -0.05, 0)
alt.add("ALT_KI", double_t, 0, "Altitude integral gain", 0.01, 0, 0.05)
# side-slip hold
sideslip = gen.add_group("Side Slip")
sideslip.add("BETA_KP", double_t, 0, "Side slip proportional gain", -0.1164, 0, 0.3)
sideslip.add("BETA_KD", double_t, 0, "Side slip derivative gain", 0, -0.15, 0)
sideslip.add("BETA_KI", double_t, 0, "Side slip integral gain", -0.0037111, 0, 0.05)
exit(gen.generate(PACKAGE, "rosplane", "Controller"))

17
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/cfg/Follower.cfg Executable file
View File

@ -0,0 +1,17 @@
#!/usr/bin/env python
PACKAGE = "hector"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Chi Infinity
gen.add("CHI_INFTY", double_t, 0, "Chi Infinity", 1.0472, 0 , 1.5708)
# K Path
gen.add("K_PATH", double_t, 0, "K Path", 0.025, 0, 1)
# K Orbit
gen.add("K_ORBIT", double_t, 0, "K Orbit", 4.0, 0, 15)
exit(gen.generate(PACKAGE, "rosplane", "Follower"))

145
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/controller_base.h Normal file
View File

@ -0,0 +1,145 @@
/**
* @file controller_base.h
*
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef CONTROLLER_BASE_H
#define CONTROLLER_BASE_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Controller_Commands.h>
#include <hector_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <hector/ControllerConfig.h>
namespace hector
{
enum class alt_zones
{
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD
};
class controller_base
{
public:
controller_base();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float psi; //
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};
struct output_s
{
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
float delta_e; //升降舵偏移量-拉高降低
float delta_r; //方向舵偏移量-保持稳定，消除测滑
float delta_t; //油门偏移量
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher actuators_pub_;
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
hector_msgs::Controller_Commands controller_commands_;
hector_msgs::State vehicle_state_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
void controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<hector::ControllerConfig> server_;
dynamic_reconfigure::Server<hector::ControllerConfig>::CallbackType func_;
void reconfigure_callback(hector::ControllerConfig &config, uint32_t level);
/**
* Convert from deflection angle to pwm
*/
void convert_to_pwm(struct output_s &output);
/**
* Publish the outputs
*/
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // CONTROLLER_BASE_H

145
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/controller_base99.h Normal file
View File

@ -0,0 +1,145 @@
/**
* @file controller_base99.h
*
* Base class definition for autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef CONTROLLER_BASE99_H
#define CONTROLLER_BASE99_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Controller_Commands.h>
#include <hector_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <hector/ControllerConfig.h>
namespace hector
{
enum class alt_zones
{
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD
};
class controller_base99
{
public:
controller_base99();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float h; /** altitude */
float va; /** airspeed */
float phi; /** roll angle */
float psi; //
float theta; /** pitch angle */
float chi; /** course angle */
float p; /** body frame roll rate */
float q; /** body frame pitch rate */
float r; /** body frame yaw rate */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (rad) */
float phi_ff; /** feed forward term for orbits (rad) */
};
struct output_s
{
float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
float delta_e; //升降舵偏移量-拉高降低
float delta_r; //方向舵偏移量-保持稳定，消除测滑
float delta_t; //油门偏移量
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
double c_kd;
double c_ki;
double r_kp;
double r_kd;
double r_ki;
double p_kp;
double p_kd;
double p_ki;
double p_ff;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_kp;
double a_t_kd;
double a_t_ki;
double a_kp;
double a_kd;
double a_ki;
double b_kp;
double b_kd;
double b_ki;
double trim_e;
double trim_a;
double trim_r;
double trim_t;
double max_e;
double max_a;
double max_r;
double max_t;
double pwm_rad_e;
double pwm_rad_a;
double pwm_rad_r;
};
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher actuators_pub_;
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
hector_msgs::Controller_Commands controller_commands_;
hector_msgs::State vehicle_state_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
void controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<hector::ControllerConfig> server_;
dynamic_reconfigure::Server<hector::ControllerConfig>::CallbackType func_;
void reconfigure_callback(hector::ControllerConfig &config, uint32_t level);
/**
* Convert from deflection angle to pwm
*/
void convert_to_pwm(struct output_s &output);
/**
* Publish the outputs
*/
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // CONTROLLER_BASE99_H

53
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/controller_example.h Normal file
View File

@ -0,0 +1,53 @@
#ifndef CONTROLLER_EXAMPLE_H
#define CONTROLLER_EXAMPLE_H
#include "controller_base.h"
namespace hector
{
class controller_example : public controller_base
{
public:
controller_example();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error_;
float r_integrator;
float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error_;
float p_integrator_;
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // CONTROLLER_EXAMPLE_H

56
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/controller_example99.h Normal file
View File

@ -0,0 +1,56 @@
#ifndef CONTROLLER_EXAMPLE99_H
#define CONTROLLER_EXAMPLE99_H
#include "controller_base99.h"
namespace hector
{
class controller_example99 : public controller_base99
{
public:
controller_example99();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
float r_error_;
float r_integrator;
float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
float p_error_;
float p_integrator_;
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
float decend_airspeed_with_pitch_hold(float h_c, float h, const struct params_s &params, float Ts);
float decend_airspeed_oil(float h_c, float h, const params_s &params, float Ts);
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // CONTROLLER_EXAMPLE99_H

131
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/estimator_base.h Normal file
View File

@ -0,0 +1,131 @@
/**
* @file estimator_base.h
*
* Base class definition for autopilot estimator in chapter 8 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
*/
#ifndef ESTIMATOR_BASE_H
#define ESTIMATOR_BASE_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <rosflight_msgs/GPS.h>
#include <sensor_msgs/Imu.h>
#include <rosflight_msgs/Barometer.h>
#include <rosflight_msgs/Airspeed.h>
#include <rosflight_msgs/Status.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
class estimator_base
{
public:
estimator_base();
protected:
struct input_s
{
float gyro_x;
float gyro_y;
float gyro_z;
float accel_x;
float accel_y;
float accel_z;
float static_pres;
float diff_pres;
bool gps_new;
float gps_n;
float gps_e;
float gps_h;
float gps_Vg;
float gps_course;
bool status_armed;
bool armed_init;
};
struct output_s
{
float pn;
float pe;
float h;
float Va;
float alpha;
float beta;
float phi;
float theta;
float psi;
float chi;
float p;
float q;
float r;
float Vg;
float wn;
float we;
};
struct params_s
{
double gravity;
double rho;
double sigma_accel;
double sigma_n_gps;
double sigma_e_gps;
double sigma_Vg_gps;
double sigma_course_gps;
double Ts;
};
virtual void estimate(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber gps_sub_;
ros::Subscriber imu_sub_;
ros::Subscriber baro_sub_;
ros::Subscriber airspeed_sub_;
ros::Subscriber status_sub_;
void update(const ros::TimerEvent &);
void gpsCallback(const rosflight_msgs::GPS &msg);
void imuCallback(const sensor_msgs::Imu &msg);
void baroAltCallback(const rosflight_msgs::Barometer &msg);
void airspeedCallback(const rosflight_msgs::Airspeed &msg);
void statusCallback(const rosflight_msgs::Status &msg);
double update_rate_;
ros::Timer update_timer_;
std::string gps_topic_;
std::string imu_topic_;
std::string baro_topic_;
std::string airspeed_topic_;
std::string status_topic_;
bool gps_new_;
bool gps_init_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool armed_first_time_; /**< Arm before starting estimation */
bool baro_init_; /**< Initial barometric pressure */
float init_static_; /**< Initial static pressure (mbar) */
int baro_count_; /**< Used to grab the first set of baro measurements */
std::vector<float> init_static_vector_; /**< Used to grab the first set of baro measurements */
struct params_s params_;
struct input_s input_;
};
} //end namespace
#endif // ESTIMATOR_BASE_H

45
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/estimator_base_99pub.h Normal file
View File

@ -0,0 +1,45 @@
// modified by kobe and zhangshuai
#ifndef ESTIMATOR_BASE_PUB_H
#define ESTIMATOR_BASE_PUB_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/State29.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
class estimator_base_99pub
{
public:
estimator_base_99pub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state99_pub_;
ros::Subscriber true_state_sub_;
void update(const ros::TimerEvent &);
void truestateCallback(const hector_msgs::StateConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string truth_topic_;
bool state_init_;
hector_msgs::State true_state_;
};
} //end namespace
#endif //

46
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/estimator_base_statesub.h Normal file
View File

@ -0,0 +1,46 @@
// modified by kobe and zhangshuai
#ifndef ESTIMATOR_BASE_SUB_H
#define ESTIMATOR_BASE_SUB_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/State29.h> //采用新的消息头文件
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
class estimator_base_statesub
{
public:
estimator_base_statesub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber state29_sub_;
void update(const ros::TimerEvent &);
void state99Callback(const hector_msgs::State29ConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string state29_topic_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool state_init_;
hector_msgs::State29 state29_;
};
} //end namespace
#endif //

46
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/estimator_base_updata_statesub.h Normal file
View File

@ -0,0 +1,46 @@
// modified by kobe
#ifndef ESTIMATOR_BASE_UPDATA_SUB_H
#define ESTIMATOR_BASE_UPDATA_SUB_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Up_Data_New.h>
#include <math.h>
#include <Eigen/Eigen>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
class estimator_base_updata_statesub
{
public:
estimator_base_updata_statesub();
protected:
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher vehicle_state_pub_;
ros::Subscriber state29_sub_;
void update(const ros::TimerEvent &);
void state99Callback(const hector_msgs::Up_Data_NewConstPtr &msg);
double update_rate_;
ros::Timer update_timer_;
std::string state29_topic_;
double init_lat_; /**< Initial latitude in degrees */
double init_lon_; /**< Initial longitude in degrees */
float init_alt_; /**< Initial altitude in meters above MSL */
bool state_init_;
hector_msgs::Up_Data_New state29_;
};
} //end namespace
#endif //

73
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/estimator_example.h Normal file
View File

@ -0,0 +1,73 @@
#ifndef ESTIMATOR_EXAMPLE_H
#define ESTIMATOR_EXAMPLE_H
#include "estimator_base.h"
#include <math.h>
#include <Eigen/Eigen>
namespace hector
{
class estimator_example : public estimator_base
{
public:
estimator_example();
private:
virtual void estimate(const params_s &params, const input_s &input, output_s &output);
// float gps_n_old_;
// float gps_e_old_;
// float gps_Vg_old_;
// float gps_course_old_;
double lpf_a_;
float alpha_;
float alpha1_;
int N_;
float lpf_gyro_x_;
float lpf_gyro_y_;
float lpf_gyro_z_;
float lpf_static_;
float lpf_diff_;
float lpf_accel_x_;
float lpf_accel_y_;
float lpf_accel_z_;
float phat_;
float qhat_;
float rhat_;
float Vwhat_;
float phihat_;
float thetahat_;
float psihat_;
Eigen::Vector2f xhat_a_; // 2
Eigen::Matrix2f P_a_; // 2x2
Eigen::VectorXf xhat_p_; // 7
Eigen::MatrixXf P_p_; // 7x7
Eigen::Matrix2f Q_a_; // 2x2
float R_accel_;
Eigen::Vector2f f_a_; // 2
Eigen::Matrix2f A_a_; // 2x2
float h_a_;
Eigen::Vector2f C_a_; // 2
Eigen::Vector2f L_a_; // 2
Eigen::MatrixXf Q_p_; // 7x7
Eigen::MatrixXf R_p_; // 6x6
Eigen::VectorXf f_p_; // 7
Eigen::MatrixXf A_p_; // 7x7
float h_p_;
Eigen::VectorXf C_p_; // 7
Eigen::VectorXf L_p_; // 7
void check_xhat_a();
};
} //end namespace
#endif // ESTIMATOR_EXAMPLE_H

103
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/path_follower_base.h Normal file
View File

@ -0,0 +1,103 @@
//* @author AIRC-DA group,questions contack kobe.
#ifndef PATH_FOLLOWER_BASE_H
#define PATH_FOLLOWER_BASE_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Controller_Commands.h>
#include <dynamic_reconfigure/server.h>
#include <hector/FollowerConfig.h>
#include <hector_msgs/Current_Path.h>
#include <nav_msgs/Odometry.h>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
enum class path_type
{
Orbit,
Line,
Star //add by kobe
};
class path_follower_base
{
public:
path_follower_base();
float spin();
protected:
struct input_s
{
enum path_type p_type;
float Va_d;
float r_path[3];
float q_path[3];
float c_orbit[3];
float rho_orbit;
int lam_orbit;
float pn; /** position north */
float pe; /** position east */
float h; /** altitude */
float Va; /** airspeed */
float chi; /** course angle */
float h_c_path; /** goal attitude */
float Va_c_path; /** goal attitude */
bool landing;
bool takeoff;
};
struct output_s
{
double Va_c; /** commanded airspeed (m/s) */
double h_c; /** commanded altitude (m) */
double chi_c; /** commanded course (rad) */
double phi_ff; /** feed forward term for orbits (rad) */
};
struct params_s
{
double chi_infty;
double k_path;
double k_orbit;
};
virtual void follow(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber bebop1_state_sub_;
ros::Subscriber current_path_sub_;
ros::Publisher controller_commands_pub_;
double update_rate_ = 100.0;
ros::Timer update_timer_;
hector_msgs::Controller_Commands controller_commands_;
struct params_s params_; /**< params */
struct input_s input_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
bool state_init_;
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
void current_path_callback(const hector_msgs::Current_PathConstPtr &msg);
bool current_path_init_;
dynamic_reconfigure::Server<hector::FollowerConfig> server_;
dynamic_reconfigure::Server<hector::FollowerConfig>::CallbackType func_;
void reconfigure_callback(hector::FollowerConfig &config, uint32_t level);
void update(const ros::TimerEvent &);
};
} // end namespace
#endif // PATH_FOLLOWER_BASE_H

18
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/path_follower_example.h Normal file
View File

@ -0,0 +1,18 @@
#ifndef PATH_FOLLOWER_EXAMPLE_H
#define PATH_FOLLOWER_EXAMPLE_H
#include "path_follower_base.h"
namespace hector
{
class path_follower_example : public path_follower_base
{
public:
path_follower_example();
private:
virtual void follow(const struct params_s &params, const struct input_s &input, struct output_s &output);
};
} //end namespace
#endif // PATH_FOLLOWER_EXAMPLE_H

122
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/path_manager_base.h Normal file
View File

@ -0,0 +1,122 @@
//* @author AIRC-DA group,questions contack kobe.
#ifndef PATH_MANAGER_BASE_H
#define PATH_MANAGER_BASE_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Current_Path.h>
#include <hector_msgs/Goal_Info.h>
#include <hector_msgs/Down_Data_New.h>
// #include <commond_msgs/Waypoint.h>
#include <hector_msgs/Waypoint.h>
#include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/FluidPressure.h>
#include <math.h>
#include <Eigen/Eigen>
// #include <hector/ControllerConfig.h>
#include <nav_msgs/Odometry.h>
#define EARTH_RADIUS 6378145.0f
namespace hector
{
class path_manager_base
{
public:
path_manager_base();
protected:
//modified by kobe
struct waypoint_s
{
float w[3];
float lat;
float lon;
float chi_d;
bool chi_valid;
float Va_d;
bool landing;
bool takeoff;
};
std::vector<waypoint_s> waypoints_;
waypoint_s waypoint_start_;
waypoint_s waypoint_end_;
bool waypoint_received_;
int num_waypoints_;
int idx_a_; /** index to the waypoint that was most recently achieved */
float min_distance_;
double min_distance_titude;
bool show; //add by kobe
bool show2;
struct input_s
{
float pn; /** position north */
float pe; /** position east */
float lat; //latitude
float lon; //lontitude
float h; /** altitude */
float chi; /** course angle */
float va; //add by kobe
};
struct output_s
{
float Va_c;
int flag; /** Inicates strait line or orbital path (1 is line, 0 is orbit, 2 is star) modified by kobe*/
float Va_d; /** Desired airspeed (m/s) */
float r[3]; /** Vector to origin of straight line path (m) */
float q[3]; /** Unit vector, desired direction of travel for line path */
float c[3]; /** Center of orbital path (m) */
float gxy[2]; /** x-y-coordinate (m) */
float gll[2]; /** latitude-longtitude */
float rho; /** Radius of orbital path (m) */
float h_c;
bool landing;
bool takeoff;
int8_t lambda; /** Direction of orbital path (cw is 1, ccw is -1) */
};
struct params_s
{
double R_min;
};
virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_; /**< vehicle state subscription */
ros::Subscriber bebop1_state_sub_;
ros::Subscriber new_waypoint_sub_; /**< new waypoint subscription */
ros::Publisher current_path_pub_; /**< controller commands publication */
ros::Publisher goal_info_pub_; /**< goal info publication */
ros::Publisher down_data_pub_;
ros::Subscriber waypoint_received_sub_;
struct params_s params_;
hector_msgs::State vehicle_state_; /**< vehicle state */
nav_msgs::Odometry bebop1_state_;
double update_rate_;
ros::Timer update_timer_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
bool state_init_;
void new_waypoint_callback(const hector_msgs::Waypoint &msg);
void current_path_publish(const ros::TimerEvent &);
void waypoint_received_callback(const hector_msgs::Waypoint &msg);
};
} //end namespace
#endif // PATH_MANAGER_BASE_H

78
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/path_manager_example.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef PATH_MANAGER_EXAMPLE_H
#define PATH_MANAGER_EXAMPLE_H
#include "path_manager_base.h"
#include <Eigen/Eigen>
//#include <lib/mathlib/mathlib.h>
#define M_PI_F 3.14159265358979323846f
#define M_PI_2_F 1.57079632679489661923f
namespace hector
{
enum class fillet_state
{
STRAIGHT,
ORBIT
};
enum class dubin_state
{
FIRST,
BEFORE_H1,
BEFORE_H1_WRONG_SIDE,
STRAIGHT,
BEFORE_H3,
BEFORE_H3_WRONG_SIDE
};
class path_manager_example : public path_manager_base
{
public:
path_manager_example();
private:
virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output);
void manage_line(const struct params_s &params, const struct input_s &input, struct output_s &output);
void manage_star(const struct params_s &params, const struct input_s &input, struct output_s &output);//add by kobe
void manage_fillet(const struct params_s &params, const struct input_s &input, struct output_s &output);
fillet_state fil_state_;
void manage_dubins(const struct params_s &params, const struct input_s &input, struct output_s &output);
dubin_state dub_state_;
double earth_distance(double, double, double, double);
bool judge_condition1;
bool judge_condition2;
bool judge_condition3;
bool judge_condition4;
bool judge_condition5;
struct dubinspath_s
{
Eigen::Vector3f ps; /** the start position */
float chis; /** the start course angle */
Eigen::Vector3f pe; /** the end position */
float chie; /** the end course angle */
float R; /** turn radius */
float L; /** length of the path */
Eigen::Vector3f cs; /** center of the start circle */
int lams; /** direction of the start circle */
Eigen::Vector3f ce; /** center of the endcircle */
int lame; /** direction of the end circle */
Eigen::Vector3f w1; /** vector defining half plane H1 */
Eigen::Vector3f q1; /** unit vector along striaght line path */
Eigen::Vector3f w2; /** vector defining half plane H2 */
Eigen::Vector3f w3; /** vector defining half plane H3 */
Eigen::Vector3f q3; /** unit vector defining direction of half plane H3 */
};
struct dubinspath_s dubinspath_;
void dubinsParameters(const struct waypoint_s start_node, const struct waypoint_s end_node, float R);
Eigen::Matrix3f rotz(float theta);
float mo(float in);
};
} //end namespace
#endif // PATH_MANAGER_EXAMPLE_H

123
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/path_point_transfer.h Normal file
View File

@ -0,0 +1,123 @@
/**
* @file path_manager_base.h
*
* Base class definition for autopilot path follower in chapter 10 of UAVbook, see http://uavbook.byu.edu/doku.php
*
* @author Gary Ellingson <gary.ellingson@byu.edu>
* adapted by Judd Mehr and Brian Russel for RosPlane software
*/
#ifndef PATH_POINT_TRANSFER_H
#define PATH_POINT_TRANSFER_H
#include <ros/ros.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Current_Path.h>
#include <hector_msgs/Down_Data_New.h>
// #include <hector_msgs/Waypoint.h>
// #include <sensor_msgs/Imu.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <sensor_msgs/FluidPressure.h>
#include <math.h>
#include <Eigen/Eigen>
#include <hector/ControllerConfig.h>
#include <nav_msgs/Odometry.h>
namespace hector
{
class path_point_transfer
{
public:
path_point_transfer();
~path_point_transfer();
protected:
//modified by kobe
struct waypoint_s
{
float w[3];
float lat;
float lon;
float chi_d;
bool chi_valid;
float Va_d;
};
std::vector<waypoint_s> waypoints_;
waypoint_s waypoint_start_;
waypoint_s waypoint_end_;
bool waypoint_received_;
int num_waypoints_;
int idx_a_; /** index to the waypoint that was most recently achieved */
float min_distance_;
double min_distance_titude;
bool show; //add by kobe
// struct input_s
// {
// float pn; /** position north */
// float pe; /** position east */
// float lat; //latitude
// float lon; //lontitude
// float h; /** altitude */
// float chi; /** course angle */
// float va; //add by kobe
// };
// struct output_s
// {
// int flag; /** Inicates strait line or orbital path (1 is line, 0 is orbit, 2 is star) modified by kobe*/
// float Va_d; /** Desired airspeed (m/s) */
// float r[3]; /** Vector to origin of straight line path (m) */
// float q[3]; /** Unit vector, desired direction of travel for line path */
// float c[3]; /** Center of orbital path (m) */
// float gxy[2]; /** x-y-coordinate (m) */
// float gll[2]; /** latitude-longtitude */
// float rho; /** Radius of orbital path (m) */
// float h_c;
// int8_t lambda; /** Direction of orbital path (cw is 1, ccw is -1) */
// };
struct params_s
{
double R_min;
};
// virtual void manage(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_; /**< vehicle state subscription */
ros::Subscriber bebop1_state_sub_;
ros::Subscriber new_waypoint_sub_; /**< new waypoint subscription */
ros::Publisher current_path_pub_; /**< controller commands publication */
// ros::Publisher goal_info_pub_; /**< goal info publication */
// ros::Publisher down_data_pub_;
ros::Subscriber waypoint_received_sub_;
struct params_s params_;
hector_msgs::State vehicle_state_; /**< vehicle state */
hector_msgs::Down_Data_New point_state_;
nav_msgs::Odometry bebop1_state_;
double update_rate_;
ros::Timer update_timer_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
// float Quaternion_to_Euler(float, float, float, float);
bool state_init_;
void new_waypoint_callback(const hector_msgs::Down_Data_NewConstPtr &msg);
void current_path_publish(const ros::TimerEvent &);
// void waypoint_received_callback(const hector_msgs::Waypoint &msg);
};
} // namespace hector
#endif // PATH_MANAGER_BASE_H

137
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/pseudo_controller_base.h Normal file
View File

@ -0,0 +1,137 @@
/**
*
* Refer autopilot controller in chapter 6 of UAVbook, see http://uavbook.byu.edu/doku.php
*pseudo fix-wing 四旋翼模拟固定翼飞行，订阅当前实际及命令的速度、高度和角度，发布向前，向上下速度和角度
* @author AIRC-DA group,questions contack kobe.
*/
#ifndef PSEUDO_CONTROLLER_BASE_H
#define PSEUDO_CONTROLLER_BASE_H
#include <ros/ros.h>
#include <rosflight_msgs/Command.h>
#include <geometry_msgs/Twist.h>
#include <hector_msgs/State.h>
#include <hector_msgs/Controller_Commands.h>
#include <hector_msgs/Controller_Internals.h>
#include <dynamic_reconfigure/server.h>
#include <hector/ControllerConfig.h>
#include <nav_msgs/Odometry.h>
namespace hector
{
enum class alt_zones
{
PREPARE,
TAKE_OFF,
CLIMB,
DESCEND,
ALTITUDE_HOLD,
LANDING
};
class pseudo_controller_base
{
public:
pseudo_controller_base();
float spin();
protected:
struct input_s
{
float Ts; /** time step */
float va; /** airspeed */
float h; /** altitude */
float chi; /** course angle */
float Va_c; /** commanded airspeed (m/s) */
float h_c; /** commanded altitude (m) */
float chi_c; /** commanded course (north)(-pi to pi,clockwise:-) */
bool takeoff;
bool landing;
};
struct output_s
{
//float theta_c; //俯仰角--即平行于机身轴线并指向飞行器前方的向量与地面的夹角
//float phi_c; //滚转角--机体坐标系OZb轴与通过机体OXb轴的铅垂面间的夹角，机体向右滚为正，反之为负
//float delta_a; //副翼偏移量-转弯用-副翼跳变造成油门和升降舵数值的波动
//float delta_e; //升降舵偏移量-拉高降低
//float delta_r; //方向舵偏移量-保持稳定，消除测滑
//float delta_t; //油门偏移量
float forward_trans; //前进力度
float up_down_trans; //向上、向下力度,上正下负
float left_right_trans; //向左右力度,左正右负
float roll_trans; //左右滚转，x轴,逆时针为正，顺时针为负，[-pi/2,pi/2]
float rotate_trans; //z旋转,逆时针为正，顺时针为负，[-pi/2,pi/2]
float rotate_value;
alt_zones current_zone;
};
struct params_s
{
double alt_hz; /**< altitude hold zone */
double alt_toz; /**< altitude takeoff zone */
double tau;
double c_kp;
//double c_kd;
double c_ki;
double a_p_kp;
double a_p_kd;
double a_p_ki;
double a_t_ki;
double a_t_kp;
double a_t_kd;
double a_kp;
double a_kd;
double a_ki;
double trim_t;
//double max_a;y
double max_t;
};
bool reached;
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output) = 0;
private:
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Subscriber vehicle_state_sub_;
ros::Subscriber bebop1_state_sub_;
ros::Subscriber controller_commands_sub_;
ros::Publisher pesudors_pub_;//actuators
ros::Publisher internals_pub_;
ros::Timer act_pub_timer_;
struct params_s params_; /**< params */
hector_msgs::Controller_Commands controller_commands_;
hector_msgs::State vehicle_state_;
nav_msgs::Odometry bebop1_state_;
void vehicle_state_callback(const hector_msgs::StateConstPtr &msg);
void bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg);
float Quaternion_to_Euler(float,float,float,float);
void controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg);
bool command_recieved_;
dynamic_reconfigure::Server<hector::ControllerConfig> server_;
dynamic_reconfigure::Server<hector::ControllerConfig>::CallbackType func_;
void reconfigure_callback(hector::ControllerConfig &config, uint32_t level);
/*Convert from deflection angle to pwm*/
//void convert_to_pwm(struct output_s &output);
/* Publish the outputs */
void actuator_controls_publish(const ros::TimerEvent &);
};
} //end namespace
#endif // PSEUDO_CONTROLLER_BASE_H

66
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/pseudo_controller_example.h Normal file
View File

@ -0,0 +1,66 @@
#ifndef PSEUDO_CONTROLLER_EXAMPLE_H
#define PSEUDO_CONTROLLER_EXAMPLE_H
#include "pseudo_controller_base.h"
namespace hector
{
class pseudo_controller_example : public pseudo_controller_base
{
public:
pseudo_controller_example();
private:
virtual void control(const struct params_s &params, const struct input_s &input, struct output_s &output);
alt_zones current_zone;
float pseudo_course_hold(float chi,float chi_c, const params_s &params, float Ts);
//float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s &params, float Ts);
float c_error_;
float c_integrator_;
float r_error_;
float r_integrator_;
//float roll_hold(float phi_c, float phi, float p, const struct params_s &params, float Ts);
//float r_error_;
//float r_integrator_;
//float pitch_hold(float theta_c, float theta, float q, const struct params_s &params, float Ts);
//float p_error_;
//float p_integrator_;
float pseudo_course(float chi, float chi_c);
float pseudo_left_right(float chi, float chi_c, float forward_trans);
//float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float pseudo_pitch_hold(float h_c, float h, const struct params_s &params, float Ts);
float ap_error_;
float ap_integrator_;
float ap_differentiator_;
//float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s &params, float Ts);
float pseudo_throttle_hold(float Va_c, float Va, const params_s &params, float Ts);
float at_error_;
float at_integrator_;
float at_differentiator_;
//float altitiude_hold(float h_c, float h, const struct params_s &params, float Ts);
float pseudo_altitiude_hold(float h_c, float h, const params_s &params, float Ts);
float a_error_;
float a_integrator_;
float a_differentiator_;
// float cooridinated_turn_hold(float v, const struct params_s &params, float Ts);
// float ct_error_;
// float ct_integrator_;
// float ct_differentiator_;
float sat(float value, float up_limit, float low_limit);
};
} //end namespace
#endif // PSEUDO_CONTROLLER_EXAMPLE_H

42
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/udp_recv_image.h Normal file
View File

@ -0,0 +1,42 @@
/************************************************************************
* *
* Author: ake *
* Date: 2018-10-23 *
* Description: recv image of the camera to server with udp *
* *
************************************************************************/
#ifndef _UDP_RECV_IMAGE_H_
#define _UDP_RECV_IMAGE_H_
#include "packageHeader.h"
namespace rosplane
{
class udp_recv_image
{
public:
udp_recv_image();
~udp_recv_image();
void recv_image();
void udp_close();
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it;
image_transport::Publisher image_pub;
struct PackageHeader* packageHead;
int server_sockfd;
struct sockaddr_in my_addr;
unsigned int sin_size;
struct sockaddr_in remote_addr;
unsigned char frameBuffer[BUF_SIZE];
};
}
#endif

42
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/include/udp_send_image.h Normal file
View File

@ -0,0 +1,42 @@
/************************************************************************
* *
* Author: ake *
* Date: 2018-10-23 *
* Description: send image of the camera to server with udp *
* *
************************************************************************/
#ifndef _UDP_SEND_IMAGE_H_
#define _UDP_SEND_IMAGE_H_
#include "packageHeader.h"
namespace hector
{
class udp_send_image
{
public:
udp_send_image();
~udp_send_image();
void imageCallback(const sensor_msgs::ImageConstPtr& tem_msg);
void udp_close();
private:
ros::NodeHandle nh_;
image_transport::ImageTransport it;
image_transport::Subscriber image_sub;
struct PackageHeader packageHead;
int client_sockfd;
bool bConnected;
struct sockaddr_in remote_addr;
unsigned char frameBuffer[BUF_SIZE];
};
}
#endif

33
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/package.xml Normal file
View File

@ -0,0 +1,33 @@
<?xml version="1.0"?>
<package>
<name>hector</name>
<version>0.0.0</version>
<description>The hector package</description>
<maintainer email="gary.ellingson@byu.edu">Gary Ellingson</maintainer>
<author email="gary.ellingson@byu.edu">Gary Ellingson</author>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cmake_modules</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>rosflight_msgs</build_depend>
<build_depend>hector_msgs</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>rosflight_msgs</run_depend>
<run_depend>hector_msgs</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<export>
</export>
</package>

33
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/param/aerosonde.yaml Normal file
View File

@ -0,0 +1,33 @@
TRIM_E: 0
TRIM_A: 0
TRIM_R: 0
TRIM_T: 0.6
COURSE_KP: 0.7329
COURSE_KD: 0
COURSE_KI: 0.07
ROLL_KP: 1.17
ROLL_KD: -0.13
ROLL_KI: 0
PITCH_KP: 0.1
PITCH_KD: -0.017
PITCH_KI: 0
PITCH_FF: 0
AS_PITCH_KP: -0.0713
AS_PITCH_KD: -0.0635
AS_PITCH_KI: 0
AS_THR_KP: 3.2
AS_THR_KD: 0
AS_THR_KI: 1.0
ALT_KP: 0.045
ALT_KD: 0
ALT_KI: 0.01
BETA_KP: -0.1164
BETA_KD: 0
BETA_KI: -0.0037111

41
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/param/bixler.yaml Normal file
View File

@ -0,0 +1,41 @@
TRIM_E: 0
TRIM_A: 0
TRIM_R: 0
TRIM_T: 0.6
COURSE_KP: 0.7329
COURSE_KD: 0
COURSE_KI: 0.0
ROLL_KP: 1.2855
ROLL_KD: -0.1
ROLL_KI: 0
PITCH_KP: 1.0
PITCH_KD: -0.17
PITCH_KI: 0
PITCH_FF: 0
AS_PITCH_KP: -0.0713
AS_PITCH_KD: -0.0635
AS_PITCH_KI: 0
AS_THR_KP: 3.2
AS_THR_KD: 0
AS_THR_KI: 1.0
ALT_KP: 0.045
ALT_KD: 0
ALT_KI: 0.01
BETA_KP: -0.1164
BETA_KD: 0
BETA_KI: -0.0037111
CHI_INFTY: 1.0472
K_PATH: 0.025
K_ORBIT: 2.00

203
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/controller_base.cpp Normal file
View File

@ -0,0 +1,203 @@
#include "controller_base.h"
#include "controller_example.h"
namespace hector
{
controller_base::controller_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base::vehicle_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base::controller_commands_callback,
this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
func_ = boost::bind(&controller_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
internals_pub_ = nh_.advertise<hector_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base::actuator_controls_publish, this);
command_recieved_ = false;
}
void controller_base::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void controller_base::controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void controller_base::reconfigure_callback(hector::ControllerConfig &config, uint32_t level)
{
params_.trim_e = config.TRIM_E;
params_.trim_a = config.TRIM_A;
params_.trim_r = config.TRIM_R;
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.r_kp = config.ROLL_KP;
params_.r_kd = config.ROLL_KD;
params_.r_ki = config.ROLL_KI;
params_.p_kp = config.PITCH_KP;
params_.p_kd = config.PITCH_KD;
params_.p_ki = config.PITCH_KI;
params_.p_ff = config.PITCH_FF;
params_.a_p_kp = config.AS_PITCH_KP;
params_.a_p_kd = config.AS_PITCH_KD;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
params_.b_kp = config.BETA_KP;
params_.b_kd = config.BETA_KD;
params_.b_ki = config.BETA_KI;
}
void controller_base::convert_to_pwm(controller_base::output_s &output)
{
output.delta_e = output.delta_e*params_.pwm_rad_e;
output.delta_a = output.delta_a*params_.pwm_rad_a;
output.delta_r = output.delta_r*params_.pwm_rad_r;
}
void controller_base::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
input.h = -vehicle_state_.position[2];
input.va = vehicle_state_.Va;
input.phi = vehicle_state_.phi;
input.theta = vehicle_state_.theta;
input.chi = vehicle_state_.chi;
input.psi = vehicle_state_.psi;
input.p = vehicle_state_.p;
input.q = vehicle_state_.q;
input.r = vehicle_state_.r;
input.Va_c = controller_commands_.Va_c;
input.h_c = controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
input.phi_ff = controller_commands_.phi_ff;
input.Ts = 0.01f;
struct output_s output;
//ROS_INFO("%g",input.va);
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
if (command_recieved_ == true)
{
control(params_, input, output);
convert_to_pwm(output);
rosflight_msgs::Command actuators;
/* publish actuator controls */
actuators.ignore = 0;
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
actuators_pub_.publish(actuators);
//ROS_INFO("Hello-----------------------------------------------------------------------");
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
if (internals_pub_.getNumSubscribers() > 0)
{
hector_msgs::Controller_Internals inners;
inners.phi_c = output.phi_c;
inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_controller");
hector::controller_base *cont = new hector::controller_example();
ros::spin();
return 0;
}

203
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/controller_base99.cpp Normal file
View File

@ -0,0 +1,203 @@
#include "controller_base99.h"
#include "controller_example99.h"
namespace hector
{
controller_base99::controller_base99():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
vehicle_state_sub_ = nh_.subscribe("state", 10, &controller_base99::vehicle_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &controller_base99::controller_commands_callback,
this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_E", params_.trim_e, 0.0);
nh_private_.param<double>("TRIM_A", params_.trim_a, 0.0);
nh_private_.param<double>("TRIM_R", params_.trim_r, 0.0);
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.6);
nh_private_.param<double>("PWM_RAD_E", params_.pwm_rad_e, 1.0);
nh_private_.param<double>("PWM_RAD_A", params_.pwm_rad_a, 1.0);
nh_private_.param<double>("PWM_RAD_R", params_.pwm_rad_r, 1.0);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 20.0); /**< altitude takeoff zone */
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 10.0); /**< altitude hold zone */
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("ROLL_KP", params_.r_kp, 1.2855);
nh_private_.param<double>("ROLL_KD", params_.r_kd, -0.325);
nh_private_.param<double>("ROLL_KI", params_.r_ki, 0.0);//0.10f);
nh_private_.param<double>("PITCH_KP", params_.p_kp, 1.0);
nh_private_.param<double>("PITCH_KD", params_.p_kd, -0.17);
nh_private_.param<double>("PITCH_KI", params_.p_ki, 0.0);
nh_private_.param<double>("PITCH_FF", params_.p_ff, 0.0);
nh_private_.param<double>("AS_PITCH_KP", params_.a_p_kp, -0.0713);
nh_private_.param<double>("AS_PITCH_KD", params_.a_p_kd, -0.0635);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 3.2);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.01);
nh_private_.param<double>("BETA_KP", params_.b_kp, -0.1164);
nh_private_.param<double>("BETA_KD", params_.b_kd, 0.0);
nh_private_.param<double>("BETA_KI", params_.b_ki, -0.0037111);
nh_private_.param<double>("MAX_E", params_.max_e, 0.610);
nh_private_.param<double>("MAX_A", params_.max_a, 0.523);
nh_private_.param<double>("MAX_R", params_.max_r, 0.523);
nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
func_ = boost::bind(&controller_base99::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
actuators_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
internals_pub_ = nh_.advertise<hector_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &controller_base99::actuator_controls_publish, this);
command_recieved_ = false;
}
void controller_base99::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void controller_base99::controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void controller_base99::reconfigure_callback(hector::ControllerConfig &config, uint32_t level)
{
params_.trim_e = config.TRIM_E;
params_.trim_a = config.TRIM_A;
params_.trim_r = config.TRIM_R;
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.r_kp = config.ROLL_KP;
params_.r_kd = config.ROLL_KD;
params_.r_ki = config.ROLL_KI;
params_.p_kp = config.PITCH_KP;
params_.p_kd = config.PITCH_KD;
params_.p_ki = config.PITCH_KI;
params_.p_ff = config.PITCH_FF;
params_.a_p_kp = config.AS_PITCH_KP;
params_.a_p_kd = config.AS_PITCH_KD;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
params_.b_kp = config.BETA_KP;
params_.b_kd = config.BETA_KD;
params_.b_ki = config.BETA_KI;
}
void controller_base99::convert_to_pwm(controller_base99::output_s &output)
{
output.delta_e = output.delta_e*params_.pwm_rad_e;
output.delta_a = output.delta_a*params_.pwm_rad_a;
output.delta_r = output.delta_r*params_.pwm_rad_r;
}
void controller_base99::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
input.h = -vehicle_state_.position[2];//>0
input.va = vehicle_state_.Va;
input.phi = vehicle_state_.phi;
input.theta = vehicle_state_.theta;
input.chi = vehicle_state_.chi;
input.psi = vehicle_state_.psi;
input.p = vehicle_state_.p;
input.q = vehicle_state_.q;
input.r = vehicle_state_.r;
input.Va_c = controller_commands_.Va_c;
input.h_c = controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
input.phi_ff = controller_commands_.phi_ff;
input.Ts = 0.01f;
struct output_s output;
//ROS_INFO("%g",input.va);
//std::cout<<"command_recieved_"<<command_recieved_<<std::endl;
if (command_recieved_ == true)
{
control(params_, input, output);
convert_to_pwm(output);
rosflight_msgs::Command actuators;
/* publish actuator controls */
actuators.ignore = 0;
actuators.mode = rosflight_msgs::Command::MODE_PASS_THROUGH;
actuators.x = output.delta_a;//(isfinite(output.delta_a)) ? output.delta_a : 0.0f;
actuators.y = output.delta_e;//(isfinite(output.delta_e)) ? output.delta_e : 0.0f;
actuators.z = output.delta_r;//(isfinite(output.delta_r)) ? output.delta_r : 0.0f;
actuators.F = output.delta_t;//(isfinite(output.delta_t)) ? output.delta_t : 0.0f;
actuators_pub_.publish(actuators);
//ROS_INFO("Hello-----------------------------------------------------------------------");
//ROS_INFO("command is here: %f %f %f",input.va,input.psi,input.chi);
//ROS_INFO("%g %g %g %g",actuators.x,actuators.y,actuators.z,actuators.F);
//std::cout<<"actuators.x: "<<actuators.x<<std::endl;
//std::cout<<"actuators.y: "<<actuators.y<<std::endl;
//std::cout<<"actuators.z: "<<actuators.z<<std::endl;
//std::cout<<"actuators.F: "<<actuators.F<<std::endl;
if (internals_pub_.getNumSubscribers() > 0)
{
hector_msgs::Controller_Internals inners;
inners.phi_c = output.phi_c;
inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_controller99");
hector::controller_base99 *cont = new hector::controller_example99();
ros::spin();
return 0;
}

265
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/controller_example.cpp Normal file
View File

@ -0,0 +1,265 @@
#include "controller_example.h"
namespace hector
{
controller_example::controller_example() : controller_base()
{
current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator = 0;
p_error_ = 0;
p_integrator_ = 0;
}
void controller_example::control(const params_s &params, const input_s &input, output_s &output)
{
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
switch (current_zone)
{
case alt_zones::TAKE_OFF:
output.phi_c = 0;
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
output.delta_t = params.max_t;
output.theta_c = 15.0*3.14/180.0;
if (input.h >= params.alt_toz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::CLIMB:
output.delta_t = params.max_t;
//modified by kobe
if (input.Va_c < input.va)
{
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
}
else
{
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
}
//
if (input.h >= input.h_c - params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
else if (input.h <= params.alt_toz)
{
ROS_WARN("takeoff");
current_zone = alt_zones::TAKE_OFF;
}
break;
case alt_zones::DESCEND:
output.delta_t = 0;
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
if (input.h <= input.h_c + params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
break;
case alt_zones::ALTITUDE_HOLD:
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
if (input.h >= input.h_c + params.alt_hz)
{
ROS_WARN("desend");
current_zone = alt_zones::DESCEND;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
else if (input.h <= input.h_c - params.alt_hz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
default:
break;
}
output.current_zone = current_zone;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float controller_example::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params, float Ts)
{
float error = chi_c - chi;
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
float up = params.c_kp*error;
float ui = params.c_ki*c_integrator_;
float ud = params.c_kd*r;
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
if (fabs(params.c_ki) >= 0.00001)
{
float phi_c_unsat = up + ui + ud + phi_ff;
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
}
c_error_ = error;
return phi_c;
}
float controller_example::roll_hold(float phi_c, float phi, float p, const params_s &params, float Ts)
{
float error = phi_c - phi;
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
float up = params.r_kp*error;
float ui = params.r_ki*r_integrator;
float ud = params.r_kd*p;
float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
if (fabs(params.r_ki) >= 0.00001)
{
float delta_a_unsat = up + ui + ud;
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
}
r_error_ = error;
return delta_a;
}
float controller_example::pitch_hold(float theta_c, float theta, float q, const params_s &params, float Ts)
{
float error = theta_c - theta;
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
float up = params.p_kp*error;
float ui = params.p_ki*p_integrator_;
float ud = params.p_kd*q;
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
if (fabs(params.p_ki) >= 0.00001)
{
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
}
p_error_ = error;
return delta_e;
}
float controller_example::airspeed_with_pitch_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - ap_error_);
float up = params.a_p_kp*error;
float ui = params.a_p_ki*ap_integrator_;
float ud = params.a_p_kd*ap_differentiator_;
float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
if (fabs(params.a_p_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
}
ap_error_ = error;
return theta_c;
}
float controller_example::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - at_error_);
float up = params.a_t_kp*error;
float ui = params.a_t_ki*at_integrator_;
float ud = params.a_t_kd*at_differentiator_;
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
if (fabs(params.a_t_ki) >= 0.00001)
{
float delta_t_unsat = params.trim_t + up + ui + ud;
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
}
at_error_ = error;
return delta_t;
}
float controller_example::altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - a_error_);
float up = params.a_kp*error;
float ui = params.a_ki*a_integrator_;
float ud = params.a_kd*a_differentiator_;
float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
if (fabs(params.a_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
}
at_error_ = error;
return theta_c;
}
//float controller_example::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float controller_example::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} //end namespace

293
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/controller_example99.cpp Normal file
View File

@ -0,0 +1,293 @@
#include "controller_example99.h"
namespace hector
{
controller_example99::controller_example99() : controller_base99()
{
current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator = 0;
p_error_ = 0;
p_integrator_ = 0;
}
void controller_example99::control(const params_s &params, const input_s &input, output_s &output)
{
output.delta_r = 0; //cooridinated_turn_hold(input.beta, params, input.Ts)
output.phi_c = course_hold(input.chi_c, input.chi, input.phi_ff, input.r, params, input.Ts);
output.delta_a = roll_hold(output.phi_c, input.phi, input.p, params, input.Ts);
switch (current_zone)
{
case alt_zones::TAKE_OFF:
output.phi_c = 0;
output.delta_a = roll_hold(0.0, input.phi, input.p, params, input.Ts);
output.delta_t = params.max_t;//modified by kobe
output.theta_c = 1.0*3.14/180.0;
//output.theta_c = 15.0*3.14/180.0;
//ROS_WARN("TAKE_OFF");
if (input.h >= params.alt_toz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::CLIMB:
output.delta_t = params.max_t;
//modified by kobe
if (input.Va_c < input.va)
{
output.theta_c = airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);//original
}
else
{
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
}
//
if (input.h >= input.h_c - params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
else if (input.h <= params.alt_toz)
{
//ROS_WARN("takeoff");
current_zone = alt_zones::TAKE_OFF;
}
break;
case alt_zones::DESCEND:
output.delta_t = decend_airspeed_oil(input.h_c, input.h, params, input.Ts);
//output.theta_c =airspeed_with_pitch_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c =decend_airspeed_with_pitch_hold(input.h_c, input.h, params, input.Ts);
//ROS_INFO("Look here %f %f",output.theta_c,pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts));
if (input.h <= input.h_c + params.alt_hz)
{
ROS_WARN("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
break;
case alt_zones::ALTITUDE_HOLD:
output.delta_t = airspeed_with_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.theta_c = altitiude_hold(input.h_c, input.h, params, input.Ts);
//ROS_INFO("Look here %f %f %f",output.delta_t,output.theta_c,input.Va_c);
if (input.h >= input.h_c + 200)//add by kobe
//if (input.h >= input.h_c + params.alt_hz)//original
{
ROS_WARN("desend");
current_zone = alt_zones::DESCEND;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
else if (input.h <= input.h_c - 200)
//else if (input.h <= input.h_c - params.alt_hz)
{
ROS_WARN("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
default:
break;
}
output.current_zone = current_zone;
output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float controller_example99::course_hold(float chi_c, float chi, float phi_ff, float r, const params_s &params, float Ts)
{
float error = chi_c - chi;
c_integrator_ = c_integrator_ + (Ts/2.0)*(error + c_error_);
float up = params.c_kp*error;
float ui = params.c_ki*c_integrator_;
float ud = params.c_kd*r;
float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0); //add by kobe
//float phi_c = sat(up + ui + ud + phi_ff, 40.0*3.14/180.0, -40.0*3.14/180.0);
if (fabs(params.c_ki) >= 0.00001)
{
float phi_c_unsat = up + ui + ud + phi_ff;
c_integrator_ = c_integrator_ + (Ts/params.c_ki)*(phi_c - phi_c_unsat);
}
c_error_ = error;
return phi_c;
}
float controller_example99::roll_hold(float phi_c, float phi, float p, const params_s &params, float Ts)
{
float error = phi_c - phi;
r_integrator = r_integrator + (Ts/2.0)*(error + r_error_);
float up = params.r_kp*error;
float ui = params.r_ki*r_integrator;
float ud = params.r_kd*p;
float delta_a = sat(up + ui + ud,params.max_a, -params.max_a);//add by kobe,0.1,-0.1
//float delta_a = sat(up + ui + ud, params.max_a, -params.max_a);
if (fabs(params.r_ki) >= 0.00001)
{
float delta_a_unsat = up + ui + ud;
r_integrator = r_integrator + (Ts/params.r_ki)*(delta_a - delta_a_unsat);
}
r_error_ = error;
return delta_a;
}
float controller_example99::pitch_hold(float theta_c, float theta, float q, const params_s &params, float Ts)
{
float error = theta_c - theta;
p_integrator_ = p_integrator_ + (Ts/2.0)*(error + p_error_);
float up = params.p_kp*error;
float ui = params.p_ki*p_integrator_;
float ud = params.p_kd*q;
float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, 0.1, -0.1);// add by kobe
//float delta_e = sat(params.trim_e/params.pwm_rad_e + up + ui + ud, params.max_e, -params.max_e);
if (fabs(params.p_ki) >= 0.00001)
{
float delta_e_unsat = params.trim_e/params.pwm_rad_e + up + ui + ud;
p_integrator_ = p_integrator_ + (Ts/params.p_ki)*(delta_e - delta_e_unsat);
}
p_error_ = error;
return delta_e;
}
float controller_example99::airspeed_with_pitch_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
ap_integrator_ = ap_integrator_ + (Ts/2.0)*(error + ap_error_);
ap_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*ap_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - ap_error_);
float up = params.a_p_kp*error;
float ui = params.a_p_ki*ap_integrator_;
float ud = params.a_p_kd*ap_differentiator_;
float theta_c = sat(up + ui + ud, 2.0*3.14/180.0, -2.0*3.14/180.0);//add by kobe
//float theta_c = sat(up + ui + ud, 20.0*3.14/180.0, -25.0*3.14/180.0);
if (fabs(params.a_p_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
ap_integrator_ = ap_integrator_ + (Ts/params.a_p_ki)*(theta_c - theta_c_unsat);
}
ap_error_ = error;
return theta_c;
}
float controller_example99::decend_airspeed_with_pitch_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = 0.01*(h_c - h);
float theta_c = sat(error, 0, -90.0*3.14/180.0);
return theta_c;
}
float controller_example99::decend_airspeed_oil(float h_c, float h, const params_s &params, float Ts)
{
float error = pow(2,(h_c - h)/50);
float theta_t = sat(error, 1, 0);
return theta_t;
}
float controller_example99::airspeed_with_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
at_integrator_ = at_integrator_ + (Ts/2.0)*(error + at_error_);
at_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*at_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - at_error_);
float up = params.a_t_kp*error;
float ui = params.a_t_ki*at_integrator_;
float ud = params.a_t_kd*at_differentiator_;
float delta_t = sat(params.trim_t + up + ui + ud, params.max_t, 0);
if (fabs(params.a_t_ki) >= 0.00001)
{
float delta_t_unsat = params.trim_t + up + ui + ud;
at_integrator_ = at_integrator_ + (Ts/params.a_t_ki)*(delta_t - delta_t_unsat);
}
at_error_ = error;
return delta_t;
}
float controller_example99::altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
a_integrator_ = a_integrator_ + (Ts/2.0)*(error + a_error_);
a_differentiator_ = (2.0*params.tau - Ts)/(2.0*params.tau + Ts)*a_differentiator_ + (2.0 /
(2.0*params.tau + Ts))*(error - a_error_);
float up = params.a_kp*error;
float ui = params.a_ki*a_integrator_;
float ud = params.a_kd*a_differentiator_;
float theta_c = sat(up + ui + ud, 5.0*3.14/180.0, -5.0*3.14/180.0);//add by kobe
//float theta_c = sat(up + ui + ud, 35.0*3.14/180.0, -35.0*3.14/180.0);
if (fabs(params.a_ki) >= 0.00001)
{
float theta_c_unsat = up + ui + ud;
a_integrator_ = a_integrator_ + (Ts/params.a_ki)*(theta_c - theta_c_unsat);
}
at_error_ = error;
return theta_c;
}
//float controller_example99::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float controller_example99::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} //end namespace

228
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/estimator_base.cpp Normal file
View File

@ -0,0 +1,228 @@
#include "estimator_base.h"
#include "estimator_example.h"
namespace hector
{
estimator_base::estimator_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
nh_private_.param<std::string>("gps_topic", gps_topic_, "gps");
nh_private_.param<std::string>("imu_topic", imu_topic_, "imu/data");
nh_private_.param<std::string>("baro_topic", baro_topic_, "baro");
nh_private_.param<std::string>("airspeed_topic", airspeed_topic_, "airspeed");
nh_private_.param<std::string>("status_topic", status_topic_, "status");
nh_private_.param<double>("update_rate", update_rate_, 100.0);
params_.Ts = 1.0f/update_rate_;
params_.gravity = 9.8;
nh_private_.param<double>("rho", params_.rho, 1.225);
nh_private_.param<double>("sigma_accel", params_.sigma_accel, 0.0245);
nh_private_.param<double>("sigma_n_gps", params_.sigma_n_gps, 0.21);
nh_private_.param<double>("sigma_e_gps", params_.sigma_e_gps, 0.21);
nh_private_.param<double>("sigma_Vg_gps", params_.sigma_Vg_gps, 0.0500);
nh_private_.param<double>("sigma_couse_gps", params_.sigma_course_gps, 0.0045);
gps_sub_ = nh_.subscribe(gps_topic_, 10, &estimator_base::gpsCallback, this);
imu_sub_ = nh_.subscribe(imu_topic_, 10, &estimator_base::imuCallback, this);
baro_sub_ = nh_.subscribe(baro_topic_, 10, &estimator_base::baroAltCallback, this);
airspeed_sub_ = nh_.subscribe(airspeed_topic_, 10, &estimator_base::airspeedCallback, this);
status_sub_ = nh_.subscribe(status_topic_, 1, &estimator_base::statusCallback, this);
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base::update, this);
vehicle_state_pub_ = nh_.advertise<hector_msgs::State>("state", 10);
init_static_ = 0;
baro_count_ = 0;
armed_first_time_ = false;
}
void estimator_base::update(const ros::TimerEvent &)
{
struct output_s output;
if (armed_first_time_)
{
estimate(params_, input_, output);
}
else
{
output.pn = output.pe = output.h = 0;
output.phi = output.theta = output.psi = 0;
output.alpha = output.beta = output.chi = 0;
output.p = output.q = output.r = 0;
output.Va = 0;
}
input_.gps_new = false;
hector_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
msg.position[0] = output.pn;
msg.position[1] = output.pe;
msg.position[2] = -output.h;
if (gps_init_)
{
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
}
msg.Va = output.Va;
msg.alpha = output.alpha;
msg.beta = output.beta;
msg.phi = output.phi;
msg.theta = output.theta;
msg.psi = output.psi;
msg.chi = output.chi;
msg.p = output.p;
msg.q = output.q;
msg.r = output.r;
msg.Vg = output.Vg;
msg.wn = output.wn;
msg.we = output.we;
msg.quat_valid = false;
msg.psi_deg = fmod(output.psi, 2*M_PI)*180/M_PI; //-360 to 360
msg.psi_deg += (msg.psi_deg < -180 ? 360 : 0);
msg.psi_deg -= (msg.psi_deg > 180 ? 360 : 0);
msg.chi_deg = fmod(output.chi, 2*M_PI)*180/M_PI; //-360 to 360
msg.chi_deg += (msg.chi_deg < -180 ? 360 : 0);
msg.chi_deg -= (msg.chi_deg > 180 ? 360 : 0);
vehicle_state_pub_.publish(msg);
}
void estimator_base::gpsCallback(const rosflight_msgs::GPS &msg)
{
if (msg.fix != true || msg.NumSat < 4 || !std::isfinite(msg.latitude))
{
input_.gps_new = false;
return;
}
if (!gps_init_)
{
gps_init_ = true;
init_alt_ = msg.altitude;
init_lat_ = msg.latitude;
init_lon_ = msg.longitude;
}
else
{
input_.gps_n = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
input_.gps_e = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
input_.gps_h = msg.altitude - init_alt_;
input_.gps_Vg = msg.speed;
if (msg.speed > 0.3)
input_.gps_course = msg.ground_course;
input_.gps_new = true;
//ROS_INFO("%g %g %g",input_.gps_n,input_.gps_e,input_.gps_h);
}
}
void estimator_base::imuCallback(const sensor_msgs::Imu &msg)
{
input_.accel_x = msg.linear_acceleration.x;
input_.accel_y = msg.linear_acceleration.y;
input_.accel_z = msg.linear_acceleration.z;
input_.gyro_x = msg.angular_velocity.x;
input_.gyro_y = msg.angular_velocity.y;
input_.gyro_z = msg.angular_velocity.z;
}
void estimator_base::baroAltCallback(const rosflight_msgs::Barometer &msg)
{
if (armed_first_time_ && !baro_init_)
{
if (baro_count_ < 100)
{
init_static_ += msg.pressure;
init_static_vector_.push_back(msg.pressure);
input_.static_pres = 0;
baro_count_ += 1;
}
else
{
init_static_ = std::accumulate(init_static_vector_.begin(), init_static_vector_.end(),
0.0)/init_static_vector_.size();
baro_init_ = true;
//Check that it got a good calibration.
std::sort(init_static_vector_.begin(), init_static_vector_.end());
float q1 = (init_static_vector_[24] + init_static_vector_[25])/2.0;
float q3 = (init_static_vector_[74] + init_static_vector_[75])/2.0;
float IQR = q3 - q1;
float upper_bound = q3 + 2.0*IQR;
float lower_bound = q1 - 2.0*IQR;
for (int i = 0; i < 100; i++)
{
if (init_static_vector_[i] > upper_bound)
{
baro_init_ = false;
baro_count_ = 0;
init_static_vector_.clear();
ROS_WARN("Bad baro calibration. Recalibrating");
break;
}
else if (init_static_vector_[i] < lower_bound)
{
baro_init_ = false;
baro_count_ = 0;
init_static_vector_.clear();
ROS_WARN("Bad baro calibration. Recalibrating");
break;
}
}
}
}
else
{
float static_pres_old = input_.static_pres;
input_.static_pres = -msg.pressure + init_static_;
float gate_gain = 1.35*params_.rho*params_.gravity;
if (input_.static_pres < static_pres_old - gate_gain)
{
input_.static_pres = static_pres_old - gate_gain;
}
else if (input_.static_pres > static_pres_old + gate_gain)
{
input_.static_pres = static_pres_old + gate_gain;
}
}
}
void estimator_base::airspeedCallback(const rosflight_msgs::Airspeed &msg)
{
float diff_pres_old = input_.diff_pres;
input_.diff_pres = msg.differential_pressure;
float gate_gain = pow(3, 2)*params_.rho/2.0;
if (input_.diff_pres < diff_pres_old - gate_gain)
{
input_.diff_pres = diff_pres_old - gate_gain;
}
else if (input_.diff_pres > diff_pres_old + gate_gain)
{
input_.diff_pres = diff_pres_old + gate_gain;
}
}
void estimator_base::statusCallback(const rosflight_msgs::Status &msg)
{
if (!armed_first_time_ && msg.armed)
armed_first_time_ = true;
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_estimator");
hector::estimator_base *est = new hector::estimator_example();
ros::spin();
return 0;
}

76
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/estimator_base_99pub.cpp Normal file
View File

@ -0,0 +1,76 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_99pub.h"
namespace hector
{
estimator_base_99pub::estimator_base_99pub():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
true_state_sub_ = nh_.subscribe("truth", 1, &estimator_base_99pub::truestateCallback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0/update_rate_), &estimator_base_99pub::update, this);
vehicle_state99_pub_ = nh_.advertise<hector_msgs::State29>("state29", 10);
state_init_ = false;
}
void estimator_base_99pub::update(const ros::TimerEvent &)
{
hector_msgs::State29 msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position_gps[0]=(true_state_.position[0]*180)/(EARTH_RADIUS*M_PI)+true_state_.initial_lat;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position_gps[1]=(true_state_.position[1]*180)/(EARTH_RADIUS*cos(true_state_.initial_lat*M_PI/180.0)*M_PI)+true_state_.initial_lon;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
// input_.gps_h = msg.altitude - init_alt_;
msg.position_gps[2] =-true_state_.position[2]-true_state_.initial_alt; //>0
msg.attitude_angle[0] = true_state_.phi;
msg.attitude_angle[1] = true_state_.theta;
msg.attitude_angle[2] = true_state_.psi;
msg.velocity[0] = true_state_.Vn;
msg.velocity[1] = true_state_.Ve;
msg.velocity[2] = true_state_.Vd;
msg.angular_velocity[0] = true_state_.p;
msg.angular_velocity[1] = true_state_.q;
msg.angular_velocity[2] = true_state_.r;
msg.acceleration[0] = -1;
msg.acceleration[1] = -1;
msg.acceleration[2] = -1;
msg.electric_quantity = -1;
msg.state_word = -1;
// ROS_INFO("GPS-position: %g %g %g",msg.position_gps[0],msg.position_gps[1],msg.position_gps[2]);
// ROS_INFO("xyz-position: %g %g %g",true_state_.position[0],true_state_.position[1],true_state_.position[2]);
}
else
{
// ROS_WARN("No plane truth data.");
}
vehicle_state99_pub_.publish(msg);
}
void estimator_base_99pub::truestateCallback(const hector_msgs::StateConstPtr &msg)
{
true_state_ = *msg;
state_init_ = true;
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_estimator_29pub");
hector::estimator_base_99pub *cont = new hector::estimator_base_99pub();
ros::spin();
return 0;
}

75
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/estimator_base_statesub.cpp Normal file
View File

@ -0,0 +1,75 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_statesub.h"
namespace hector
{
estimator_base_statesub::estimator_base_statesub() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
state29_sub_ = nh_.subscribe("state29", 10, &estimator_base_statesub::state99Callback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_statesub::update, this);
vehicle_state_pub_ = nh_.advertise<hector_msgs::State>("state", 10);
state_init_ = false;
init_lat_=0; /**< Initial latitude in degrees */
init_lon_=0; /**< Initial longitude in degrees */
init_alt_=0; //>0
}
void estimator_base_statesub::update(const ros::TimerEvent &)
{
hector_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position[0] = EARTH_RADIUS*(state29_.position_gps[0] - init_lat_)*M_PI/180.0;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.position_gps[1] - init_lon_)*M_PI/180.0;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
msg.position[2] = -state29_.position_gps[2]- init_alt_; //<0
//input_.gps_h = msg.altitude - init_alt_;
msg.Wd=state29_.position_gps[0];
msg.Jd=state29_.position_gps[1];
msg.Va=sqrt(pow(state29_.velocity[0], 2.0) + pow(state29_.velocity[1], 2.0) + pow(state29_.velocity[2], 2.0));
msg.Vg=msg.Va; //true for rosflight
msg.phi=state29_.attitude_angle[0];
msg.theta=state29_.attitude_angle[1];
msg.psi=state29_.attitude_angle[2];
//msg.chi=msg.psi;//watch here
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
msg.p=state29_.angular_velocity[0];
msg.q=state29_.angular_velocity[1];
msg.r=state29_.angular_velocity[2];
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
}
else
{
// ROS_WARN("No State29 data.");
}
vehicle_state_pub_.publish(msg);
}
void estimator_base_statesub::state99Callback(const hector_msgs::State29ConstPtr &msg)
{
state29_ = *msg;
state_init_ = true;
}
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_estimator_statesub");
hector::estimator_base_statesub *cont = new hector::estimator_base_statesub();
ros::spin();
return 0;
}

76
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/estimator_base_updata_statesub.cpp Normal file
View File

@ -0,0 +1,76 @@
//* @author AIRC-DA group,questions contack kobe.
#include "estimator_base_updata_statesub.h"
namespace hector
{
estimator_base_updata_statesub::estimator_base_updata_statesub() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
state29_sub_ = nh_.subscribe("/serial_commu_up", 10, &estimator_base_updata_statesub::state99Callback, this);
nh_private_.param<double>("update_rate", update_rate_, 100.0);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &estimator_base_updata_statesub::update, this);
vehicle_state_pub_ = nh_.advertise<hector_msgs::State>("state", 10);
state_init_ = false;
init_lat_=0; /**< Initial latitude in degrees */
init_lon_=0; /**< Initial longitude in degrees */
init_alt_=0; //>0
}
void estimator_base_updata_statesub::update(const ros::TimerEvent &)
{
hector_msgs::State msg;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = 1; // Denotes global frame
if (state_init_)
{
msg.position[0] = EARTH_RADIUS*(state29_.latitude - init_lat_)*M_PI/180.0;
// pn = EARTH_RADIUS*(msg.latitude - init_lat_)*M_PI/180.0;
msg.position[1] = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(state29_.longitude - init_lon_)*M_PI/180.0;
// pe = EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*(msg.longitude - init_lon_)*M_PI/180.0;
msg.position[2] = -state29_.altitude- init_alt_; //<0
//input_.gps_h = msg.altitude - init_alt_;
msg.Wd=state29_.latitude;
msg.Jd=state29_.longitude;
msg.Va=sqrt(pow(state29_.north_direction_speed, 2.0) + pow(state29_.east_direction_speed, 2.0) + pow(state29_.ground_direction_speed, 2.0));
msg.Vg=msg.Va; //true for rosflight
msg.phi=state29_.roll_angle;
msg.theta=state29_.pitch_angle;
msg.psi=state29_.yaw_angle;
//msg.chi=msg.psi;//watch here
msg.chi = atan2(msg.Va*sin(msg.psi), msg.Va*cos(msg.psi));
msg.p=state29_.angular_rate_x;
msg.q=state29_.angular_rate_y;
msg.r=state29_.angular_rate_z;
msg.initial_lat = init_lat_;
msg.initial_lon = init_lon_;
msg.initial_alt = init_alt_;
// ROS_WARN("Recieved up-data data.");
//ROS_INFO("Hello--- %f %f %f %f",state29_.latitude,state29_.longitude,msg.Vg,state29_.altitude);
}
else
{
// ROS_WARN("No up-data data.");
}
vehicle_state_pub_.publish(msg);
}
void estimator_base_updata_statesub::state99Callback(const hector_msgs::Up_Data_NewConstPtr &msg)
{
state29_ = *msg;
state_init_ = true;
}
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_estimator_updata_statesub");
hector::estimator_base_updata_statesub *cont = new hector::estimator_base_updata_statesub();
ros::spin();
return 0;
}

416
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/estimator_example.cpp Normal file
View File

@ -0,0 +1,416 @@
#include "estimator_base.h"
#include "estimator_example.h"
namespace hector
{
float radians(float degrees)
{
return M_PI*degrees/180.0;
}
estimator_example::estimator_example() :
estimator_base(),
xhat_a_(Eigen::Vector2f::Zero()),
P_a_(Eigen::Matrix2f::Identity()),
Q_a_(Eigen::Matrix2f::Identity()),
xhat_p_(Eigen::VectorXf::Zero(7)),
P_p_(Eigen::MatrixXf::Identity(7, 7)),
Q_p_(Eigen::MatrixXf::Identity(7, 7)),
R_p_(Eigen::MatrixXf::Zero(7, 7)),
f_p_(7),
A_p_(7, 7),
C_p_(7),
L_p_(7)
{
P_a_ *= powf(radians(5.0f), 2);
Q_a_(0, 0) = 0.00000001;
Q_a_(1, 1) = 0.00000001;
P_p_ = Eigen::MatrixXf::Identity(7, 7);
P_p_(0, 0) = .03;
P_p_(1, 1) = .03;
P_p_(2, 2) = .01;
P_p_(3, 3) = radians(5.0f);
P_p_(4, 4) = .04;
P_p_(5, 5) = .04;
P_p_(6, 6) = radians(5.0f);
Q_p_ *= 0.0001f;
Q_p_(3, 3) = 0.000001f;
phat_ = 0;
qhat_ = 0;
rhat_ = 0;
phihat_ = 0;
thetahat_ = 0;
psihat_ = 0;
Vwhat_ = 0;
lpf_static_ = 0;
lpf_diff_ = 0;
N_ = 10;
alpha_ = 0.0f;
}
void estimator_example::estimate(const params_s &params, const input_s &input, output_s &output)
{
if (alpha_ == 0.0f) //initailze stuff that comes from params
{
R_accel_ = powf(params.sigma_accel, 2);
R_p_(0, 0) = powf(params.sigma_n_gps, 2);
R_p_(1, 1) = powf(params.sigma_e_gps, 2);
R_p_(2, 2) = powf(params.sigma_Vg_gps, 2);
R_p_(3, 3) = powf(params.sigma_course_gps, 2);
R_p_(4, 4) = 0.001;
R_p_(5, 5) = 0.001;
float lpf_a = 50.0;
float lpf_a1 = 8.0;
alpha_ = exp(-lpf_a*params.Ts);
alpha1_ = exp(-lpf_a1*params.Ts);
}
// low pass filter gyros to estimate angular rates
lpf_gyro_x_ = alpha_*lpf_gyro_x_ + (1 - alpha_)*input.gyro_x;
lpf_gyro_y_ = alpha_*lpf_gyro_y_ + (1 - alpha_)*input.gyro_y;
lpf_gyro_z_ = alpha_*lpf_gyro_z_ + (1 - alpha_)*input.gyro_z;
float phat = lpf_gyro_x_;
float qhat = lpf_gyro_y_;
float rhat = lpf_gyro_z_;
// low pass filter static pressure sensor and invert to esimate altitude
lpf_static_ = alpha1_*lpf_static_ + (1 - alpha1_)*input.static_pres;
float hhat = lpf_static_/params.rho/params.gravity;
// low pass filter diff pressure sensor and invert to extimate Va
lpf_diff_ = alpha1_*lpf_diff_ + (1 - alpha1_)*input.diff_pres;
// when the plane isn't moving or moving slowly, the noise in the sensor
// will cause the differential pressure to go negative. This will catch
// those cases.
if (lpf_diff_ <= 0)
lpf_diff_ = 0.000001;
float Vahat = sqrt(2/params.rho*lpf_diff_);
// low pass filter accelerometers
lpf_accel_x_ = alpha_*lpf_accel_x_ + (1 - alpha_)*input.accel_x;
lpf_accel_y_ = alpha_*lpf_accel_y_ + (1 - alpha_)*input.accel_y;
lpf_accel_z_ = alpha_*lpf_accel_z_ + (1 - alpha_)*input.accel_z;
// implement continuous-discrete EKF to estimate roll and pitch angles
// prediction step
float cp; // cos(phi)
float sp; // sin(phi)
float tt; // tan(thata)
float ct; // cos(thata)
float st; // sin(theta)
for (int i = 0; i < N_; i++)
{
cp = cosf(xhat_a_(0)); // cos(phi)
sp = sinf(xhat_a_(0)); // sin(phi)
tt = tanf(xhat_a_(1)); // tan(thata)
ct = cosf(xhat_a_(1)); // cos(thata)
f_a_(0) = phat + (qhat*sp + rhat*cp)*tt;
f_a_(1) = qhat*cp - rhat*sp;
A_a_ = Eigen::Matrix2f::Zero();
A_a_(0, 0) = (qhat*cp - rhat*sp)*tt;
A_a_(0, 1) = (qhat*sp + rhat*cp)/ct/ct;
A_a_(1, 0) = -qhat*sp - rhat*cp;
xhat_a_ += f_a_*(params.Ts/N_);
P_a_ += (A_a_*P_a_ + P_a_*A_a_.transpose() + Q_a_)*(params.Ts/N_);
}
// measurement updates
cp = cosf(xhat_a_(0));
sp = sinf(xhat_a_(0));
ct = cosf(xhat_a_(1));
st = sinf(xhat_a_(1));
Eigen::Matrix2f I;
I = Eigen::Matrix2f::Identity();
// x-axis accelerometer
h_a_ = qhat*Vahat*st + params.gravity*st;
C_a_ = Eigen::Vector2f::Zero();
C_a_(1) = qhat*Vahat*ct + params.gravity*ct;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*((hhat < 15 ? lpf_accel_x_/3 : lpf_accel_x_) - h_a_);
// y-axis accelerometer
h_a_ = rhat*Vahat*ct - phat*Vahat*st - params.gravity*ct*sp;
C_a_ = Eigen::Vector2f::Zero();
C_a_(0) = -params.gravity*cp*ct;
C_a_(1) = -rhat*Vahat*st - phat*Vahat*ct + params.gravity*st*sp;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*(lpf_accel_y_ - h_a_);
// z-axis accelerometer
h_a_ = -qhat*Vahat*ct - params.gravity*ct*cp;
C_a_ = Eigen::Vector2f::Zero();
C_a_(0) = params.gravity*sp*ct;
C_a_(1) = (qhat*Vahat + params.gravity*cp)*st;
L_a_ = (P_a_*C_a_)/(R_accel_ + C_a_.transpose()*P_a_*C_a_);
P_a_ = (I - L_a_*C_a_.transpose())*P_a_;
xhat_a_ += L_a_*(lpf_accel_z_ - h_a_);
check_xhat_a();
float phihat = xhat_a_(0);
float thetahat = xhat_a_(1);
// implement continous-discrete EKF to estimate pn, pe, chi, Vg
// prediction step
float psidot, tmp, Vgdot;
if (fabsf(xhat_p_(2)) < 0.01f)
{
xhat_p_(2) = 0.01; // prevent devide by zero
}
for (int i = 0; i < N_; i++)
{
psidot = (qhat*sinf(phihat) + rhat*cosf(phihat))/cosf(thetahat);
tmp = -psidot*Vahat*(xhat_p_(4)*cosf(xhat_p_(6)) + xhat_p_(5)*sinf(xhat_p_(6)))/xhat_p_(2);
Vgdot = ((Vahat*cosf(xhat_p_(6)) + xhat_p_(4))*(-psidot*Vahat*sinf(xhat_p_(6))) + (Vahat*sinf(xhat_p_(
6)) + xhat_p_(5))*(psidot*Vahat*cosf(xhat_p_(6))))/xhat_p_(2);
f_p_ = Eigen::VectorXf::Zero(7);
f_p_(0) = xhat_p_(2)*cosf(xhat_p_(3));
f_p_(1) = xhat_p_(2)*sinf(xhat_p_(3));
f_p_(2) = Vgdot;
f_p_(3) = params.gravity/xhat_p_(2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
f_p_(6) = psidot;
A_p_ = Eigen::MatrixXf::Zero(7, 7);
A_p_(0, 2) = cos(xhat_p_(3));
A_p_(0, 3) = -xhat_p_(2)*sinf(xhat_p_(3));
A_p_(1, 2) = sin(xhat_p_(3));
A_p_(1, 3) = xhat_p_(2)*cosf(xhat_p_(3));
A_p_(2, 2) = -Vgdot/xhat_p_(2);
A_p_(2, 4) = -psidot*Vahat*sinf(xhat_p_(6))/xhat_p_(2);
A_p_(2, 5) = psidot*Vahat*cosf(xhat_p_(6))/xhat_p_(2);
A_p_(2, 6) = tmp;
A_p_(3, 2) = -params.gravity/powf(xhat_p_(2), 2)*tanf(phihat)*cosf(xhat_p_(3) - xhat_p_(6));
A_p_(3, 3) = -params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
A_p_(3, 6) = params.gravity/xhat_p_(2)*tanf(phihat)*sinf(xhat_p_(3) - xhat_p_(6));
xhat_p_ += f_p_*(params.Ts/N_);
P_p_ += (A_p_*P_p_ + P_p_*A_p_.transpose() + Q_p_)*(params.Ts/N_);
}
// while(xhat_p(3) > radians(180.0f)) xhat_p(3) = xhat_p(3) - radians(360.0f);
// while(xhat_p(3) < radians(-180.0f)) xhat_p(3) = xhat_p(3) + radians(360.0f);
// if(xhat_p(3) > radians(180.0f) || xhat_p(3) < radians(-180.0f))
// {
// ROS_WARN("Course estimate not wrapped from -pi to pi");
// xhat_p(3) = 0;
// }
// measurement updates
if (input.gps_new)
{
Eigen::MatrixXf I_p(7, 7);
I_p = Eigen::MatrixXf::Identity(7, 7);
// gps North position
h_p_ = xhat_p_(0);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(0) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(0, 0) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_n - h_p_);
// gps East position
h_p_ = xhat_p_(1);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(1) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(1, 1) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_e - h_p_);
// gps ground speed
h_p_ = xhat_p_(2);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(2) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(2, 2) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(input.gps_Vg - h_p_);
// gps course
//wrap course measurement
float gps_course = fmodf(input.gps_course, radians(360.0f));
while (gps_course - xhat_p_(3) > radians(180.0f)) gps_course = gps_course - radians(360.0f);
while (gps_course - xhat_p_(3) < radians(-180.0f)) gps_course = gps_course + radians(360.0f);
h_p_ = xhat_p_(3);
C_p_ = Eigen::VectorXf::Zero(7);
C_p_(3) = 1;
L_p_ = (P_p_*C_p_)/(R_p_(3, 3) + (C_p_.transpose()*P_p_*C_p_));
P_p_ = (I_p - L_p_*C_p_.transpose())*P_p_;
xhat_p_ = xhat_p_ + L_p_*(gps_course - h_p_);
// // pseudo measurement #1 y_1 = Va*cos(psi)+wn-Vg*cos(chi)
// h_p = Vahat*cosf(xhat_p(6)) + xhat_p(4) - xhat_p(2)*cosf(xhat_p(3)); // pseudo measurement
// C_p = Eigen::VectorXf::Zero(7);
// C_p(2) = -cos(xhat_p(3));
// C_p(3) = xhat_p(2)*sinf(xhat_p(3));
// C_p(4) = 1;
// C_p(6) = -Vahat*sinf(xhat_p(6));
// L_p = (P_p*C_p)/(R_p(4,4) + (C_p.transpose()*P_p*C_p));
// P_p = (I_p - L_p*C_p.transpose())*P_p;
// xhat_p = xhat_p + L_p*(0 - h_p);
// // pseudo measurement #2 y_2 = Va*sin(psi) + we - Vg*sin(chi)
// h_p = Vahat*sinf(xhat_p(6))+xhat_p(5)-xhat_p(2)*sinf(xhat_p(3)); // pseudo measurement
// C_p = Eigen::VectorXf::Zero(7);
// C_p(2) = -sin(xhat_p(3));
// C_p(3) = -xhat_p(2)*cosf(xhat_p(3));
// C_p(5) = 1;
// C_p(6) = Vahat*cosf(xhat_p(6));
// L_p = (P_p*C_p)/(R_p(5,5) + (C_p.transpose()*P_p*C_p));
// P_p = (I_p - L_p*C_p.transpose())*P_p;
// xhat_p = xhat_p + L_p*(0 - h_p);
if (xhat_p_(0) > 10000 || xhat_p_(0) < -10000)
{
ROS_WARN("gps n limit reached");
xhat_p_(0) = input.gps_n;
}
if (xhat_p_(1) > 10000 || xhat_p_(1) < -10000)
{
ROS_WARN("gps e limit reached");
xhat_p_(1) = input.gps_e;
}
}
bool problem = false;
int prob_index;
for (int i = 0; i < 7; i++)
{
if (!std::isfinite(xhat_p_(i)))
{
if (!problem)
{
problem = true;
prob_index = i;
}
switch (i)
{
case 0:
xhat_p_(i) = input.gps_n;
break;
case 1:
xhat_p_(i) = input.gps_e;
break;
case 2:
xhat_p_(i) = input.gps_Vg;
break;
case 3:
xhat_p_(i) = input.gps_course;
break;
case 6:
xhat_p_(i) = input.gps_course;
break;
default:
xhat_p_(i) = 0;
}
P_p_ = Eigen::MatrixXf::Identity(7, 7);
P_p_(0, 0) = .03;
P_p_(1, 1) = .03;
P_p_(2, 2) = .01;
P_p_(3, 3) = radians(5.0f);
P_p_(4, 4) = .04;
P_p_(5, 5) = .04;
P_p_(6, 6) = radians(5.0f);
}
}
if (problem)
{
ROS_WARN("position estimator reinitialized due to non-finite state %d", prob_index);
}
if (xhat_p_(6) - xhat_p_(3) > radians(360.0f) || xhat_p_(6) - xhat_p_(3) < radians(-360.0f))
{
//xhat_p(3) = fmodf(xhat_p(3),2*M_PI);
xhat_p_(6) = fmodf(xhat_p_(6), 2*M_PI);
}
float pnhat = xhat_p_(0);
float pehat = xhat_p_(1);
float Vghat = xhat_p_(2);
float chihat = xhat_p_(3);
float wnhat = xhat_p_(4);
float wehat = xhat_p_(5);
float psihat = xhat_p_(6);
output.pn = pnhat;
output.pe = pehat;
output.h = hhat;
output.Va = Vahat;
output.alpha = 0;
output.beta = 0;
output.phi = phihat;
output.theta = thetahat;
output.chi = chihat;
output.p = phat;
output.q = qhat;
output.r = rhat;
output.Vg = Vghat;
output.wn = wnhat;
output.we = wehat;
output.psi = psihat;
}
void estimator_example::check_xhat_a()
{
if (xhat_a_(0) > radians(85.0) || xhat_a_(0) < radians(-85.0) || !std::isfinite(xhat_a_(0)))
{
if (!std::isfinite(xhat_a_(0)))
{
xhat_a_(0) = 0;
P_a_ = Eigen::Matrix2f::Identity();
P_a_ *= powf(radians(20.0f), 2);
ROS_WARN("attiude estimator reinitialized due to non-finite roll");
}
else if (xhat_a_(0) > radians(85.0))
{
xhat_a_(0) = radians(82.0);
//ROS_WARN("max roll angle");
}
else if (xhat_a_(0) < radians(-85.0))
{
xhat_a_(0) = radians(-82.0);
//ROS_WARN("min roll angle");
}
}
if (xhat_a_(1) > radians(80.0) || xhat_a_(1) < radians(-80.0) || !std::isfinite(xhat_a_(1)))
{
if (!std::isfinite(xhat_a_(1)))
{
xhat_a_(1) = 0;
P_a_ = Eigen::Matrix2f::Identity();
P_a_ *= powf(radians(20.0f), 2);
ROS_WARN("attiude estimator reinitialized due to non-finite pitch");
}
else if (xhat_a_(1) > radians(80.0))
{
xhat_a_(1) = radians(77.0);
ROS_WARN("max pitch angle");
}
else if (xhat_a_(1) < radians(-80.0))
{
xhat_a_(1) = radians(-77.0);
ROS_WARN("min pitch angle");
}
}
}
} //end namespace

162
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_follower_base.cpp Normal file
View File

@ -0,0 +1,162 @@
#include "path_follower_base.h"
#include "path_follower_example.h"
namespace hector
{
path_follower_base::path_follower_base() : nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle("~"))
{
vehicle_state_sub_ = nh_.subscribe<hector_msgs::State>("state", 1, &path_follower_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("ground_truth/state", 1, &path_follower_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe<nav_msgs::Odometry>("/test/odom", 1, &path_follower_base::bebop1_state_callback, this);
current_path_sub_ = nh_.subscribe<hector_msgs::Current_Path>("current_path", 1,
&path_follower_base::current_path_callback, this);
nh_private_.param<double>("CHI_INFTY", params_.chi_infty, 1.0472);
nh_private_.param<double>("K_PATH", params_.k_path, 0.025);
nh_private_.param<double>("K_ORBIT", params_.k_orbit, 4.0);
func_ = boost::bind(&path_follower_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_follower_base::update, this);
controller_commands_pub_ = nh_.advertise<hector_msgs::Controller_Commands>("controller_commands", 1);
state_init_ = false;
current_path_init_ = false;
}
void path_follower_base::update(const ros::TimerEvent &)
{
struct output_s output;
if (state_init_ == true && current_path_init_ == true)
{
follow(params_, input_, output);
hector_msgs::Controller_Commands msg;
msg.chi_c = output.chi_c;
msg.landing=input_.landing;
msg.takeoff=input_.takeoff;
if (input_.Va_c_path == 0)
{
// ROS_INFO("Hello1");
msg.Va_c = 0;
}
else
{
// ROS_INFO("Hello2");
msg.Va_c = input_.Va_d;
}
// ROS_INFO("what %f",msg.Va_c);
msg.h_c = output.h_c;
msg.phi_ff = output.phi_ff;
controller_commands_pub_.publish(msg);
}
}
void path_follower_base::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
{
// input_.pn = msg->position[0]; /** position north */
// input_.pe = msg->position[1]; /** position east */
// input_.h = -msg->position[2]; /** altitude */
// input_.chi = msg->chi;
// input_.Va = msg->Va;
// state_init_ = true;
}
void path_follower_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
// ROS_INFO("%g", msg->pose.pose.position.x); /** position north */
input_.pn = msg->pose.pose.position.x; /** position north */
input_.pe = msg->pose.pose.position.y; /** position east */
input_.h = msg->pose.pose.position.z; /** altitude */
input_.Va = sqrt(pow(msg->twist.twist.linear.x, 2) + pow(msg->twist.twist.linear.y, 2));
input_.chi = path_follower_base::Quaternion_to_Euler(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, msg->pose.pose.orientation.z, msg->pose.pose.orientation.w);
// ROS_INFO("%g %g %g %g %g",input_.pn,input_.pe,input_.h,input_.Va,input_.chi);
state_init_ = true;
}
float path_follower_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
{
float Radx;
float sum;
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
if (sum == 0)
{
sum = 0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
if (fabs(q2) < fabs(q3))
{
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
if (q2 * q3 > 0)
{
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
}
//Radx = asin(2.0f*(q2*q3 + q0*q1));
//Rady = atan2(-2 * q1 * q3 + 2 * q0 * q2, q3*q3 - q2 * q2 - q1 * q1 + q0 * q0);
//Radz = atan2(2 * q1*q2 - 2 * q0*q3, q2*q2 - q3*q3 + q0*q0 - q1*q1);
//return Radx;
}
void path_follower_base::current_path_callback(const hector_msgs::Current_PathConstPtr &msg)
{
if (msg->path_type == msg->LINE_PATH)
input_.p_type = path_type::Line;
else if (msg->path_type == msg->ORBIT_PATH)
input_.p_type = path_type::Orbit;
else if (msg->path_type == msg->STAR_PATH) // add by kobe
input_.p_type = path_type::Star;
input_.Va_d = msg->Va_d;
// ROS_INFO("hello %f",input_.Va_d);
for (int i = 0; i < 3; i++)
{
input_.r_path[i] = msg->r[i];
input_.q_path[i] = msg->q[i];
input_.c_orbit[i] = msg->c[i];
}
input_.rho_orbit = msg->rho;
input_.lam_orbit = msg->lambda;
input_.h_c_path = msg->h_c; //add by kobe
input_.Va_c_path = msg->Va_d;
input_.landing = msg->landing;
input_.takeoff = msg->takeoff;
current_path_init_ = true;
}
void path_follower_base::reconfigure_callback(hector::FollowerConfig &config, uint32_t level)
{
params_.chi_infty = config.CHI_INFTY;
params_.k_path = config.K_PATH;
params_.k_orbit = config.K_ORBIT;
}
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_path_follower");
hector::path_follower_base *path = new hector::path_follower_example();
ros::spin();
return 0;
}

81
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_follower_example.cpp Normal file
View File

@ -0,0 +1,81 @@
#include "path_follower_example.h"
namespace hector
{
path_follower_example::path_follower_example()
{
}
void path_follower_example::follow(const params_s &params, const input_s &input, output_s &output)
{
if (input.p_type == path_type::Line) // follow straight line path specified by r and q
{
// compute wrapped version of the path angle
float chi_q = atan2f(input.q_path[1], input.q_path[0]);
// modified by kobe
float chi=input.chi;
while (chi < -M_PI)
chi += 2*M_PI;
while (chi > M_PI)
chi -= 2*M_PI;
while (chi_q - chi < -M_PI)
chi_q += 2*M_PI;
while (chi_q - chi > M_PI)
chi_q -= 2*M_PI;
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
// heading command
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error);
//ROS_INFO("hello1: %g %g %g %g",chi_q,input.chi,chi,output.chi_c);
// desired altitude
output.h_c = -input.h_c_path;//h_c_path is negative;h_c is positive.
//kobe modified end
/* ////for truth not state
while (chi_q - input.chi < -M_PI)
chi_q += 2*M_PI;
while (chi_q - input.chi > M_PI)
chi_q -= 2*M_PI;
float path_error = -sinf(chi_q)*(input.pn - input.r_path[0]) + cosf(chi_q)*(input.pe - input.r_path[1]);
// heading command
output.chi_c = chi_q - params.chi_infty*2/M_PI*atanf(params.k_path*path_error); //desired altitude
float h_d = -input.r_path[2] - sqrtf(powf((input.r_path[0] - input.pn), 2) + powf((input.r_path[1] - input.pe),
2))*(input.q_path[2])/sqrtf(powf(input.q_path[0], 2) + powf(input.q_path[1], 2));
// commanded altitude is desired altitude
output.h_c = h_d;// -input.r_path[2] - x// important for hover// kobe denote
*/
output.phi_ff = 0.0;
}
else if(input.p_type == path_type::Orbit)// follow a orbit path specified by c_orbit, rho_orbit, and lam_orbit
{
float d = sqrtf(powf((input.pn - input.c_orbit[0]), 2) + powf((input.pe - input.c_orbit[1]),
2)); // distance from orbit center
// compute wrapped version of angular position on orbit
float varphi = atan2f(input.pe - input.c_orbit[1], input.pn - input.c_orbit[0]);
while ((varphi - input.chi) < -M_PI)
varphi += 2*M_PI;
while ((varphi - input.chi) > M_PI)
varphi -= 2*M_PI;
//compute orbit error
float norm_orbit_error = (d - input.rho_orbit)/input.rho_orbit;
output.chi_c = varphi + input.lam_orbit*(M_PI/2 + atanf(params.k_orbit*norm_orbit_error));
// commanded altitude is the height of the orbit
float h_d = -input.c_orbit[2];
output.h_c = h_d;
output.phi_ff = 0;//(norm_orbit_error < 0.5 ? input.lam_orbit*atanf(input.Va*input.Va/(9.8*input.rho_orbit)) : 0);
}
else if(input.p_type == path_type::Star)// towards to a point in any direction on platform,r is the center;add by kobe
{
}
output.Va_c = input.Va_d;
}
} //end namespace

238
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_manager_base.cpp Normal file
View File

@ -0,0 +1,238 @@
#include "path_manager_base.h"
#include "path_manager_example.h"
namespace hector
{
path_manager_base::path_manager_base() : nh_(ros::NodeHandle()), /** nh_ stuff added here */
nh_private_(ros::NodeHandle("~"))
{
nh_private_.param<double>("R_min", params_.R_min, 25); //25
nh_private_.param<double>("update_rate", update_rate_, 10.0);
vehicle_state_sub_ = nh_.subscribe("state", 10, &path_manager_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &path_manager_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &path_manager_base::bebop1_state_callback, this);
new_waypoint_sub_ = nh_.subscribe("waypoint_path", 10, &path_manager_base::new_waypoint_callback, this);
current_path_pub_ = nh_.advertise<hector_msgs::Current_Path>("current_path", 10);
goal_info_pub_ = nh_.advertise<hector_msgs::Goal_Info>("Goal_Info", 10);
down_data_pub_ = nh_.advertise<hector_msgs::Down_Data_New>("serial_commu_down", 10);
waypoint_received_sub_ = nh_.subscribe("new_waypoint", 10, &path_manager_base::waypoint_received_callback, this);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_manager_base::current_path_publish, this);
num_waypoints_ = 0;
state_init_ = false;
waypoint_received_ = false;
}
void path_manager_base::waypoint_received_callback(const hector_msgs::Waypoint &msg)
{
float ell = sqrtf((waypoint_end_.w[0] - msg.w[0]) * (waypoint_end_.w[0] - msg.w[0]) +
(waypoint_end_.w[1] - msg.w[1]) * (waypoint_end_.w[1] - msg.w[1]));
if (ell < 2.0 * params_.R_min)
{
ROS_ERROR("The distance between nodes must be larger than 2R.");
return;
}
waypoint_start_ = waypoint_end_;
waypoint_end_.w[0] = msg.w[0];
waypoint_end_.w[1] = msg.w[1];
waypoint_end_.w[2] = msg.w[2];
waypoint_end_.chi_d = msg.chi_d;
waypoint_end_.chi_valid = msg.chi_valid;
waypoint_end_.Va_d = msg.Va_d;
waypoint_received_ = true;
}
void path_manager_base::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
//state_init_ = true;
}
void path_manager_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
bebop1_state_ = *msg;
state_init_ = true;
}
void path_manager_base::new_waypoint_callback(const hector_msgs::Waypoint &msg)
{
if (msg.clear_wp_list == true)
{
waypoints_.clear();
num_waypoints_ = 0;
idx_a_ = 0;
min_distance_ = 1000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
min_distance_titude = 10000000000;
//return;
}
if (msg.set_current || num_waypoints_ == 0)//mean to set the start point as the a goal-never use---kobe denote
{
waypoint_s currentwp;
currentwp.w[0] = bebop1_state_.pose.pose.position.x;
currentwp.w[1] = bebop1_state_.pose.pose.position.y;
currentwp.w[2] = (-bebop1_state_.pose.pose.position.z > -25 ? msg.w[2] : -bebop1_state_.pose.pose.position.z);
currentwp.chi_d= path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
// currentwp.w[0] = vehicle_state_.position[0];
// currentwp.w[1] = vehicle_state_.position[1];
// currentwp.w[2] = (vehicle_state_.position[2] > -25 ? msg.w[2] : vehicle_state_.position[2]);
// currentwp.chi_d = vehicle_state_.chi;
// add by kobe
currentwp.lat = vehicle_state_.Wd;
currentwp.lon = vehicle_state_.Jd;
currentwp.chi_valid = msg.chi_valid;
currentwp.Va_d = msg.Va_d;
currentwp.landing=msg.landing;
currentwp.takeoff=msg.takeoff;
waypoints_.clear();
waypoints_.push_back(currentwp);
num_waypoints_ = 1;
idx_a_ = 0;
min_distance_ = 10000000000; //add by kobe, just need to be larger than the large-enter-ball's radius.
min_distance_titude = 10000000000;
show = 1;
show2=1;
}
waypoint_s nextwp;
nextwp.w[0] = msg.w[0];
nextwp.w[1] = msg.w[1];
nextwp.w[2] = msg.w[2];
// add by kobe
nextwp.lat=msg.lat;
nextwp.lon=msg.lon;
nextwp.chi_d = msg.chi_d;
nextwp.chi_valid = msg.chi_valid;
nextwp.Va_d = msg.Va_d;
nextwp.landing=msg.landing;
nextwp.takeoff=msg.takeoff;
waypoints_.push_back(nextwp);
num_waypoints_++;
// ROS_INFO("num_waypoints_: %d", num_waypoints_);
}
void path_manager_base::current_path_publish(const ros::TimerEvent &)
{
struct input_s input;
input.pn = bebop1_state_.pose.pose.position.x; /** position north */
input.pe = bebop1_state_.pose.pose.position.y; /** position east */
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
input.chi =path_manager_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
// input.pn = vehicle_state_.position[0]; /** position north */
// input.pe = vehicle_state_.position[1]; /** position east */
// input.h = -vehicle_state_.position[2]; /** altitude */
// input.chi = vehicle_state_.chi;
// input.va = vehicle_state_.Va;
//add by kobe
input.lat=vehicle_state_.Wd;
input.lon=vehicle_state_.Jd;
struct output_s output;
if (state_init_ == true)
{
manage(params_, input, output);
}
hector_msgs::Current_Path current_path;
hector_msgs::Goal_Info goal_info;
hector_msgs::Down_Data_New down_data;
// modified by kobe
if (output.flag == 1)
current_path.path_type = current_path.LINE_PATH;
else if (output.flag == 0)
current_path.path_type = current_path.ORBIT_PATH;
else if (output.flag == 2)
current_path.path_type = current_path.STAR_PATH;
for (int i = 0; i < 3; i++)
{
current_path.r[i] = output.r[i];
current_path.q[i] = output.q[i];
current_path.c[i] = output.c[i];
}
current_path.rho = output.rho;
current_path.lambda = output.lambda;
current_path.h_c = output.h_c;
current_path.Va_d = output.Va_c; //new speed
current_path.takeoff=output.takeoff;
current_path.landing=output.landing;
current_path_pub_.publish(current_path);
goal_info.altitude=-output.h_c;
goal_info.v_d=output.Va_d;
goal_info.action=1;//desired action move
for (int i = 0; i < 2; i++)
{
goal_info.xy[i] = output.gxy[i];
goal_info.ll[i] = output.gll[i];
}
goal_info_pub_.publish(goal_info); // add by kobe
down_data.latitude = output.gll[0];
down_data.longitude = output.gll[1];
down_data.altitude = -output.h_c;
down_data.plane_speed= output.Va_d;
down_data.status_word= 1;
//ROS_INFO("Look here %f %f %f %f",down_data.latitude,down_data.longitude,down_data.altitude,output.Va_d);
down_data_pub_.publish(down_data); // add by kobe
}
//add by kobe
float path_manager_base::Quaternion_to_Euler(float q0, float q1, float q2, float q3)
{
float Radx;
float sum;
sum = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
if (sum == 0)
{
sum = 0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
if (fabs(q2) < fabs(q3))
{
Radx = asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
if (q2 * q3 > 0)
{
Radx = 3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
else
{
Radx = -3.14159 - asin(2.0f * (q2 * q3 + q0 * q1));
return Radx;
}
}
}
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_path_manager");
hector::path_manager_base *est = new hector::path_manager_example();
ros::spin();
return 0;
}

717
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_manager_example.cpp Normal file
View File

@ -0,0 +1,717 @@
#include "path_manager_example.h"
#include "ros/ros.h"
#include <cmath>
namespace hector
{
path_manager_example::path_manager_example() : path_manager_base()
{
fil_state_ = fillet_state::STRAIGHT;
// use this !
dub_state_ = dubin_state::FIRST;
}
// done
void path_manager_example::manage(const params_s &params, const input_s &input, output_s &output)
{
//ROS_INFO("num_waypoints_: %d",num_waypoints_);
if (num_waypoints_ < 2)
{
ROS_WARN_THROTTLE(4, "No waypoints received! Loitering about origin at 50m");
output.flag = 0; //modified by kobe
output.Va_d = 12;
output.c[0] = 0.0f;
output.c[1] = 0.0f;
output.c[2] = -50.0f;
// Radius of orbital path (m)
output.rho = params.R_min;
// Direction of orbital path (clockwise is 1, counterclockwise is -1)
output.lambda = 1;
}
else
{
// do this, idx_a_ is initialized zero
if (waypoints_[idx_a_].chi_valid)
{
manage_dubins(params, input, output);
}
else
{
/** Switch the following for flying directly to waypoints, or filleting corners */
manage_line(params, input, output);
//manage_fillet(params, input, output);
// manage_star(params, input, output); //add by kobe
}
}
}
// add by kobe
void path_manager_example::manage_star(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f w_start;
w_start << input.pn, input.pe, -input.h; //aircraft's position
int idx_b;
int idx_c;
float the_distance_;
double the_distance_titude; //for latitude and longtitude
idx_b = idx_a_ + 1;
if (idx_a_ < num_waypoints_ - 2)
{
idx_c = idx_b + 1;
}
else
{
idx_c = idx_b;
}
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w); //the goal
the_distance_ = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2));
the_distance_titude = earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon);
output.flag = 1; //modified by kobe
output.Va_d = waypoints_[idx_b].Va_d; //modified by kobe
//ROS_INFO("position: %g %g %g",w_start(0),w_start(1),w_start(2));
//ROS_INFO("Current info are: %f %f %f %f", input.va, -input.h,w_start(0),w_start(1));
output.r[0] = w_start(0);
output.r[1] = w_start(1);
output.r[2] = w_start(2);
Eigen::Vector3f q_im1 = (w_i - w_start).normalized();
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
output.gxy[0] = w_i(0);
output.gxy[1] = w_i(1);
output.gll[0] = waypoints_[idx_b].lat;
output.gll[1] = waypoints_[idx_b].lon;
output.h_c = w_i(2); //altitude of the goal
if (idx_a_ == 0&&show2 == 1)
{
ROS_INFO("First goal info are: %f %f", w_i(0), w_i(1));
ROS_INFO("Desired fly speed and height are: %f %f", output.Va_d, -w_i(2));
show2=0;
}
//enter the slow-down area,recompute the speed
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < output.Va_d * 2 && idx_a_ == num_waypoints_ - 2) //enter the slowdown-ball and the goal is the last goal
{
output.Va_c = sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2))/2;
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5 )
{
output.Va_c = 0;
}
// ROS_INFO("Now the speed is: %f",output.Va_c);
}
else
{
output.Va_c = output.Va_d;
}
output.landing=waypoints_[idx_b].landing;
output.takeoff=waypoints_[idx_b].takeoff;
/* //1.enter a tiny ball and switch goal
if (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 5) //enter a tiny ball and switch goal
{
//ROS_INFO("00000o- %g %g %g", w_start(2), w_i(2), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
//ROS_INFO("last goal is: %f %f", w_i(0), w_i(1));
//ROS_INFO("Va- %f/n", input.va);
if (idx_a_ == num_waypoints_ - 2)
{
idx_a_ = idx_a_; //do not update goal
//ROS_INFO("idx_a_- %f/n", idx_a_);
}
else
{
idx_a_++;
}
}
*/
/*
//2.enter a half-plane and switch,ahead 5 m from the goal point
Eigen::Vector3f w_i_plane;
w_i_plane=w_i;
w_i_plane(0)=w_i_plane(0)-5*( ( w_i(0)-w_im1(0) ) / ( 0.001+fabs(w_i(0)-w_im1(0)) ) );
w_i_plane(1)=w_i_plane(1)-5*( ( w_i(1)-w_im1(1) ) / ( 0.001+fabs(w_i(1)-w_im1(1)) ) );
if ((w_i_plane - w_start).normalized().dot((w_i_plane - w_im1).normalized())<0)//plane-method
{
if (idx_a_ == num_waypoints_ - 2)
idx_a_ = idx_a_;//do not update goal
else
idx_a_++;
}
*/
//3.Enter a large ball(large-enter-ball) first. Anyway, when the distance from the aircraft to goal become larger,switch to the next goal.
if (the_distance_ < min_distance_)
{
min_distance_ = the_distance_;
}
if (the_distance_titude < min_distance_titude)
{
min_distance_titude = the_distance_titude;
}
// ROS_INFO("distance is : %.20f %f", earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon), sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)));
//ROS_INFO("000distance- %f %f", the_distance_, min_distance_);
//1.进入大圈且最小距离开始增大时;2.进入一个小圈时<E59C88>?分别针对北东坐标和经纬度坐标
judge_condition1 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 3 && the_distance_ > min_distance_ + 1);
judge_condition2 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 3 && the_distance_titude > min_distance_titude + 1);
judge_condition3 = (sqrt(pow(w_start(0) - w_i(0), 2) + pow(w_start(1) - w_i(1), 2)) < 0.5);
judge_condition4 = (earth_distance(input.lat, input.lon, waypoints_[idx_b].lat, waypoints_[idx_b].lon) < 0.5);
// height judge
judge_condition5 = (sqrt(pow(w_start(2) - w_i(2), 2)) < 10);
//notice here
judge_condition2=0;
judge_condition4=0;
if ((judge_condition1 || judge_condition2 || judge_condition3 || judge_condition4) && judge_condition5) //switch
{
//ROS_INFO("distance- %f %f", the_distance_, min_distance_);
if (idx_a_ == num_waypoints_ - 2)
{
if (show == 1)
{
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
ROS_INFO("No more goal, just hover and wait for the next goal.");
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
ROS_INFO("Current speed and height are: %f %f\n\n", input.va, input.h);
show = 0;
}
idx_a_ = idx_a_; //do not update goal
}
else
{
ROS_INFO("This goal is reached: %f %f", w_i(0), w_i(1));
ROS_INFO("Current position are: %f %f", input.pn, input.pe);
ROS_INFO("Desired speed and height are: %f %f", output.Va_d, -w_i(2));
ROS_INFO("Current speed and height are: %f %f", input.va, input.h);
ROS_INFO("New goal info are: %f %f\n\n", w_ip1(0), w_ip1(1));
min_distance_ = sqrt(pow(w_i(0) - w_ip1(0), 2) + pow(w_i(1) - w_ip1(1), 2));
idx_a_++;
}
}
//
}
double path_manager_example::earth_distance(double latitude1, double longitude1, double latitude2, double longitude2)
{
double Lat1 = latitude1 * M_PI_F / 180.00; // 纬度
double Lat2 = latitude2 * M_PI_F / 180.00;
double a = Lat1 - Lat2; //两点纬度之差
double b = longitude1 * M_PI_F / 180.00 - longitude2 * M_PI_F / 180.00; //经度之差
double s = 2 * asin(sqrt(pow(sin(a / 2), 2) + cos(Lat1) * cos(Lat2) * pow(sin(b / 2), 2))); //计算两点距离的公<E79A84>?/better when value is very samll
//double s2 = acos(cos(Lat1)*cos(Lat2)*cos(b)+sin(Lat1)*sin(Lat2));//计算两点距离的公<E79A84>?
s = s * EARTH_RADIUS; //弧长乘地球半径（半径为米<E4B8BA>?
//s2 = s2 * EARTH_RADIUS;//弧长乘地球半径（半径为米<E4B8BA>?
return s;
}
void path_manager_example::manage_line(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f p;
p << input.pn, input.pe, -input.h;
int idx_b;
int idx_c;
if (idx_a_ == num_waypoints_ - 1)
{
idx_b = 0;
idx_c = 1;
}
else if (idx_a_ == num_waypoints_ - 2)
{
idx_b = num_waypoints_ - 1;
idx_c = 0;
}
else
{
idx_b = idx_a_ + 1;
idx_c = idx_b + 1;
}
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w);
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
output.flag = true;
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = w_im1(0);
output.r[1] = w_im1(1);
output.r[2] = w_im1(2);
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
Eigen::Vector3f n_i = (q_im1 + q_i).normalized();
if ((p - w_i).dot(n_i) > 0.0f)
{
if (idx_a_ == num_waypoints_ - 1)
idx_a_ = 0;
else
idx_a_++;
}
}
void path_manager_example::manage_fillet(const params_s &params, const input_s &input, output_s &output)
{
if (num_waypoints_ < 3) //since it fillets don't make sense between just two points
{
manage_line(params, input, output);
return;
}
Eigen::Vector3f p;
p << input.pn, input.pe, -input.h;
int idx_b;
int idx_c;
if (idx_a_ == num_waypoints_ - 1)
{
idx_b = 0;
idx_c = 1;
}
else if (idx_a_ == num_waypoints_ - 2)
{
idx_b = num_waypoints_ - 1;
idx_c = 0;
}
else
{
idx_b = idx_a_ + 1;
idx_c = idx_b + 1;
}
Eigen::Vector3f w_im1(waypoints_[idx_a_].w);
Eigen::Vector3f w_i(waypoints_[idx_b].w);
Eigen::Vector3f w_ip1(waypoints_[idx_c].w);
float R_min = params.R_min;
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = w_im1(0);
output.r[1] = w_im1(1);
output.r[2] = w_im1(2);
Eigen::Vector3f q_im1 = (w_i - w_im1).normalized();
Eigen::Vector3f q_i = (w_ip1 - w_i).normalized();
float beta = acosf(-q_im1.dot(q_i));
Eigen::Vector3f z;
switch (fil_state_)
{
case fillet_state::STRAIGHT:
output.flag = 1; //modified by kobe
output.q[0] = q_im1(0);
output.q[1] = q_im1(1);
output.q[2] = q_im1(2);
output.c[0] = 1;
output.c[1] = 1;
output.c[2] = 1;
output.rho = 1;
output.lambda = 1;
z = w_i - q_im1 * (R_min / tanf(beta / 2.0));
if ((p - z).dot(q_im1) > 0)
fil_state_ = fillet_state::ORBIT;
break;
case fillet_state::ORBIT:
output.flag = 0; //modified by kobe
output.q[0] = q_i(0);
output.q[1] = q_i(1);
output.q[2] = q_i(2);
Eigen::Vector3f c = w_i - (q_im1 - q_i).normalized() * (R_min / sinf(beta / 2.0));
output.c[0] = c(0);
output.c[1] = c(1);
output.c[2] = c(2);
output.rho = R_min;
output.lambda = ((q_im1(0) * q_i(1) - q_im1(1) * q_i(0)) > 0 ? 1 : -1);
z = w_i + q_i * (R_min / tanf(beta / 2.0));
if ((p - z).dot(q_i) > 0)
{
if (idx_a_ == num_waypoints_ - 1)
idx_a_ = 0;
else
idx_a_++;
fil_state_ = fillet_state::STRAIGHT;
}
break;
}
}
void path_manager_example::manage_dubins(const params_s &params, const input_s &input, output_s &output)
{
Eigen::Vector3f p;
//ROS_INFO("input.pn: %f, input.pe: %f, -input.h: %f",input.pn, input.pe, -input.h);
p << input.pn, input.pe, -input.h;
/* uint8 path_type # Indicates strait line or orbital path
float32 Va_d # Desired airspeed (m/s)
float32[3] r # Vector to origin of straight line path (m)
float32[3] q # Unit vector, desired direction of travel for line path
float32[3] c # Center of orbital path (m)
float32 rho # Radius of orbital path (m)
int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1)
uint8 ORBIT_PATH = 0
uint8 LINE_PATH = 1
int8 CLOCKWISE = 1
int8 COUNT_CLOCKWISE = -1 */
output.Va_d = waypoints_[idx_a_].Va_d;
output.r[0] = 0;
output.r[1] = 0;
output.r[2] = 0;
output.q[0] = 0;
output.q[1] = 0;
output.q[2] = 0;
output.c[0] = 0;
output.c[1] = 0;
output.c[2] = 0;
//ROS_INFO("dub_state_: %d",dub_state_);
switch (dub_state_)
{
case dubin_state::FIRST:
// path planning
// Algorithm11 P211
dubinsParameters(waypoints_[0], waypoints_[1], params.R_min);
waypoint_start_ = waypoints_[0];
waypoint_end_ = waypoints_[1];
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
// p: current pos
// w1: waypoint 1
// q1: unit vector of H1
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
break;
case dubin_state::BEFORE_H1:
//waypoint_received_ = false;
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // entering H1
{
dub_state_ = dubin_state::STRAIGHT;
}
break;
case dubin_state::BEFORE_H1_WRONG_SIDE:
//waypoint_received_ = false;
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.cs(0);
output.c[1] = dubinspath_.cs(1);
output.c[2] = dubinspath_.cs(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lams;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) < 0) // exit H1
{
dub_state_ = dubin_state::BEFORE_H1;
}
break;
case dubin_state::STRAIGHT:
output.flag = 1; //modified by kobe
output.r[0] = dubinspath_.w1(0);
output.r[1] = dubinspath_.w1(1);
output.r[2] = dubinspath_.w1(2);
// output.r[0] = dubinspath_.z1(0);
// output.r[1] = dubinspath_.z1(1);
// output.r[2] = dubinspath_.z1(2);
output.q[0] = dubinspath_.q1(0);
output.q[1] = dubinspath_.q1(1);
output.q[2] = dubinspath_.q1(2);
output.rho = 1;
output.lambda = 1;
//ROS_INFO("pos: [%f,%f,%f],w2: [%f,%f,%f],q1: [%f,%f,%f]",p[0],p[1],p[2],dubinspath_.w2[0],dubinspath_.w2[1],dubinspath_.w2[2],dubinspath_.q1[0],dubinspath_.q1[1],dubinspath_.q1[2]);
//ROS_INFO("w3: [%f,%f,%f]",dubinspath_.w3[0],dubinspath_.w3[1],dubinspath_.w3[2]);
//ROS_INFO("w1: [%f,%f,%f]",dubinspath_.w1[0],dubinspath_.w1[1],dubinspath_.w1[2]);
if (waypoint_received_)
{
//start new path
// plan new Dubin's path to next waypoint configuration
waypoint_start_.w[0] = p[0];
waypoint_start_.w[1] = p[1];
waypoint_start_.w[2] = p[2];
waypoint_start_.chi_d = 0;
waypoint_start_.chi_valid = true;
waypoint_start_.Va_d = 12;
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
waypoint_received_ = false;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
}
else
{
if ((p - dubinspath_.w2).dot(dubinspath_.q1) >= 0) // entering H2
{
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // start in H3
{
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H3;
}
}
}
break;
case dubin_state::BEFORE_H3:
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.ce(0);
output.c[1] = dubinspath_.ce(1);
output.c[2] = dubinspath_.ce(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lame;
if ((p - dubinspath_.w3).dot(dubinspath_.q3) >= 0) // entering H3
{
if (!waypoint_received_)
{
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
//dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
}
else
{
//dub_state_ = dubin_state::BEFORE_H1;
dub_state_ = dubin_state::BEFORE_H3;
}
}
else
{
//start new path
// plan new Dubin's path to next waypoint configuration
dubinsParameters(waypoint_start_, waypoint_end_, params.R_min);
waypoint_received_ = false;
if ((p - dubinspath_.w1).dot(dubinspath_.q1) >= 0) // start in H1
{
dub_state_ = dubin_state::BEFORE_H1_WRONG_SIDE;
}
else
{
dub_state_ = dubin_state::BEFORE_H1;
}
}
}
break;
case dubin_state::BEFORE_H3_WRONG_SIDE:
output.flag = 0; //modified by kobe
output.c[0] = dubinspath_.ce(0);
output.c[1] = dubinspath_.ce(1);
output.c[2] = dubinspath_.ce(2);
output.rho = dubinspath_.R;
output.lambda = dubinspath_.lame;
/*
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
{
if(!waypoint_received_)
//dub_state_ = dubin_state::BEFORE_H1;
dub_state_ = dubin_state::BEFORE_H3;
else
dub_state_ = dubin_state::BEFORE_H1;
}
*/
if ((p - dubinspath_.w3).dot(dubinspath_.q3) < 0) // exit H3
dub_state_ = dubin_state::BEFORE_H3;
else
dub_state_ = dubin_state::BEFORE_H3_WRONG_SIDE;
break;
}
}
Eigen::Matrix3f path_manager_example::rotz(float theta)
{
Eigen::Matrix3f R;
R << cosf(theta), -sinf(theta), 0,
sinf(theta), cosf(theta), 0,
0, 0, 1;
return R;
}
// done
float path_manager_example::mo(float in)
{
float val;
if (in > 0)
val = fmod(in, 2.0 * M_PI_F);
else
{
float n = floorf(in / 2.0 / M_PI_F);
val = in - n * 2.0 * M_PI_F;
}
return val;
}
void path_manager_example::dubinsParameters(const waypoint_s start_node, const waypoint_s end_node, float R)
{
float ell = sqrtf((start_node.w[0] - end_node.w[0]) * (start_node.w[0] - end_node.w[0]) +
(start_node.w[1] - end_node.w[1]) * (start_node.w[1] - end_node.w[1]));
if (ell < 2.0 * R)
{
ROS_ERROR("SHOULD NOT HAPPEN: The distance between nodes must be larger than 2R.");
}
else
{
dubinspath_.ps(0) = start_node.w[0];
dubinspath_.ps(1) = start_node.w[1];
dubinspath_.ps(2) = start_node.w[2];
dubinspath_.chis = start_node.chi_d;
dubinspath_.pe(0) = end_node.w[0];
dubinspath_.pe(1) = end_node.w[1];
dubinspath_.pe(2) = end_node.w[2];
dubinspath_.chie = end_node.chi_d;
Eigen::Vector3f crs = dubinspath_.ps;
crs(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chis) - sinf(M_PI_2_F) * sinf(dubinspath_.chis));
crs(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chis) + cosf(M_PI_2_F) * sinf(dubinspath_.chis));
Eigen::Vector3f cls = dubinspath_.ps;
cls(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chis) - sinf(-M_PI_2_F) * sinf(dubinspath_.chis));
cls(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chis) + cosf(-M_PI_2_F) * sinf(dubinspath_.chis));
Eigen::Vector3f cre = dubinspath_.pe;
cre(0) += R * (cosf(M_PI_2_F) * cosf(dubinspath_.chie) - sinf(M_PI_2_F) * sinf(dubinspath_.chie));
cre(1) += R * (sinf(M_PI_2_F) * cosf(dubinspath_.chie) + cosf(M_PI_2_F) * sinf(dubinspath_.chie));
Eigen::Vector3f cle = dubinspath_.pe;
cle(0) += R * (cosf(-M_PI_2_F) * cosf(dubinspath_.chie) - sinf(-M_PI_2_F) * sinf(dubinspath_.chie));
cle(1) += R * (sinf(-M_PI_2_F) * cosf(dubinspath_.chie) + cosf(-M_PI_2_F) * sinf(dubinspath_.chie));
float theta, theta2;
// compute L1
theta = atan2f(cre(1) - crs(1), cre(0) - crs(0));
float L1 = (crs - cre).norm() + R * mo(2.0 * M_PI_F + mo(theta - M_PI_2_F) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta - M_PI_2_F));
// compute L2
ell = (cle - crs).norm();
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
float L2;
if (2.0 * R > ell)
L2 = 9999.0f;
else
{
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
L2 = sqrtf(ell * ell - 4.0 * R * R) + R * mo(2.0 * M_PI_F + mo(theta2) - mo(dubinspath_.chis - M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta2 + M_PI_F) - mo(dubinspath_.chie + M_PI_2_F));
}
// compute L3
ell = (cre - cls).norm();
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
float L3;
if (2.0 * R > ell)
L3 = 9999.0f;
else
{
theta2 = acosf(2.0 * R / ell);
L3 = sqrtf(ell * ell - 4 * R * R) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + theta2)) + R * mo(2.0 * M_PI_F + mo(dubinspath_.chie - M_PI_2_F) - mo(theta + theta2 - M_PI_F));
}
// compute L4
theta = atan2f(cle(1) - cls(1), cle(0) - cls(0));
float L4 = (cls - cle).norm() + R * mo(2.0 * M_PI_F + mo(dubinspath_.chis + M_PI_2_F) - mo(theta + M_PI_2_F)) + R * mo(2.0 * M_PI_F + mo(theta + M_PI_2_F) - mo(dubinspath_.chie + M_PI_2_F));
// L is the minimum distance
int idx = 1;
dubinspath_.L = L1;
if (L2 < dubinspath_.L)
{
dubinspath_.L = L2;
idx = 2;
}
if (L3 < dubinspath_.L)
{
dubinspath_.L = L3;
idx = 3;
}
if (L4 < dubinspath_.L)
{
dubinspath_.L = L4;
idx = 4;
}
Eigen::Vector3f e1;
// e1.zero();
e1(0) = 1;
e1(1) = 0;
e1(2) = 0;
switch (idx)
{
case 1:
dubinspath_.cs = crs;
dubinspath_.lams = 1;
dubinspath_.ce = cre;
dubinspath_.lame = 1;
dubinspath_.q1 = (cre - crs).normalized();
dubinspath_.w1 = dubinspath_.cs + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(-M_PI_2_F) * dubinspath_.q1) * R;
break;
case 2:
dubinspath_.cs = crs;
dubinspath_.lams = 1;
dubinspath_.ce = cle;
dubinspath_.lame = -1;
ell = (cle - crs).norm();
theta = atan2f(cle(1) - crs(1), cle(0) - crs(0));
theta2 = theta - M_PI_2_F + asinf(2.0 * R / ell);
dubinspath_.q1 = rotz(theta2 + M_PI_2_F) * e1;
dubinspath_.w1 = dubinspath_.cs + (rotz(theta2) * e1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(theta2 + M_PI_F) * e1) * R;
break;
case 3:
dubinspath_.cs = cls;
dubinspath_.lams = -1;
dubinspath_.ce = cre;
dubinspath_.lame = 1;
ell = (cre - cls).norm();
theta = atan2f(cre(1) - cls(1), cre(0) - cls(0));
theta2 = acosf(2.0 * R / ell);
dubinspath_.q1 = rotz(theta + theta2 - M_PI_2_F) * e1;
dubinspath_.w1 = dubinspath_.cs + (rotz(theta + theta2) * e1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(theta + theta2 - M_PI_F) * e1) * R;
break;
case 4:
dubinspath_.cs = cls;
dubinspath_.lams = -1;
dubinspath_.ce = cle;
dubinspath_.lame = -1;
dubinspath_.q1 = (cle - cls).normalized();
dubinspath_.w1 = dubinspath_.cs + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
dubinspath_.w2 = dubinspath_.ce + (rotz(M_PI_2_F) * dubinspath_.q1) * R;
break;
}
dubinspath_.w3 = dubinspath_.pe;
dubinspath_.q3 = rotz(dubinspath_.chie) * e1;
dubinspath_.R = R;
}
}
} // namespace hector

63
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_planner.cpp Normal file
View File

@ -0,0 +1,63 @@
#include <ros/ros.h>
#include <hector_msgs/Waypoint.h>
#define EARTH_RADIUS 6378145.0f
#define num_waypoints 3
float pn2latitude(float pn,float init_lat_)
{
return (pn*180)/(EARTH_RADIUS*M_PI)+init_lat_;
}
float pe2longtitude(float pe,float init_lat_,float init_lon_)
{
return (pe*180)/(EARTH_RADIUS*cos(init_lat_*M_PI/180.0)*M_PI)+init_lon_;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_simple_path_planner");
ros::NodeHandle nh_;
ros::Publisher waypointPublisher = nh_.advertise<hector_msgs::Waypoint>("waypoint_path", 10);
float Va = 3;
int tmp;
tmp = 7; //notice here
float wps[tmp * num_waypoints] =
{
// pn,pe,altitude,-,va,lat,lon
// 20, 10, -10, 0, 3, pn2latitude(20,0), pe2longtitude(0,0,0),
// 30, 20, -10, 0, Va, pn2latitude(30,0), pe2longtitude(30,0,0),
// 0, 20, -12, 0, 5, pn2latitude(0,0), pe2longtitude(20,0,0),
0, 10, -10, 0, 3, pn2latitude(20,0), pe2longtitude(0,0,0),
};
for (int i(0); i < num_waypoints; i++)
{
ros::Duration(0.5).sleep();
hector_msgs::Waypoint new_waypoint;
new_waypoint.w[0] = wps[i * tmp + 0];
new_waypoint.w[1] = wps[i * tmp + 1];
new_waypoint.w[2] = wps[i * tmp + 2];
new_waypoint.chi_d = wps[i * tmp + 3];
new_waypoint.Va_d = wps[i * tmp + 4];
// add by kobe
new_waypoint.lat = wps[i * tmp + 5];
new_waypoint.lon = wps[i * tmp + 6];
new_waypoint.chi_valid = false; //kobe
if (i == 0)
new_waypoint.set_current = true;
else
new_waypoint.set_current = false;
new_waypoint.clear_wp_list = false;
waypointPublisher.publish(new_waypoint);
}
ros::Duration(1.5).sleep();
return 0;
}

93
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/path_point_transfer.cpp Normal file
View File

@ -0,0 +1,93 @@
#include "path_point_transfer.h"
// #include "path_point_transfer.h"
#define EARTH_RADIUS 6378137.0f //6378145.0f
namespace hector
{
float init_lat_ = 41.71671500; /**< Initial latitude in degrees */
float init_lon_ = 87.53306850; /**< Initial longitude in degrees */
float init_alt_ = 1180; //>0 1180
float latitude2pn(float latitude, float init_lat_)
{
return EARTH_RADIUS * (latitude - init_lat_) * M_PI / 180.0;
}
float longtitude2pe(float longitude, float init_lat_, float init_lon_)
{
return EARTH_RADIUS * cos(init_lat_ * M_PI / 180.0) * (longitude - init_lon_) * M_PI / 180.0;
}
path_point_transfer::path_point_transfer() : nh_(ros::NodeHandle()), /** nh_ stuff added here */
nh_private_(ros::NodeHandle("~"))
{
nh_private_.param<double>("update_rate", update_rate_, 10.0);
// vehicle_state_sub_ = nh_.subscribe("state", 1000, &path_point_transfer::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &path_point_transfer::bebop1_state_callback, this);
new_waypoint_sub_ = nh_.subscribe("serial_commu_down", 1000, &path_point_transfer::new_waypoint_callback, this);
current_path_pub_ = nh_.advertise<hector_msgs::Current_Path>("current_path", 1000);
update_timer_ = nh_.createTimer(ros::Duration(1.0 / update_rate_), &path_point_transfer::current_path_publish, this);
state_init_ = false;
waypoint_received_ = false;
}
path_point_transfer::~path_point_transfer()
{
// stop();
}
// void path_point_transfer::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
// {
// vehicle_state_ = *msg;
// state_init_ = true;
// }
void path_point_transfer::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
bebop1_state_ = *msg;
state_init_ = true;
}
void path_point_transfer::new_waypoint_callback(const hector_msgs::Down_Data_NewConstPtr &msg)
{
point_state_ = *msg;
waypoint_received_ = true;
}
void path_point_transfer::current_path_publish(const ros::TimerEvent &)
{
Eigen::Vector3f w_start;
Eigen::Vector3f w_end;
w_start << bebop1_state_.pose.pose.position.x, bebop1_state_.pose.pose.position.y, bebop1_state_.pose.pose.position.z; //aircraft's position
w_end << latitude2pn(point_state_.latitude, init_lat_), longtitude2pe(point_state_.longitude, init_lat_, init_lon_), -point_state_.altitude;
ROS_WARN("startpn,%g,startpe,%g,pn,%g,pe,%g",vehicle_state_.position[0], vehicle_state_.position[1],latitude2pn(point_state_.latitude, init_lat_), longtitude2pe(point_state_.longitude, init_lat_, init_lon_));
Eigen::Vector3f q_1 = (w_end - w_start).normalized();
hector_msgs::Current_Path current_path;
current_path.path_type = current_path.LINE_PATH;
current_path.Va_d = point_state_.plane_speed;
for (int i = 0; i < 3; i++)
{
current_path.r[i] = w_start[i];
current_path.q[i] = q_1[i];
}
current_path.h_c = -point_state_.altitude; //current_path.h_c<0 和state统一
current_path_pub_.publish(current_path);
}
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_path_point_transfer");
hector::path_point_transfer *cont = new hector::path_point_transfer();
// hector::path_point_transfer pp;
ros::spin();
return 0;
}

212
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/pseudo_controller_base.cpp Normal file
View File

@ -0,0 +1,212 @@
#include "pseudo_controller_base.h"
#include "pseudo_controller_example.h"
// modified by kobe
namespace hector
{
pseudo_controller_base::pseudo_controller_base():
nh_(ros::NodeHandle()),
nh_private_(ros::NodeHandle())
{
//ROS_INFO("hello");
vehicle_state_sub_ = nh_.subscribe("state", 10, &pseudo_controller_base::vehicle_state_callback, this);
bebop1_state_sub_ = nh_.subscribe("ground_truth/state", 10, &pseudo_controller_base::bebop1_state_callback, this);
//bebop1_state_sub_ = nh_.subscribe("/test/odom", 10, &pseudo_controller_base::bebop1_state_callback, this);
controller_commands_sub_ = nh_.subscribe("controller_commands", 10, &pseudo_controller_base::controller_commands_callback,this);
memset(&vehicle_state_, 0, sizeof(vehicle_state_));
memset(&bebop1_state_, 0, sizeof(bebop1_state_));
memset(&controller_commands_, 0, sizeof(controller_commands_));
nh_private_.param<double>("TRIM_T", params_.trim_t, 0.3);
nh_private_.param<double>("ALT_TOZ", params_.alt_toz, 1.5);
nh_private_.param<double>("ALT_HZ", params_.alt_hz, 0.2);
nh_private_.param<double>("TAU", params_.tau, 5.0);
nh_private_.param<double>("COURSE_KP", params_.c_kp, 0.7329);
//nh_private_.param<double>("COURSE_KD", params_.c_kd, 0.0);
nh_private_.param<double>("COURSE_KI", params_.c_ki, 0.0);
nh_private_.param<double>("AS_PITCH_KI", params_.a_p_ki, 0.0);
nh_private_.param<double>("AS_THR_KP", params_.a_t_kp, 0.9);
nh_private_.param<double>("AS_THR_KD", params_.a_t_kd, 0.0);
nh_private_.param<double>("AS_THR_KI", params_.a_t_ki, 0.0);
nh_private_.param<double>("ALT_KP", params_.a_kp, 0.045);
nh_private_.param<double>("ALT_KD", params_.a_kd, 0.0);
nh_private_.param<double>("ALT_KI", params_.a_ki, 0.0);
//nh_private_.param<double>("MAX_A", params_.max_a, 0.523);//rotate_trans
//nh_private_.param<double>("MAX_T", params_.max_t, 1.0);
nh_private_.param<double>("MAX_T", params_.max_t, 200.0);
func_ = boost::bind(&pseudo_controller_base::reconfigure_callback, this, _1, _2);
server_.setCallback(func_);
//pesudors_pub_ = nh_.advertise<rosflight_msgs::Command>("command", 10);
pesudors_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 10);
internals_pub_ = nh_.advertise<hector_msgs::Controller_Internals>("controller_inners", 10);
act_pub_timer_ = nh_.createTimer(ros::Duration(1.0/100.0), &pseudo_controller_base::actuator_controls_publish, this);
command_recieved_ = false;
}
void pseudo_controller_base::vehicle_state_callback(const hector_msgs::StateConstPtr &msg)
{
vehicle_state_ = *msg;
}
void pseudo_controller_base::bebop1_state_callback(const nav_msgs::Odometry::ConstPtr &msg)
{
bebop1_state_ = *msg;
}
void pseudo_controller_base::controller_commands_callback(const hector_msgs::Controller_CommandsConstPtr &msg)
{
command_recieved_ = true;
controller_commands_ = *msg;
}
void pseudo_controller_base::reconfigure_callback(hector::ControllerConfig &config, uint32_t level)
{
params_.trim_t = config.TRIM_T;
params_.c_kp = config.COURSE_KP;
//params_.c_kd = config.COURSE_KD;
params_.c_ki = config.COURSE_KI;
params_.a_p_ki = config.AS_PITCH_KI;
params_.a_t_kp = config.AS_THR_KP;
params_.a_t_kd = config.AS_THR_KD;
params_.a_t_ki = config.AS_THR_KI;
params_.a_kp = config.ALT_KP;
params_.a_kd = config.ALT_KD;
params_.a_ki = config.ALT_KI;
}
void pseudo_controller_base::actuator_controls_publish(const ros::TimerEvent &)
{
struct input_s input;
//input.h = -vehicle_state_.position[2];
//input.va = vehicle_state_.Va;
//input.chi = vehicle_state_.chi;
input.h = bebop1_state_.pose.pose.position.z; /** altitude */
input.va = sqrt(pow(bebop1_state_.twist.twist.linear.x,2)+pow(bebop1_state_.twist.twist.linear.y,2));
input.chi =pseudo_controller_base::Quaternion_to_Euler(bebop1_state_.pose.pose.orientation.x,bebop1_state_.pose.pose.orientation.y,bebop1_state_.pose.pose.orientation.z,bebop1_state_.pose.pose.orientation.w);
// ROS_INFO("LOOK here1: %f",input.chi);
input.Va_c = controller_commands_.Va_c;
input.h_c = controller_commands_.h_c;//3>controller_commands_.h_c)?3:controller_commands_.h_c;
input.chi_c = controller_commands_.chi_c;
input.takeoff = controller_commands_.takeoff;
input.landing = controller_commands_.landing;
input.Ts = 0.01f;
if (controller_commands_.Va_c==0)
{
reached=1;
}
struct output_s output;
if (command_recieved_ == true)
{
control(params_, input, output);
//convert_to_pwm(output);
geometry_msgs::Twist cmd_vel;
//rosflight_msgs::Command pseudors;
/* publish pseudors controls */
cmd_vel.linear.x = output.forward_trans;//head vel
cmd_vel.linear.y = output.left_right_trans;
cmd_vel.linear.z = output.up_down_trans;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;//output.roll_trans;
if (input.va > 0.1)
cmd_vel.angular.z = output.rotate_trans;//output.rotate_trans
else
cmd_vel.angular.z = 0;
pesudors_pub_.publish(cmd_vel);
//ROS_INFO("hello %f",bebop1_state_.pose.pose.position.z);
// ROS_INFO("x-y-z-w: %f %f %f %f",cmd_vel.linear.x,cmd_vel.linear.y,cmd_vel.linear.z,cmd_vel.angular.z);
// ROS_INFO("six: %f %f %f %f %f %f\n",input.h,input.h_c,input.va,input.Va_c,input.chi,input.chi_c);
// ROS_INFO("position: %f %f %f",bebop1_state_.pose.pose.position.x,bebop1_state_.pose.pose.position.y,bebop1_state_.pose.pose.position.z);
if (internals_pub_.getNumSubscribers() > 0)
{
hector_msgs::Controller_Internals inners;
//sinners.phi_c = output.phi_c;
//inners.theta_c = output.theta_c;
switch (output.current_zone)
{
case alt_zones::TAKE_OFF:
inners.alt_zone = inners.ZONE_TAKE_OFF;
break;
case alt_zones::CLIMB:
inners.alt_zone = inners.ZONE_CLIMB;
break;
case alt_zones::DESCEND:
inners.alt_zone = inners.ZONE_DESEND;
break;
case alt_zones::ALTITUDE_HOLD:
inners.alt_zone = inners.ZONE_ALTITUDE_HOLD;
break;
default:
break;
}
inners.aux_valid = false;
internals_pub_.publish(inners);
}
}
}
float pseudo_controller_base::Quaternion_to_Euler(float q0,float q1,float q2,float q3)
{
float Radx;
float sum;
sum = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
if (sum==0)
{
sum=0.0001;
}
q0 = q0 / sum;
q1 = q1 / sum;
q2 = q2 / sum;
q3 = q3 / sum;
// ROS_INFO("LOOK here2: %f",asin(2.0f*(q2*q3 + q0*q1)));
if (fabs(q2)<fabs(q3))
{
Radx = asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
else
{
if(q2*q3>0)
{
Radx = 3.14159-asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
else
{
Radx = -3.14159-asin(2.0f*(q2*q3 + q0*q1));
return Radx;
}
}
}
} //end namespace
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_controller");
hector::pseudo_controller_base *cont = new hector::pseudo_controller_example();
ros::spin();
return 0;
}

326
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/pseudo_controller_example.cpp Normal file
View File

@ -0,0 +1,326 @@
#include "pseudo_controller_example.h"
//modified by kobe
namespace hector
{
pseudo_controller_example::pseudo_controller_example() : pseudo_controller_base()
{
current_zone = alt_zones::PREPARE;
//current_zone = alt_zones::TAKE_OFF;
c_error_ = 0;
c_integrator_ = 0;
r_error_ = 0;
r_integrator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
at_error_ = 0;
at_integrator_ = 0;
ap_error_ = 0;
ap_integrator_ = 0;
}
void pseudo_controller_example::control(const params_s &params, const input_s &input, output_s &output)
{
output.up_down_trans = 0;
output.forward_trans = 0;
output.rotate_trans = 0;
output.left_right_trans = 0;
output.rotate_value=pseudo_course(input.chi, input.chi_c);
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
if (input.landing==true) // protect-landing first
{
current_zone = alt_zones::LANDING;
}
if (input.takeoff==true && current_zone==alt_zones::LANDING)
{
current_zone = alt_zones::PREPARE;
}
switch (current_zone)
{
case alt_zones::PREPARE:
output.up_down_trans = 0.5;
output.forward_trans = 0;
output.left_right_trans = 0;
output.rotate_trans=0;
if (input.h >= 1) //
{
ROS_DEBUG("TAKEOFF");
current_zone = alt_zones::TAKE_OFF;
}
break;
case alt_zones::TAKE_OFF:
//output.rotate_trans = 1.5*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);//recover
//output.forward_trans = 0.7;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.forward_trans = input.Va_c;//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.up_down_trans = pow(1.1,input.h)*pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
//output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
//if (input.h >= params.alt_toz) //alt_toz=1.5//recovery
//output.left_right_trans = 1.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
if (input.h >= 1.5) //
{
ROS_DEBUG("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::CLIMB:
output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
//output.forward_trans = 0.8;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
//output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
//output.up_down_trans = 0.01 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
//output.rotate_trans = 0.3*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
////if (input.h >= input.h_c - params.alt_hz) //alt_hz=0.2//recovery
if (input.h >= input.h_c - 0.3) //alt_hz=0.2
{
ROS_DEBUG("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
//else if (input.h <= params.alt_toz) //1.5//recovery
// else if (input.h <= 1) //1.5
// {
// ROS_DEBUG("takeoff");
// current_zone = alt_zones::TAKE_OFF; // Is that necessary?
// }
break;
case alt_zones::DESCEND:
output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
//output.forward_trans = 0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
//output.left_right_trans = 0.1 * pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
//output.rotate_trans = pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
//output.up_down_trans = 0.1 * pseudo_pitch_hold(input.h_c, input.h, params, input.Ts);
//if (input.h <= input.h_c + params.alt_hz)//recovery
if (input.h <= input.h_c + 0.3)
{
if (input.h < 0) //modify later
{
ROS_DEBUG("landing");
current_zone = alt_zones::LANDING;
// output.forward_trans = sat(input.Va_c*cos(output.rotate_trans),1,0);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts) * 0.5;
// output.up_down_trans = pseudo_pitch_hold(input.h_c, input.h, params, input.Ts) / 2;
// output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
}
else
{
ROS_DEBUG("hold");
current_zone = alt_zones::ALTITUDE_HOLD;
at_error_ = 0;
at_integrator_ = 0;
at_differentiator_ = 0;
a_error_ = 0;
a_integrator_ = 0;
a_differentiator_ = 0;
}
}
break;
case alt_zones::ALTITUDE_HOLD:
output.forward_trans = input.Va_c;//sat(input.Va_c*cos(output.rotate_trans),5,0.5);//pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
output.left_right_trans = 0;//sat(input.Va_c*sin(output.rotate_trans),5,-5);//0.5*pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
output.up_down_trans = pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
output.rotate_trans = output.rotate_value;//pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
//output.forward_trans = 1;//0.1 * pseudo_throttle_hold(input.Va_c, input.va, params, input.Ts);
//output.left_right_trans = pseudo_left_right(input.chi, input.chi_c, output.forward_trans);
//output.up_down_trans = 0.1 * pseudo_altitiude_hold(input.h_c, input.h, params, input.Ts);
//output.rotate_trans = 0.7*pseudo_course_hold(input.chi, input.chi_c, params, input.Ts);
//if (input.h >= input.h_c + params.alt_hz)//recovery
if (input.h >= input.h_c + 0.3)
{
ROS_DEBUG("desend");
current_zone = alt_zones::DESCEND;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
//else if (input.h <= input.h_c - params.alt_hz)//recovery
else if (input.h <= input.h_c - 0.3)
{
ROS_DEBUG("climb");
current_zone = alt_zones::CLIMB;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
}
break;
case alt_zones::LANDING:
ROS_DEBUG("landing");
current_zone = alt_zones::LANDING;
ap_error_ = 0;
ap_integrator_ = 0;
ap_differentiator_ = 0;
output.forward_trans = 0;
if (input.h < 0.6)
{
output.up_down_trans = 0;
}
else
{
output.up_down_trans = -input.h/3;
}
output.rotate_trans =0;
output.left_right_trans = 0;
break;
default:
break;
}
output.current_zone = current_zone;
//output.delta_e = pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
//output.up_down_trans=pseudo_pitch_hold(output.theta_c, input.theta, input.q, params, input.Ts);
}
float pseudo_controller_example::pseudo_course(float chi, float chi_c)
{
chi_c=sat(chi_c,3.14159,-3.14159);
chi=sat(chi,3.14159,-3.14159);
float rotate = sat(chi_c-chi,2*3.14159,-2*3.14159);//-pi to pi
// float lr;
// if (rotate>3.14159)
// {
// lr=-1;
// }
// else if(rotate<-3.14159)
// {
// lr=1;
// }
//rotate=rotate+lr*2*3.14159;//more easier to rotate
// rotate=sat(rotate,3.14159/4,-3.14159/4);//limit
return rotate;
}
float pseudo_controller_example::pseudo_left_right(float chi, float chi_c, float forward_trans)
{
float error= chi_c-chi;
float a = fabs(forward_trans);
float x = (3.14 / 4 > fabs(error)) ? fabs(error) : 3.14 / 4;
float b = tan(x);
float c = error / (fabs(error + 0.001));
float left_right_trans = sat(a * b * c, forward_trans, -forward_trans);
return left_right_trans;
}
float pseudo_controller_example::pseudo_course_hold(float chi, float chi_c, const params_s &params, float Ts)
{
float error = chi_c-chi;
//c_integrator_ = c_integrator_ + (Ts / 2.0) * (error + c_error_);
//float up = params.c_kp * error;
//float ui = params.c_ki * c_integrator_;
float rotate_trans = sat((error + c_error_) / 2, 45.0 * 3.14 / 180.0, -45.0 * 3.14 / 180.0);
//if (fabs(params.c_ki) >= 0.00001)
//{
//float rotate_trans_unsat = up + ui;
//c_integrator_ = c_integrator_ + (Ts / params.c_ki) * (rotate_trans - rotate_trans_unsat);
//}
c_error_ = error;
return rotate_trans;
}
float pseudo_controller_example::pseudo_pitch_hold(float h_c, float h, const struct params_s &params, float Ts)
{
float error = h_c - h;
//ap_integrator_ = ap_integrator_ + (Ts / 2.0) * (error + ap_error_);
// float up = 0.5 * error;
//float ui = params.a_p_ki * ap_integrator_;
//float up_down_trans = sat(up + ui, 0.8, -0.8);//recovery
float up_down_trans = sat(0.5 * error, 2, -2);
//if (fabs(params.a_p_ki) >= 0.00001)
//{
//float up_down_trans_unsat = up + ui;
//ap_integrator_ = ap_integrator_ + (Ts / params.a_p_ki) * (up_down_trans - up_down_trans_unsat);
//}
// ap_error_ = error;
return up_down_trans;
}
float pseudo_controller_example::pseudo_throttle_hold(float Va_c, float Va, const params_s &params, float Ts)
{
float error = Va_c - Va;
//at_integrator_ = at_integrator_ + (Ts / 2.0) * (error + at_error_);
//at_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * at_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) *(error - at_error_);
float up = params.a_t_kp * error;
//float ui = params.a_t_ki * at_integrator_;
//float ud = params.a_t_kd * at_differentiator_;
//float forward_trans = sat(Va + up + ui + ud, params.max_t, 0.1);
float forward_trans = sat(Va+error, params.max_t, 0.5);
//if (fabs(params.a_t_ki) >= 0.00001)
//{
//float forward_trans_unsat = params.trim_t + up + ui + ud;
//at_integrator_ = at_integrator_ + (Ts / params.a_t_ki) * (forward_trans - forward_trans_unsat);
//}
//at_error_ = error;
return forward_trans;
}
float pseudo_controller_example::pseudo_altitiude_hold(float h_c, float h, const params_s &params, float Ts)
{
float error = h_c - h;
//a_integrator_ = a_integrator_ + (Ts / 2.0) * (error + a_error_);
//a_differentiator_ = (2.0 * params.tau - Ts) / (2.0 * params.tau + Ts) * a_differentiator_ + (2.0 /(2.0 * params.tau + Ts)) * (error - a_error_);
float up = 0.7 * error;
//float up = params.a_kp * error;
//ROS_INFO("hello %f",params.a_kp);
//float ui = params.a_ki * a_integrator_;
//float ud = params.a_kd * a_differentiator_;
//float up_down_trans = sat(up + ui + ud, 0.8, -0.8);//recovery
float up_down_trans = sat(up, 2, -2);
//if (fabs(params.a_ki) >= 0.00001)
//{
//float up_down_trans_unsat = up + ui + ud;
//a_integrator_ = a_integrator_ + (Ts / params.a_ki) * (up_down_trans - up_down_trans_unsat);
//}
// at_error_ = error;
return up_down_trans;
}
//float pseudo_controller_example::cooridinated_turn_hold(float v, const params_s &params, float Ts)
//{
// //todo finish this if you want...
// return 0;
//}
float pseudo_controller_example::sat(float value, float up_limit, float low_limit)
{
float rVal;
if (value > up_limit)
rVal = up_limit;
else if (value < low_limit)
rVal = low_limit;
else
rVal = value;
return rVal;
}
} // namespace hector

29
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/simple_takeoff.cpp Normal file
View File

@ -0,0 +1,29 @@
#include <ros/ros.h>
#include <geometry_msgs/Twist.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_simple_takeoff");
ros::NodeHandle nh_;
ros::Publisher velPublisher = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10);
geometry_msgs::Twist cmd_vel;
cmd_vel.linear.x = 0;
cmd_vel.linear.y = 0;
cmd_vel.linear.z = 0.5;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;
cmd_vel.angular.z = 0;
while (ros::ok())
{
velPublisher.publish(cmd_vel);
ros::Duration(0.01).sleep();
}
ros::spin();
return 0;
}

254
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/udp_recv_image.cpp Normal file
View File

@ -0,0 +1,254 @@
/************************************************************************
* *
* Author: ake *
* Date: 2018-10-23 *
* Description: recv image of the camera to server with udp *
* *
*************************************************************************/
#include "udp_recv_image.h"
namespace hector
{
udp_recv_image::udp_recv_image():nh_(ros::NodeHandle()),it(nh_)
{
image_pub = it.advertise("udp_image",1);
memset(&my_addr, 0, sizeof(my_addr));
my_addr.sin_family = AF_INET;
my_addr.sin_addr.s_addr = INADDR_ANY;
my_addr.sin_port = htons(8888);
sin_size = sizeof(struct sockaddr_in);
if((server_sockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
{
perror("socket error");
return;
}
if(bind(server_sockfd, (struct sockaddr *)&my_addr, sizeof(struct sockaddr)) < 0)
{
perror("bind error");
return;
}
struct timeval tv_out;
tv_out.tv_sec = 3;
tv_out.tv_usec = 0;
setsockopt(server_sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv_out,sizeof(tv_out));
int sockbufsize = 1024*1024*10;
setsockopt(server_sockfd, SOL_SOCKET, SO_RCVBUF, &sockbufsize, sizeof(sockbufsize));
}
udp_recv_image::~udp_recv_image()
{
}
void udp_recv_image::udp_close()
{
close(server_sockfd);
}
void udp_recv_image::recv_image()
{
sensor_msgs::ImagePtr msg;
struct RecvStatus recvS;
timespec t1,t2;
int nCount = 0;
bool bFlag = false;
char bufS[20];
// int num = 1;
int count = 0;
unsigned int size = 0;
char tmpBuf[1024*1024];
cv::Mat image_recv;
std::vector<uchar> buf;
std::vector<int> params;
std::string comprType = ".jpg";
int quality = 100;
int compr = cv::IMWRITE_JPEG_QUALITY;
params.push_back(compr);
params.push_back(quality);
clock_gettime(CLOCK_MONOTONIC, &t1);
while(ros::ok())
{
// std::cout<<"-----------------------------------------pos 1------------------------------------------\n";
memset(frameBuffer,0,BUF_SIZE);
int len = recvfrom(server_sockfd, frameBuffer, sizeof(PackageHeader)+UDP_MAX_SIZE, 0, (struct sockaddr *)&remote_addr, &sin_size);
// std::cout<<"-----------------------------------------pos 2------------------------------------------\n";
// std::cout<<"len = "<<len<<std::endl;
if(len > 0)
{
// std::cout<<"-----------------------------------------pos 3------------------------------------------\n";
bFlag = true;
packageHead = (PackageHeader*)frameBuffer;
if(packageHead->uDataPackageCurrIndex == 1)
{
count = 0;
size = 0;
bzero(tmpBuf, packageHead->uDataSize);
}
count++;
size += packageHead->uTransPackageSize-packageHead->uTransPackageHdrSize;
// std::cout<<"1111111111111111111111111111111111111111111111111111\n";
memcpy(tmpBuf+packageHead->uDataPackageOffset, frameBuffer + packageHead->uTransPackageHdrSize,packageHead->uTransPackageSize-packageHead->uTransPackageHdrSize);
// std::cout<<"2222222222222222222222222222222222222222222222222222\n";
if(count == packageHead->uDataPackageNum)
{
if(size == packageHead->uDataSize)
{
buf.resize(packageHead->uDataSize);
buf.assign(tmpBuf, tmpBuf+packageHead->uDataSize);
cv::imdecode(cv::Mat(buf), cv::IMREAD_COLOR, &image_recv);
msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_recv).toImageMsg();
image_pub.publish(msg);
nCount++;
// usleep(50);
// cv::imshow("test subscribe", cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image);
// cv::waitKey(30);
clock_gettime(CLOCK_MONOTONIC, &t2);
std::cout<<"time: "<<t2.tv_sec - t1.tv_sec<<"\t count = "<<nCount<<std::endl;
}
}
recvS.index = packageHead->uDataPackageCurrIndex;
recvS.flag = 1;
bzero(bufS, 20);
memcpy(bufS, (char*)&recvS,sizeof(recvS));
sendto(server_sockfd,bufS, sizeof(recvS), 0, (struct sockaddr *)&remote_addr, sin_size);
}
else
{
// std::cout<<"3333333333333333333333333333333333333333333333333\n";
// std::cout<<"-----------------------------------------pos 4------------------------------------------\n";
if(bFlag)
{
recvS.index = packageHead->uDataPackageCurrIndex;
recvS.flag = -1;
bzero(bufS, 20);
memcpy(bufS, (char*)&recvS,sizeof(recvS));
sendto(server_sockfd,bufS, sizeof(recvS), 0, (struct sockaddr *)&remote_addr, sin_size);
}
}
// if(len > 0)
// {
// // memcpy(&packageHead, frameBuffer, sizeof(struct PackageHeader));
// packageHead = (PackageHeader*)frameBuffer;
// // int *flag = new int[packageHead->uDataPackageNum];
// // for(int j = 0; j < packageHead->uDataPackageNum; j++)
// // {
// // flag[j] = 0;
// // }
// // std::cout<<"-------------1 start-----------------------\n";
// // std::cout<<"packageHead.uDataPackageCurrIndex = "<<packageHead->uDataPackageCurrIndex<<std::endl;
// // std::cout<<"-------------1 end-------------------------\n";
// if(packageHead->uDataPackageCurrIndex == num)
// {
// // std::cout<<"-------------2 start-----------------------\n";
// // std::cout<<"packageHead.uDataPackageCurrIndex = "<<packageHead->uDataPackageCurrIndex<<std::endl;
// // std::cout<<"-------------2 end-------------------------\n";
// // std::cout<<"-------------3 start-----------------------\n";
// // std::cout<<"packageHead.uDataPackageNum = "<<packageHead->uDataPackageNum<<std::endl;
// // std::cout<<"-------------3 end-------------------------\n";
// // cv::Mat image_recv;
// // switch(packageHead->channels)
// // {
// // case 1:
// // image_recv = cv::Mat(packageHead->rows,packageHead->cols,CV_8UC1);
// // break;
// // case 2:
// // image_recv = cv::Mat(packageHead->rows, packageHead->cols,CV_8UC2);
// // break;
// // case 3:
// // image_recv = cv::Mat(packageHead->rows, packageHead->cols,CV_8UC3);
// // break;
// // case 4:
// // image_recv = cv::Mat(packageHead->rows, packageHead->cols,CV_8UC4);
// // break;
// // }
// while(num <= packageHead->uDataPackageNum)
// {
// if(num == packageHead->uDataPackageCurrIndex)
// {
// size += packageHead->uTransPackageSize-packageHead->uTransPackageHdrSize;
// if(size > packageHead->uDataSize)
// {
// perror("error image data, too big.");
// size = 0;
// num = 1;
// break;
// }
// if(packageHead->uDataPackageOffset > packageHead->uDataSize)
// {
// perror("error image data, format error.");
// size = 0;
// num = 1;
// break;
// }
// // std::cout<<"size = "<<size<<"\tpackageHead.uDataSize= "<<packageHead->uDataSize<<std::endl;
// // std::cout<<"***************start*******************\n";
// std::cout<<"packageHead.uDataPackageCurrIndex = "<<packageHead->uDataPackageCurrIndex<<std::endl;
// memcpy(image_recv.data+packageHead->uDataPackageOffset, frameBuffer+packageHead->uTransPackageHdrSize,packageHead->uTransPackageSize-packageHead->uTransPackageHdrSize);
// // std::cout<<"***************end*******************\n";
// // std::cout<<"num ="<<num<<"\tpackageHead->uDataPackageNum="<<packageHead->uDataPackageNum<<"\tpackageHead->uDataPackageCurrIndex="<<packageHead->uDataPackageCurrIndex<<std::endl;
// recvS.index = num;
// recvS.flag = 1;
// sendto(server_sockfd,(char*)&recvS, sizeof(recvS), 0, (struct sockaddr *)&remote_addr, sin_size);
// if((packageHead->uDataPackageNum == packageHead->uDataPackageCurrIndex))
// {
// // std::cout<<"packageHead.uDataPackageNum= "<<packageHead->uDataPackageNum<<std::endl;
// if((size == packageHead->uDataSize))
// {
// msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_recv).toImageMsg();
// image_pub.publish(msg);
// usleep(50);
// cv::imshow("test subscribe", cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image);
// cv::waitKey(30);
// }
// num = 1;
// size = 0;
// break;
// }
// num++;
// }
// memset(frameBuffer,0,BUFSIZE);
// len = recvfrom(server_sockfd, frameBuffer, sizeof(PackageHeader)+UDP_MAX_SIZE, 0, (struct sockaddr *)&remote_addr, &sin_size);
// if(len < 0 )
// {
// recvS.index = num;
// recvS.flag = -1;
// sendto(server_sockfd,(char*)&recvS, sizeof(recvS), 0, (struct sockaddr *)&remote_addr, sin_size);
// num = 1;
// size = 0;
// break;
// }
// }
// num = 1;
// size = 0;
// }
// }
}
}
}
int main(int argc, char **argv)
{
ros::init(argc,argv,"udp_recv_image");
hector::udp_recv_image image_recv;
image_recv.recv_image();
image_recv.udp_close();
return 0;
}

323
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/udp_send_image.cpp Normal file
View File

@ -0,0 +1,323 @@
/************************************************************************
* *
* Author: ake *
* Date: 2018-10-23 *
* Description: send image of the camera to server with udp *
* *
************************************************************************/
#include "udp_send_image.h"
namespace hector
{
udp_send_image::udp_send_image():
nh_(ros::NodeHandle()),
it(nh_)
{
std::string server_ip, sub_topic;
int server_port;
ros::param::get("~server_ip",server_ip);
ros::param::get("~server_port",server_port);
ros::param::get("~sub_topic",sub_topic);
std::cout<<"server_ip: "<<server_ip<<std::endl;
std::cout<<"sub_topic: "<<sub_topic<<std::endl;
if(server_ip.empty())
{
server_ip = "192.168.1.5";
}
if(sub_topic.empty())
{
sub_topic = "/fixedwing_0/downward_cam/camera/image";
}
if(server_port == 0)
{
server_port = 8888;
}
image_sub = it.subscribe(sub_topic ,10, &udp_send_image::imageCallback,this);
memset(&remote_addr, 0, sizeof(remote_addr));
remote_addr.sin_family = AF_INET;
remote_addr.sin_addr.s_addr = inet_addr(server_ip.c_str());
remote_addr.sin_port = htons(server_port);
if((client_sockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
{
perror("socket error");
}
struct timeval tv_out;
tv_out.tv_sec = 3;
tv_out.tv_usec = 0;
setsockopt(client_sockfd, SOL_SOCKET, SO_RCVTIMEO, &tv_out,sizeof(tv_out));
int sockbufsize = 1024*1024*10;
setsockopt(client_sockfd, SOL_SOCKET, SO_RCVBUF, &sockbufsize, sizeof(sockbufsize));
}
udp_send_image::~udp_send_image()
{
}
void udp_send_image::udp_close()
{
close(client_sockfd);
}
void udp_send_image::imageCallback(const sensor_msgs::ImageConstPtr& tem_msg)
{
// try
// {
// // cv::imshow("test subscribe", cv_bridge::toCvShare(tem_msg,"mono8")->image);
// cv::imshow("test subscribe", cv_bridge::toCvCopy(tem_msg, sensor_msgs::image_encodings::BGR8)->image);
// cv::waitKey(30);
// }
// catch(cv_bridge::Exception& e)
// {
// ROS_ERROR("Could not convert from '%s' to 'BGR8'", tem_msg->encoding.c_str());
// }
struct RecvStatus recvS;
unsigned int dstLength;
int currentPacketIndex = 0;
int nLength = 0;
int dataLength = 0;
int packetNum = 0;
int lastPaketSize = 0;
cv::Mat img_send = cv_bridge::toCvCopy(tem_msg, sensor_msgs::image_encodings::BGR8)->image;
//compress the img
std::vector<uchar> buf;
std::vector<int> params;
std::string comprType = ".jpg";
int quality = 100;
int compr = cv::IMWRITE_JPEG_QUALITY;
params.push_back(compr);
params.push_back(quality);
cv::imencode(comprType.c_str(), img_send,buf,params);
// std::cout<<"buf.size() = "<<buf.size()<<std::endl;
// cv::Mat compressed;
// cv::imdecode(cv::Mat(buf), cv::IMREAD_COLOR,&compressed);
// cv::imshow("test compressed", compressed);
// cv::waitKey(30);
unsigned char* tmpBuf = new unsigned char[buf.size()];
for(int i = 0; i < buf.size(); i++)
{
tmpBuf[i] = buf[i];
}
dataLength = buf.size();
packetNum = dataLength/UDP_MAX_SIZE;
lastPaketSize = dataLength % UDP_MAX_SIZE;
if(lastPaketSize != 0)
{
packetNum = packetNum+1;
}
packageHead.uTransPackageHdrSize = sizeof(packageHead);
packageHead.uDataSize = dataLength;
packageHead.uDataPackageNum = packetNum;
bzero(frameBuffer,BUF_SIZE);
while(currentPacketIndex < packetNum)
{
if(currentPacketIndex < packetNum - 1)
{
packageHead.uTransPackageSize = sizeof(PackageHeader) + UDP_MAX_SIZE;
packageHead.uDataPackageCurrIndex = currentPacketIndex + 1;
packageHead.uDataPackageOffset = currentPacketIndex * UDP_MAX_SIZE;
memcpy(frameBuffer, &packageHead, sizeof(PackageHeader));
memcpy(frameBuffer+sizeof(PackageHeader), tmpBuf+packageHead.uDataPackageOffset, UDP_MAX_SIZE);
nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// std::cout<<"nLength = "<<nLength<<std::endl;
usleep(50);
// if(nLength != packageHead.uTransPackageSize)
// {
// printf("Send image failed, try again.");
// nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// usleep(200);
// if(nLength != packageHead.uTransPackageSize)
// {
// perror("Send image failed!");
// }
// }
// nLength = recvfrom(client_sockfd, (char*)&recvS,sizeof(recvS), 0, (struct sockaddr *)&remote_addr, &dstLength);
// if(recvS.index == currentPacketIndex++)
// {
// if(recvS.flag == 1)
// {
// currentPacketIndex++;
// }
// else if(recvS.flag == -1)
// {
// currentPacketIndex = 0;
// }
// }
}
else
{
packageHead.uTransPackageSize = sizeof(PackageHeader) + (dataLength - currentPacketIndex*UDP_MAX_SIZE);
packageHead.uDataPackageCurrIndex = currentPacketIndex + 1;
packageHead.uDataPackageOffset = currentPacketIndex * UDP_MAX_SIZE;
memcpy(frameBuffer, &packageHead, sizeof(PackageHeader));
memcpy(frameBuffer+sizeof(PackageHeader),tmpBuf+packageHead.uDataPackageOffset, dataLength - currentPacketIndex*UDP_MAX_SIZE);
// std::cout<<"packageHead.uTransPackageSize = " <<packageHead.uTransPackageSize<<std::endl;
int nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// std::cout<<"nLength = "<<nLength<<std::endl;
usleep(50);
// if(nLength != packageHead.uTransPackageSize)
// {
// printf("Send image failed, try again.");
// nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// usleep(200);
// if(nLength != packageHead.uTransPackageSize)
// {
// perror("Send image failed!");
// }
// }
}
// currentPacketIndex++;
nLength = recvfrom(client_sockfd, (char*)&recvS,sizeof(recvS), 0, (struct sockaddr *)&remote_addr, &dstLength);
// std::cout<<"currentPacketIndex = "<<currentPacketIndex<<std::endl;
if(nLength <= 0)
{
break;
}
if(recvS.index == currentPacketIndex+1)
{
if(recvS.flag == 1)
{
currentPacketIndex++;
}
else if(recvS.flag == -1)
{
currentPacketIndex = 0;
break;
}
}
}
delete []tmpBuf;
// int dataLength = img_send.dataend - img_send.datastart;
// // std::cout<<"cols = "<<img_send.cols<<"\trows = "<<img_send.rows<<"\tchannels = "<<img_send.channels()<<std::endl;
// unsigned char *dataBuffer = (unsigned char*)img_send.data;
// int packetNum = 0;
// int lastPaketSize = 0;
// int nLength;
// packetNum = dataLength/UDP_MAX_SIZE;
// lastPaketSize = dataLength % UDP_MAX_SIZE;
// int currentPacketIndex = 0;
// if(lastPaketSize != 0)
// {
// packageHead.uLastPaketSize = lastPaketSize;
// packetNum = packetNum + 1;
// }
// else
// {
// packageHead.uLastPaketSize = UDP_MAX_SIZE;
// }
// packageHead.uTransPackageHdrSize = sizeof(packageHead);
// packageHead.uDataSize = dataLength;
// packageHead.uDataPackageNum = packetNum;
// packageHead.cols = img_send.cols;
// packageHead.rows = img_send.rows;
// packageHead.channels = img_send.channels();
// memset(frameBuffer, 0, BUFSIZE);
// while(currentPacketIndex < packetNum)
// {
// if(currentPacketIndex < packetNum - 1)
// {
// packageHead.uTransPackageSize = sizeof(PackageHeader) + UDP_MAX_SIZE;
// packageHead.uDataPackageCurrIndex = currentPacketIndex + 1;
// packageHead.uDataPackageOffset = currentPacketIndex * UDP_MAX_SIZE;
// memcpy(frameBuffer, &packageHead, sizeof(PackageHeader));
// memcpy(frameBuffer+sizeof(PackageHeader), dataBuffer+packageHead.uDataPackageOffset, UDP_MAX_SIZE);
// nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// usleep(50);
// // if(nLength != packageHead.uTransPackageSize)
// // {
// // printf("Send image failed, try again.");
// // nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// // usleep(200);
// // if(nLength != packageHead.uTransPackageSize)
// // {
// // perror("Send image failed!");
// // }
// // }
// // nLength = recvfrom(client_sockfd, (char*)&recvS,sizeof(recvS), 0, (struct sockaddr *)&remote_addr, &dstLength);
// // if(recvS.index == currentPacketIndex++)
// // {
// // if(recvS.flag == 1)
// // {
// // currentPacketIndex++;
// // }
// // else if(recvS.flag == -1)
// // {
// // currentPacketIndex = 0;
// // }
// // }
// }
// else
// {
// packageHead.uTransPackageSize = sizeof(PackageHeader) + (dataLength - currentPacketIndex*UDP_MAX_SIZE);
// packageHead.uDataPackageCurrIndex = currentPacketIndex + 1;
// packageHead.uDataPackageOffset = currentPacketIndex * UDP_MAX_SIZE;
// memcpy(frameBuffer, &packageHead, sizeof(PackageHeader));
// memcpy(frameBuffer+sizeof(PackageHeader),dataBuffer+packageHead.uDataPackageOffset, dataLength - currentPacketIndex*UDP_MAX_SIZE);
// int nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// usleep(50);
// // if(nLength != packageHead.uTransPackageSize)
// // {
// // printf("Send image failed, try again.");
// // nLength = sendto(client_sockfd, (const char*)frameBuffer, packageHead.uTransPackageSize, 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
// // usleep(200);
// // if(nLength != packageHead.uTransPackageSize)
// // {
// // perror("Send image failed!");
// // }
// // }
// }
// nLength = recvfrom(client_sockfd, (char*)&recvS,sizeof(recvS), 0, (struct sockaddr *)&remote_addr, &dstLength);
// if(recvS.index == currentPacketIndex+1)
// {
// if(recvS.flag == 1)
// {
// currentPacketIndex++;
// }
// else if(recvS.flag == -1)
// {
// currentPacketIndex = 0;
// break;
// }
// }
// }
// std::cout<<"packetNum = " << packetNum <<std::endl;
// std::cout<<"currentPacketIndex="<<currentPacketIndex<<std::endl;
}
} //end namespace
// void Stop(int signo)
// {
// exit(1);
// }
int main(int argc,char **argv)
{
// signal(SIGINT, Stop);
ros::init(argc,argv,"udp_send_image");
hector::udp_send_image image_send;
ros::spin();
/*关闭套接字*/
image_send.udp_close();
return 0;
}

241
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector/src/udp_send_state.cpp Normal file
View File

@ -0,0 +1,241 @@
/*******************************
*
* This UDP Client is finished by YAN JIE. .
*
**************************/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "rosplane_msgs/Udp_Send_State.h"
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
//串口相关的头文件
#include <stdio.h> /*标准输入输出定义*/
#include <stdlib.h> /*标准函数库定义*/
#include <unistd.h> /*Unix 标准函数定义*/
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h> /*文件控制定义*/
#include <termios.h> /*PPSIX 终端控制定义*/
#include <errno.h> /*错误号定义*/
#include <string.h>
#include <math.h>
#include <boost/thread/thread.hpp>
//宏定义
#define FALSE -1
#define TRUE 0
#define TAIL 0xFFFA55AF
#define REC_BUFFER_SIZE 200
#define DOWN_CHECK_LEN 24
#define UP_CHECK_LEN 68
//#define BUFSIZ 200
// typedef struct
// {
// double plane_info_double[6]; //存放经度、纬度、高度、滚转角、俯仰角、偏航角
// int plane_info_int; //存放无人机编号
// float plane_info_float[9]; //航迹速度（目前没有用）
// unsigned char sum_check[4]; //累加校验码（目前没有用）
// unsigned char header[4]; //（目前没有用）
// } Scom_up_protocal;
typedef struct
{
double plane_info_double[2]; //存放无人机经度、纬度、高度、滚转角、俯仰角、偏航角
double camera_info_double[2]; //存放相机经度、纬度
float plane_info_float[7]; //存放无人机高度、滚转角、俯仰角、偏航角、北东地三方向速度
float camera_info_float[4]; //存放相机高度、滚转角、俯仰角、偏航角
unsigned int ID; //存放无人机编号
char name[20];
} Scom_up_protocal;
// struct scom_protocal
// {
// int plane_info_int[3]; //存放经度、纬度、高度
// float plane_info_float; //航迹速度
// };
namespace hector
{
using std::string;
/** @brief async write buffer */
class Udp_commu_send
{
public:
Udp_commu_send()
{
std::string client_ip;
int client_port = 0;
ros::param::get("~client_ip", client_ip);
ros::param::get("~client_port", client_port);
if(client_ip.empty())
{
client_ip = "192.168.1.15";
}
if(client_port == 0)
{
client_port = 8000;
}
ROS_ERROR("port is: %d\n", client_port);
memset(&remote_addr, 0, sizeof(remote_addr)); //数据初始化--清零
remote_addr.sin_family = AF_INET; //设置为IP通信
remote_addr.sin_addr.s_addr = inet_addr(client_ip.c_str()); //服务器IP地址"172.18.0.3"
remote_addr.sin_port = htons(client_port); //服务器端口号
// remote_addr.sin_port = htons(8000);
/*创建客户端套接字--IPv4协议，面向无连接通信，UDP协议*/
if ((client_sockfd = socket(PF_INET, SOCK_DGRAM, 0)) < 0)
{
perror("socket error");
}
sin_size = sizeof(struct sockaddr_in);
task_send_data = (Scom_up_protocal *)malloc(sizeof(Scom_up_protocal));
// sub = n.subscribe("/fixedwing_0/udp_send_state", 1, &Udp_commu_send::upsendCallback, this);
sub = n.subscribe("udp_send_state", 10, &Udp_commu_send::upsendCallback, this);
//write_thread_ = new boost::thread(boost::bind(&Serial_commu_send::write_handle, this));
//write_thread_ -> detach();
};
~Udp_commu_send()
{
free(task_send_data);
delete write_thread_;
};
void write_handle()
{
unsigned char temp = num;
sleep(0.1);
if ((num == temp) && flag == 1)
tcflush(fd, TCOFLUSH);
};
void upsendCallback(const rosplane_msgs::Udp_Send_State::ConstPtr &msg)
{
// printf("*************************The data to send is:\n");
// ROS_INFO("I heard: msg.status_word is: %d\n", msg->status_word);
// ROS_INFO("I heard: msg.longitude is %f\n", msg->longitude);
// ROS_INFO("I heard: msg.latitude is %f\n", msg->latitude);
// ROS_INFO("I heard: msg.altitude is: %f\n", msg->altitude);
// ROS_INFO("I heard: msg.roll_angle is: %f\n", msg->roll_angle);
double i = 50;
//***************串口发送数据 *************
// unsigned char scom_protocal::serial_num_increase = 0;
// task_send_data->plane_info_int[0] = msg->longitude * (int)(pow(10, 7));
// task_send_data->plane_info_int[1] = msg->latitude * (int)(pow(10, 7));
// task_send_data->plane_info_int[2] = msg->altitude * (int)(pow(10, 2));
// task_send_data->plane_info_int = msg->status_word;
// task_send_data->plane_info_int = 0;
// task_send_data->plane_info_double[0] = msg->longitude;
// task_send_data->plane_info_double[1] = msg->latitude;
// task_send_data->plane_info_double[2] = msg->altitude;
// task_send_data->plane_info_double[3] = msg->roll_angle;
// task_send_data->plane_info_double[4] = msg->pitch_angle;
// task_send_data->plane_info_double[5] = msg->yaw_angle;
// task_send_data->plane_info_float[0] = msg->angular_rate_x;
// task_send_data->plane_info_float[1] = msg->angular_rate_y;
// task_send_data->plane_info_float[2] = msg->angular_rate_z;
// task_send_data->plane_info_float[3] = msg->north_direction_speed;
// task_send_data->plane_info_float[4] = msg->east_direction_speed;
// task_send_data->plane_info_float[5] = msg->ground_direction_speed;
// task_send_data->plane_info_float[6] = msg->acceleration_x;
// task_send_data->plane_info_float[7] = msg->acceleration_y;
// task_send_data->plane_info_float[8] = msg->acceleration_z;
task_send_data->ID = 0;
task_send_data->plane_info_double[0] = msg->longitude;
task_send_data->plane_info_double[1] = msg->latitude;
task_send_data->plane_info_float[0] = msg->altitude;
task_send_data->plane_info_float[1] = msg->roll_angle * 180.0 / M_PI;
task_send_data->plane_info_float[2] = msg->pitch_angle * 180.0 / M_PI;
task_send_data->plane_info_float[3] = msg->yaw_angle * 180.0 / M_PI;
task_send_data->plane_info_float[4] = msg->north_direction_speed;
task_send_data->plane_info_float[5] = msg->east_direction_speed;
task_send_data->plane_info_float[6] = msg->ground_direction_speed;
task_send_data->camera_info_double[0] = msg->longitude_camera;
task_send_data->camera_info_double[1] = msg->latitude_camera;
task_send_data->camera_info_float[0] = msg->altitude_camera;
task_send_data->camera_info_float[1] = msg->roll_angle_camera * 180.0 / M_PI;
task_send_data->camera_info_float[2] = msg->pitch_angle_camera * 180.0 / M_PI;
task_send_data->camera_info_float[3] = msg->yaw_angle_camera * 180.0 / M_PI;
// task_send_data->name = msg->name;
// strcpy(task_send_data->name, msg->name);
// strcpy(task_send_data->name, "fixedwing_0");
// 向char[20]赋值
int j;
for (j = 0; j < msg->name.length(); j++)
{
task_send_data->name[j] = msg->name[j];
}
task_send_data->name[j] = '\0';
// printf("%s\n", p);
// cout << p;
// printf("UDP_send..........................................................\n");
/*向服务器发送数据包*/
int len = sendto(client_sockfd, (char *)(&task_send_data->plane_info_double[0]), sizeof(Scom_up_protocal), 0, (struct sockaddr *)&remote_addr, sizeof(struct sockaddr));
sleep(0.001);
// int len = UART0_Send((char *)(&task_send_data->header[0]), sizeof(scom_protocal));
// printf("UDP_send over..........................................................\n");
// if (len > 0)
// printf(" %f ms time send %d byte data send successfully\n", i, len);
// else
// printf("send data failed!\n");
};
int get_fd()
{
return fd;
};
void udp_Close()
{
close(client_sockfd);
}
private:
Scom_up_protocal *task_send_data;
int fd;
unsigned char num;
int flag;
boost::thread *write_thread_;
ros::NodeHandle n;
ros::Subscriber sub;
int client_sockfd;
struct sockaddr_in remote_addr; //服务器端网络地址结构体
int sin_size;
char buf[BUFSIZ]; //数据传送的缓冲区
};
} // namespace hector
int main(int argc, char **argv)
{
ros::init(argc, argv, "udp_send_0");
// int err;
hector::Udp_commu_send send;
ros::spin();
/*关闭套接字*/
send.udp_Close();
return 0;
}

30
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/CMakeLists.txt Normal file
View File

@ -0,0 +1,30 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_msgs)
find_package(catkin REQUIRED COMPONENTS
std_msgs
message_generation
)
add_message_files(
FILES
Controller_Commands.msg
Current_Path.msg
Goal_Info.msg
Controller_Internals.msg
State.msg
State29.msg
Up_Data_New.msg
Down_Data_New.msg
Formation_Type.msg
Waypoint.msg
)
generate_messages(
DEPENDENCIES
std_msgs
)
catkin_package(
CATKIN_DEPENDS std_msgs
)

11
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Controller_Commands.msg Normal file
View File

@ -0,0 +1,11 @@
# Controller commands output from the path follower, input to autopilot controller
# @warning Va_c, h_c and chi_c have always to be valid, the aux array is optional
float32 Va_c # Commanded airspeed (m/s)
float32 h_c # Commanded altitude (m)
float32 chi_c # Commanded course (rad)
float32 phi_ff # feed forward command for orbits (rad)
float32[4] aux # Optional auxiliary commands
bool aux_valid # Auxiliary commands valid
bool takeoff
bool landing

13
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Controller_Internals.msg Normal file
View File

@ -0,0 +1,13 @@
# Controller internals (inner loops, states) output from the autopilot controller, for now just for debuging and ploting
# @warning theta_c, phi_c and alt_zone have always to be valid, the aux array is optional
float32 theta_c # Commanded pitch (rad)
float32 phi_c # Commanded roll (rad)
uint8 alt_zone # Zone in the altitude state machine
float32[4] aux # Optional auxiliary commands
bool aux_valid # Auxiliary commands valid
uint8 ZONE_TAKE_OFF = 0
uint8 ZONE_CLIMB = 1
uint8 ZONE_DESEND = 2
uint8 ZONE_ALTITUDE_HOLD = 3

23
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Current_Path.msg Normal file
View File

@ -0,0 +1,23 @@
# Current path output from the path manager, input to path follower
# @warning Va_d have always to be valid,
# r and q need to be valid if path_type == LINE_PATH
# c, rho, and, lambda need to be valid if path_type == ORBIT_PATH
uint8 path_type # Indicates strait line or orbital path
float32 Va_d # Desired airspeed (m/s)
float32[3] r # Vector to origin of straight line path (m)
float32[3] q # Unit vector, desired direction of travel for line path
float32[3] c # Center of orbital path (m)
float32 rho # Radius of orbital path (m)
float32 h_c # desire altitude (m) add by kobe
int8 lambda # Direction of orbital path (clockwise is 1, counterclockwise is -1)
uint8 ORBIT_PATH = 0
uint8 LINE_PATH = 1
uint8 STAR_PATH = 2 #add by kobe
int8 CLOCKWISE = 1
int8 COUNT_CLOCKWISE = -1
bool landing
bool takeoff

5
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Down_Data_New.msg Normal file
View File

@ -0,0 +1,5 @@
int32 status_word
float32 longitude
float32 latitude
float32 altitude
float32 plane_speed #desired speed

7
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Formation_Type.msg Normal file
View File

@ -0,0 +1,7 @@
# Vehicle state 'x_hat' output from the estimator or from simulator
Header header
# Original States
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
float32 formation_type # Airspeed (m/s)

8
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Goal_Info.msg Normal file
View File

@ -0,0 +1,8 @@
# Current path goal output from the path manager, add by kobe
float32 v_d # Desired airspeed (m/s)
float32[2] xy # x-y-coordinate (m)
float32[2] ll # latitude-longtitude
int32 action # expected action move
float32 altitude # desire altitude (m)

37
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/State.msg Normal file
View File

@ -0,0 +1,37 @@
# Vehicle state 'x_hat' output from the estimator or from simulator
Header header
# Original States
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
float32[3] position # north, east, down (m)
float32 Va # Airspeed (m/s)
float32 alpha # Angle of attack (rad)
float32 beta # Slide slip angle (rad)
float32 phi # Roll angle (rad)
float32 theta # Pitch angle (rad)
float32 psi # Yaw angle (rad)
float32 chi # Course angle (rad)
float32 p # Body frame rollrate (rad/s)
float32 q # Body frame pitchrate (rad/s)
float32 r # Body frame yawrate (rad/s)
float32 Vg # Groundspeed (m/s)
float32 wn # Windspeed north component (m/s)
float32 we # Windspeed east component (m/s)
# add by kobe
float32 Wd # latitude -90--90
float32 Jd # longtitude -180--180
float32 Vn # v-north speed or x
float32 Ve # v-east or y
float32 Vd # v-down or z
# Additional States for convenience
float32[4] quat # Quaternion (wxyz, NED)
bool quat_valid # Quaternion valid
float32 chi_deg # Wrapped course angle (deg)
float32 psi_deg # Wrapped yaw angle (deg)
float32 initial_lat # Initial/origin latitude (lat. deg)
float32 initial_lon # Initial/origin longitude (lon. deg)
float32 initial_alt # Initial/origin altitude (m)

13
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/State29.msg Normal file
View File

@ -0,0 +1,13 @@
# Vehicle state 'x_hat' output from the estimator or from simulator
Header header
# Original States
# @warning roll, pitch and yaw have always to be valid, the quaternion is optional
float32[3] position_gps # latitude (lat. deg), longitude (lon. deg), altitude (m)
float32[3] attitude_angle # Roll angle (rad), Pitch angle (rad), Yaw angle (rad)
float32[3] velocity # V_north (m/s), V_east (m/s), V_ground (m/s)
float32[3] angular_velocity # Body frame rollrate (rad/s), Body frame pitchrate (rad/s), Body frame yawrate (rad/s)
float32[3] acceleration # Body frame x (m/s^2), Body frame y (m/s^2), Body frame z (m/s^2)
float32 electric_quantity # Quantity of electric charge
int8 state_word # Control Status Word

16
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Up_Data_New.msg Normal file
View File

@ -0,0 +1,16 @@
int32 status_word
float32 longitude
float32 latitude
float32 altitude # (m)
float32 roll_angle #(rad)
float32 pitch_angle
float32 yaw_angle
float32 angular_rate_x #Body frame rollrate (rad/s)
float32 angular_rate_y #Body frame pitchrate (rad/s)
float32 angular_rate_z #Body frame yawrate (rad/s)
float32 north_direction_speed #(m/s)
float32 east_direction_speed
float32 ground_direction_speed
float32 acceleration_x #(m/s^2)
float32 acceleration_y
float32 acceleration_z

15
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Waypoint (复件).msg Normal file
View File

@ -0,0 +1,15 @@
# New waypoint, input to path manager
# @warning w and Va_d always have to be valid; the chi_d is optional.
float32[3] w # Waypoint in local NED (m)
# add by kobe
float32 lat # latitude
float32 lon # longtitude
float32 chi_d # Desired course at this waypoint (rad)
bool chi_valid # Desired course valid (dubin or fillet paths)
float32 Va_d # Desired airspeed (m/s)
bool set_current # Sets this waypoint to be executed now! Starts a new list
bool clear_wp_list # Removes all waypoints and returns to origin. The rest of
# this message will be ignored

17
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/msg/Waypoint.msg Normal file
View File

@ -0,0 +1,17 @@
# New waypoint, input to path manager
# @warning w and Va_d always have to be valid; the chi_d is optional.
float32[3] w # Waypoint in local NED (m)
# add by kobe
float32 lat # latitude
float32 lon # longtitude
float32 chi_d # Desired course at this waypoint (rad)
bool chi_valid # Desired course valid (dubin or fillet paths)
float32 Va_d # Desired airspeed (m/s)
bool set_current # Sets this waypoint to be executed now! Starts a new list
bool clear_wp_list # Removes all waypoints and returns to origin. The rest of
# this message will be ignored
bool landing
bool takeoff

25
Gazebo_Hector_Test/src/hector_quadrotor/Guide_on_rosplane/hector_msgs/package.xml Normal file
View File

@ -0,0 +1,25 @@
<?xml version="1.0"?>
<package>
<name>hector_msgs</name>
<version>0.0.0</version>
<description>Message definitions for the hector package</description>
<maintainer email="gary.ellingson@byu.edu">Gary Ellingson</maintainer>
<author email="gary.ellingson@byu.edu">Gary Ellingson</author>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>std_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<export>
</export>
</package>

4
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor.rosinstall Normal file
View File

@ -0,0 +1,4 @@
- git: {local-name: hector_quadrotor, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor.git', version: indigo-devel}
- git: {local-name: hector_localization, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_localization.git', version: catkin}
- git: {local-name: hector_gazebo, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_gazebo.git', version: indigo-devel}
- git: {local-name: hector_models, uri: 'https://github.com/tu-darmstadt-ros-pkg/hector_models.git', version: indigo-devel}

26
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor/CHANGELOG.rst Normal file
View File

@ -0,0 +1,26 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
0.3.3 (2014-09-01)
------------------
* moved simulation package dependencies from hector_quadrotor metapackage to hector_quadrotor_gazebo
* Contributors: Johannes Meyer
0.3.2 (2014-03-30)
------------------
* added packages hector_quadrotor_controller and hector_quadrotor_controller_gazebo to meta-package
* Contributors: Johannes Meyer
0.3.1 (2013-12-26)
------------------
0.3.0 (2013-09-11)
------------------
* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack

4
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor)
find_package(catkin REQUIRED)
catkin_metapackage()

30
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor/package.xml Normal file
View File

@ -0,0 +1,30 @@
<package>
<name>hector_quadrotor</name>
<version>0.3.5</version>
<description>hector_quadrotor contains packages related to modeling, control and simulation of quadrotor UAV systems</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor</url>
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<!-- Dependencies needed after this package is compiled. -->
<run_depend>hector_quadrotor_controller</run_depend>
<run_depend>hector_quadrotor_description</run_depend>
<run_depend>hector_quadrotor_model</run_depend>
<run_depend>hector_quadrotor_teleop</run_depend>
<run_depend>hector_uav_msgs</run_depend>
<!-- Dependencies needed only for running tests. -->
</package>

46
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/CHANGELOG.rst Normal file
View File

@ -0,0 +1,46 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_controller
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
* updated angular/z controller parameters
* Remove redundant callback queue in twist_controller, use root_nh queue as per ros_control API
* Add controller timeout to allow faster shutdown of spawner
* Contributors: Johannes Meyer, Paul Bovbel
0.3.4 (2015-02-22)
------------------
* improved automatic landing detection and shutdown on rollovers
* slightly updated velocity controller limits and gains
* Contributors: Johannes Meyer
0.3.3 (2014-09-01)
------------------
* fixed some compiler warnings and missing return values
* increased integral gain for attitude stabilization (fix #12)
* make a copy of the root NodeHandle in all controllers
For some reason deconstructing the TwistController resulted in a pure virtual function call without this patch.
* Contributors: Johannes Meyer
0.3.2 (2014-03-30)
------------------
* Fix boost 1.53 issues
changed boost::shared_dynamic_cast to boost::dynamic_pointer_cast and
boost::shared_static_cast to boost::static_pointer_cast
* use a separate callback queue thread for the TwistController
* added optional twist limit in pose controller
* Contributors: Christopher Hrabia, Johannes Meyer
0.3.1 (2013-12-26)
------------------
* New controller implementation using ros_control
* Added pose controller
* Added motor controller that controls motor voltages from wrench commands
* Contributors: Johannes Meyer
0.3.0 (2013-09-11)
------------------
* Catkinized stack hector_quadrotor and integrated hector_quadrotor_demo package from former hector_quadrotor_apps stack
* Propulsion and aerodynamics models are in hector_quadrotor_model package now.
* Gazebo plugins are in hector_quadrotor_gazebo_plugins package now.

78
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/CMakeLists.txt Normal file
View File

@ -0,0 +1,78 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor_controller)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface)
include_directories(include ${catkin_INCLUDE_DIRS})
## System dependencies are found with CMake's conventions
find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES hector_quadrotor_controller
CATKIN_DEPENDS roscpp geometry_msgs sensor_msgs nav_msgs hector_uav_msgs std_srvs hardware_interface controller_interface
DEPENDS
)
###########
## Build ##
###########
add_library(hector_quadrotor_controller src/quadrotor_interface.cpp src/pid.cpp)
target_link_libraries(hector_quadrotor_controller ${catkin_LIBRARIES})
add_dependencies(hector_quadrotor_controller hector_uav_msgs_generate_messages_cpp)
add_library(hector_quadrotor_pose_controller src/pose_controller.cpp)
target_link_libraries(hector_quadrotor_pose_controller hector_quadrotor_controller)
add_library(hector_quadrotor_twist_controller src/twist_controller.cpp)
target_link_libraries(hector_quadrotor_twist_controller hector_quadrotor_controller ${BOOST_LIBRARIES})
add_library(hector_quadrotor_motor_controller src/motor_controller.cpp)
target_link_libraries(hector_quadrotor_motor_controller hector_quadrotor_controller)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executables and/or libraries for installation
install(TARGETS hector_quadrotor_controller hector_quadrotor_pose_controller hector_quadrotor_twist_controller hector_quadrotor_motor_controller
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(DIRECTORY launch params DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(FILES
plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

536
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/handles.h Normal file
View File

@ -0,0 +1,536 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
#define HECTOR_QUADROTOR_CONTROLLER_HANDLES_H
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/Wrench.h>
#include <sensor_msgs/Imu.h>
#include <hector_uav_msgs/MotorStatus.h>
#include <hector_uav_msgs/MotorCommand.h>
#include <hector_uav_msgs/AttitudeCommand.h>
#include <hector_uav_msgs/YawrateCommand.h>
#include <hector_uav_msgs/ThrustCommand.h>
namespace hector_quadrotor_controller {
class QuadrotorInterface;
using geometry_msgs::Pose;
using geometry_msgs::Point;
using geometry_msgs::Quaternion;
using geometry_msgs::Twist;
using geometry_msgs::Vector3;
using geometry_msgs::Wrench;
using sensor_msgs::Imu;
using hector_uav_msgs::MotorStatus;
using hector_uav_msgs::MotorCommand;
using hector_uav_msgs::AttitudeCommand;
using hector_uav_msgs::YawrateCommand;
using hector_uav_msgs::ThrustCommand;
template <class Derived, typename T>
class Handle_
{
public:
typedef T ValueType;
typedef Handle_<Derived, T> Base;
Handle_(const std::string& name, const std::string& field = std::string()) : interface_(0), name_(name), field_(field), value_(0) {}
Handle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field), value_(0) {}
Handle_(QuadrotorInterface *interface, const ValueType *source, const std::string& name, const std::string& field = std::string()) : interface_(interface), name_(name), field_(field) { *this = source; }
virtual ~Handle_() {}
virtual const std::string& getName() const { return name_; }
virtual const std::string& getField() const { return field_; }
virtual bool connected() const { return get(); }
virtual void reset() { value_ = 0; }
Derived& operator=(const ValueType *source) { value_ = source; return static_cast<Derived &>(*this); }
const ValueType *get() const { return value_; }
const ValueType &operator*() const { return *value_; }
protected:
QuadrotorInterface *interface_;
const std::string name_;
const std::string field_;
const ValueType *value_;
};
class PoseHandle : public Handle_<PoseHandle, Pose>
{
public:
using Base::operator=;
PoseHandle() : Base("pose") {}
PoseHandle(QuadrotorInterface *interface) : Base(interface, "pose") {}
PoseHandle(QuadrotorInterface *interface, const Pose *pose) : Base(interface, pose, "pose") {}
virtual ~PoseHandle() {}
const ValueType& pose() const { return *get(); }
void getEulerRPY(double &roll, double &pitch, double &yaw) const;
double getYaw() const;
Vector3 toBody(const Vector3& nav) const;
Vector3 fromBody(const Vector3& nav) const;
};
typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
class TwistHandle : public Handle_<TwistHandle, Twist>
{
public:
using Base::operator=;
TwistHandle() : Base("twist") {}
TwistHandle(QuadrotorInterface *interface) : Base(interface, "twist") {}
TwistHandle(QuadrotorInterface *interface, const Twist *twist) : Base(interface, twist, "twist") {}
virtual ~TwistHandle() {}
const ValueType& twist() const { return *get(); }
};
typedef boost::shared_ptr<TwistHandle> TwistHandlePtr;
class AccelerationHandle : public Handle_<AccelerationHandle, Vector3>
{
public:
using Base::operator=;
AccelerationHandle(QuadrotorInterface *interface, const Vector3 *acceleration) : Base(interface, acceleration, "acceleration") {}
virtual ~AccelerationHandle() {}
const ValueType& acceleration() const { return *get(); }
};
typedef boost::shared_ptr<AccelerationHandle> AccelerationHandlePtr;
class StateHandle : public PoseHandle, public TwistHandle
{
public:
StateHandle() {}
StateHandle(QuadrotorInterface *interface, const Pose *pose, const Twist *twist) : PoseHandle(interface, pose), TwistHandle(interface, twist) {}
virtual ~StateHandle() {}
virtual bool connected() const { return PoseHandle::connected() && TwistHandle::connected(); }
};
typedef boost::shared_ptr<PoseHandle> PoseHandlePtr;
class ImuHandle : public Handle_<ImuHandle, Imu>
{
public:
using Base::operator=;
ImuHandle() : Base("imu") {}
ImuHandle(QuadrotorInterface *interface, const Imu *imu) : Base(interface, imu, "imu") {}
virtual ~ImuHandle() {}
const ValueType& imu() const { return *get(); }
};
typedef boost::shared_ptr<ImuHandle> ImuHandlePtr;
class MotorStatusHandle : public Handle_<MotorStatusHandle, MotorStatus>
{
public:
using Base::operator=;
MotorStatusHandle() : Base("motor_status") {}
MotorStatusHandle(QuadrotorInterface *interface, const MotorStatus *motor_status) : Base(interface, motor_status, "motor_status") {}
virtual ~MotorStatusHandle() {}
const ValueType& motorStatus() const { return *get(); }
};
typedef boost::shared_ptr<MotorStatusHandle> MotorStatusHandlePtr;
class CommandHandle
{
public:
CommandHandle() : interface_(0), new_value_(false) {}
CommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field) : interface_(interface), name_(name), field_(field), new_value_(false) {}
virtual ~CommandHandle() {}
virtual const std::string& getName() const { return name_; }
virtual const std::string& getField() const { return field_; }
virtual bool connected() const = 0;
virtual void reset() {}
void *get() const { return 0; }
bool enabled();
bool start();
void stop();
void disconnect();
template <typename T> T* ownData(T* data) { my_.reset(data); return data; }
template <typename Derived> bool connectFrom(const Derived& output) {
Derived *me = dynamic_cast<Derived *>(this);
if (!me) return false;
ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", output.getName().c_str(), &output, me->getName().c_str(), me);
return (*me = output.get()).connected();
}
template <typename Derived> bool connectTo(Derived& input) const {
const Derived *me = dynamic_cast<const Derived *>(this);
if (!me) return false;
ROS_DEBUG("Connected output port '%s (%p)' to input port '%s (%p)'", me->getName().c_str(), me, input.getName().c_str(), &input);
return (input = me->get()).connected();
}
private:
QuadrotorInterface *interface_;
const std::string name_;
const std::string field_;
boost::shared_ptr<void> my_;
protected:
mutable bool new_value_;
bool wasNew() const { bool old = new_value_; new_value_ = false; return old; }
};
typedef boost::shared_ptr<CommandHandle> CommandHandlePtr;
namespace internal {
template <class Derived> struct FieldAccessor {
static typename Derived::ValueType *get(void *) { return 0; }
};
}
template <class Derived, typename T, class Parent = CommandHandle>
class CommandHandle_ : public Parent
{
public:
typedef T ValueType;
typedef CommandHandle_<Derived, T, Parent> Base;
CommandHandle_() : command_(0) {}
CommandHandle_(const Parent &other) : Parent(other), command_(0) {}
CommandHandle_(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Parent(interface, name, field), command_(0) {}
virtual ~CommandHandle_() {}
virtual bool connected() const { return get(); }
virtual void reset() { command_ = 0; Parent::reset(); }
using Parent::operator=;
Derived& operator=(ValueType *source) { command_ = source; return static_cast<Derived &>(*this); }
ValueType *get() const { return command_ ? command_ : internal::FieldAccessor<Derived>::get(Parent::get()); }
ValueType &operator*() const { return *command_; }
ValueType& command() { return *get(); }
const ValueType& getCommand() const { this->new_value_ = false; return *get(); }
void setCommand(const ValueType& command) { this->new_value_ = true; *get() = command; }
bool getCommand(ValueType& command) const { command = *get(); return this->wasNew(); }
bool update(ValueType& command) const {
if (!connected()) return false;
command = getCommand();
return true;
}
protected:
ValueType *command_;
};
class PoseCommandHandle : public CommandHandle_<PoseCommandHandle, Pose>
{
public:
using Base::operator=;
PoseCommandHandle() {}
PoseCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
PoseCommandHandle(Pose* command) { *this = command; }
virtual ~PoseCommandHandle() {}
};
typedef boost::shared_ptr<PoseCommandHandle> PoseCommandHandlePtr;
class HorizontalPositionCommandHandle : public CommandHandle_<HorizontalPositionCommandHandle, Point, PoseCommandHandle>
{
public:
using Base::operator=;
HorizontalPositionCommandHandle() {}
HorizontalPositionCommandHandle(const PoseCommandHandle& other) : Base(other) {}
HorizontalPositionCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.xy") {}
HorizontalPositionCommandHandle(Point* command) { *this = command; }
virtual ~HorizontalPositionCommandHandle() {}
using Base::getCommand;
virtual bool getCommand(double &x, double &y) const {
x = get()->x;
y = get()->y;
return wasNew();
}
using Base::setCommand;
virtual void setCommand(double x, double y)
{
this->new_value_ = true;
get()->x = x;
get()->y = y;
}
using Base::update;
bool update(Pose& command) const {
if (!connected()) return false;
getCommand(command.position.x, command.position.y);
return true;
}
void getError(const PoseHandle &pose, double &x, double &y) const;
};
typedef boost::shared_ptr<HorizontalPositionCommandHandle> HorizontalPositionCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HorizontalPositionCommandHandle> {
static Point *get(Pose *pose) { return &(pose->position); }
};
}
class HeightCommandHandle : public CommandHandle_<HeightCommandHandle, double, PoseCommandHandle>
{
public:
using Base::operator=;
HeightCommandHandle() {}
HeightCommandHandle(const PoseCommandHandle& other) : Base(other) {}
HeightCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "position.z") {}
HeightCommandHandle(double *command) { *this = command; }
virtual ~HeightCommandHandle() {}
using Base::update;
bool update(Pose& command) const {
if (!connected()) return false;
command.position.z = getCommand();
return true;
}
double getError(const PoseHandle &pose) const;
};
typedef boost::shared_ptr<HeightCommandHandle> HeightCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HeightCommandHandle> {
static double *get(Pose *pose) { return &(pose->position.z); }
};
}
class HeadingCommandHandle : public CommandHandle_<HeadingCommandHandle, Quaternion, PoseCommandHandle>
{
public:
using Base::operator=;
HeadingCommandHandle() {}
HeadingCommandHandle(const PoseCommandHandle& other) : Base(other), scalar_(0) {}
HeadingCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "orientation.yaw"), scalar_(0) {}
HeadingCommandHandle(Quaternion *command) { *this = command; }
virtual ~HeadingCommandHandle() {}
using Base::getCommand;
double getCommand() const;
using Base::setCommand;
void setCommand(double command);
using Base::update;
bool update(Pose& command) const;
double getError(const PoseHandle &pose) const;
protected:
double *scalar_;
};
typedef boost::shared_ptr<HeadingCommandHandle> HeadingCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HeadingCommandHandle> {
static Quaternion *get(Pose *pose) { return &(pose->orientation); }
};
}
class TwistCommandHandle : public CommandHandle_<TwistCommandHandle, Twist>
{
public:
using Base::operator=;
TwistCommandHandle() {}
TwistCommandHandle(QuadrotorInterface *interface, const std::string& name, const std::string& field = std::string()) : Base(interface, name, field) {}
TwistCommandHandle(Twist* command) { *this = command; }
virtual ~TwistCommandHandle() {}
};
typedef boost::shared_ptr<TwistCommandHandle> TwistCommandHandlePtr;
class HorizontalVelocityCommandHandle : public CommandHandle_<HorizontalVelocityCommandHandle, Vector3, TwistCommandHandle>
{
public:
using Base::operator=;
HorizontalVelocityCommandHandle() {}
HorizontalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
HorizontalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.xy") {}
HorizontalVelocityCommandHandle(Vector3* command) { *this = command; }
virtual ~HorizontalVelocityCommandHandle() {}
using Base::getCommand;
bool getCommand(double &x, double &y) const {
x = get()->x;
y = get()->y;
return true;
}
using Base::setCommand;
void setCommand(double x, double y)
{
get()->x = x;
get()->y = y;
}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
getCommand(command.linear.x, command.linear.y);
return true;
}
};
typedef boost::shared_ptr<HorizontalVelocityCommandHandle> HorizontalVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<HorizontalVelocityCommandHandle> {
static Vector3 *get(Twist *twist) { return &(twist->linear); }
};
}
class VerticalVelocityCommandHandle : public CommandHandle_<VerticalVelocityCommandHandle, double, TwistCommandHandle>
{
public:
using Base::operator=;
VerticalVelocityCommandHandle() {}
VerticalVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
VerticalVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "linear.z") {}
VerticalVelocityCommandHandle(double* command) { *this = command; }
virtual ~VerticalVelocityCommandHandle() {}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
command.linear.z = *get();
return true;
}
};
typedef boost::shared_ptr<VerticalVelocityCommandHandle> VerticalVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<VerticalVelocityCommandHandle> {
static double *get(Twist *twist) { return &(twist->linear.z); }
};
}
class AngularVelocityCommandHandle : public CommandHandle_<AngularVelocityCommandHandle, double, TwistCommandHandle>
{
public:
using Base::operator=;
AngularVelocityCommandHandle() {}
AngularVelocityCommandHandle(const TwistCommandHandle& other) : Base(other) {}
AngularVelocityCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name, "angular.z") {}
AngularVelocityCommandHandle(double* command) { *this = command; }
virtual ~AngularVelocityCommandHandle() {}
using Base::update;
bool update(Twist& command) const {
if (!connected()) return false;
command.linear.z = *get();
return true;
}
};
typedef boost::shared_ptr<AngularVelocityCommandHandle> AngularVelocityCommandHandlePtr;
namespace internal {
template <> struct FieldAccessor<AngularVelocityCommandHandle> {
static double *get(Twist *twist) { return &(twist->angular.z); }
};
}
class WrenchCommandHandle : public CommandHandle_<WrenchCommandHandle, Wrench>
{
public:
using Base::operator=;
WrenchCommandHandle() {}
WrenchCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~WrenchCommandHandle() {}
};
typedef boost::shared_ptr<WrenchCommandHandle> WrenchCommandHandlePtr;
class MotorCommandHandle : public CommandHandle_<MotorCommandHandle, MotorCommand>
{
public:
using Base::operator=;
MotorCommandHandle() {}
MotorCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~MotorCommandHandle() {}
};
typedef boost::shared_ptr<MotorCommandHandle> MotorCommandHandlePtr;
class AttitudeCommandHandle : public CommandHandle_<AttitudeCommandHandle, AttitudeCommand>
{
public:
using Base::operator=;
AttitudeCommandHandle() {}
AttitudeCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~AttitudeCommandHandle() {}
};
typedef boost::shared_ptr<AttitudeCommandHandle> AttitudeCommandHandlePtr;
class YawrateCommandHandle : public CommandHandle_<YawrateCommandHandle, YawrateCommand>
{
public:
using Base::operator=;
YawrateCommandHandle() {}
YawrateCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~YawrateCommandHandle() {}
};
typedef boost::shared_ptr<YawrateCommandHandle> YawrateCommandHandlePtr;
class ThrustCommandHandle : public CommandHandle_<ThrustCommandHandle, ThrustCommand>
{
public:
using Base::operator=;
ThrustCommandHandle() {}
ThrustCommandHandle(QuadrotorInterface *interface, const std::string& name) : Base(interface, name) {}
virtual ~ThrustCommandHandle() {}
};
typedef boost::shared_ptr<ThrustCommandHandle> ThrustCommandHandlePtr;
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_HANDLES_H

45
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/pid.h Normal file
View File

@ -0,0 +1,45 @@
#ifndef HECTOR_QUADROTOR_CONTROLLER_PID_H
#define HECTOR_QUADROTOR_CONTROLLER_PID_H
#include <ros/node_handle.h>
namespace hector_quadrotor_controller {
class PID
{
public:
struct parameters {
parameters();
bool enabled;
double time_constant;
double k_p;
double k_i;
double k_d;
double limit_i;
double limit_output;
} parameters_;
struct state {
state();
double p, i, d;
double input, dinput;
double dx;
} state_;
public:
PID();
PID(const parameters& parameters);
~PID();
void init(const ros::NodeHandle &param_nh);
void reset();
double update(double input, double x, double dx, const ros::Duration& dt);
double update(double error, double dx, const ros::Duration& dt);
double getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt);
};
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_PID_H

143
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/include/hector_quadrotor_controller/quadrotor_interface.h Normal file
View File

@ -0,0 +1,143 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H
#include <hardware_interface/internal/hardware_resource_manager.h>
#include <hector_quadrotor_controller/handles.h>
#include <map>
#include <list>
namespace hector_quadrotor_controller {
using namespace hardware_interface;
class QuadrotorInterface : public HardwareInterface
{
public:
QuadrotorInterface();
virtual ~QuadrotorInterface();
virtual PoseHandlePtr getPose() { return PoseHandlePtr(); }
virtual TwistHandlePtr getTwist() { return TwistHandlePtr(); }
virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(); }
virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(); }
virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(); }
virtual bool getMassAndInertia(double &mass, double inertia[3]) { return false; }
template <typename HandleType> boost::shared_ptr<HandleType> getHandle()
{
return boost::shared_ptr<HandleType>(new HandleType(this));
}
template <typename HandleType> boost::shared_ptr<HandleType> addInput(const std::string& name)
{
boost::shared_ptr<HandleType> input = getInput<HandleType>(name);
if (input) return input;
// create new input handle
input.reset(new HandleType(this, name));
inputs_[name] = input;
// connect to output of same name
if (outputs_.count(name)) {
boost::shared_ptr<HandleType> output = boost::dynamic_pointer_cast<HandleType>(outputs_.at(name));
output->connectTo(*input);
}
return input;
}
template <typename HandleType> boost::shared_ptr<HandleType> addOutput(const std::string& name)
{
boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
if (output) return output;
// create new output handle
output.reset(new HandleType(this, name));
outputs_[name] = output;
*output = output->ownData(new typename HandleType::ValueType());
//claim resource
claim(name);
// connect to output of same name
if (inputs_.count(name)) {
boost::shared_ptr<HandleType> input = boost::dynamic_pointer_cast<HandleType>(inputs_.at(name));
output->connectTo(*input);
}
return output;
}
template <typename HandleType> boost::shared_ptr<HandleType> getOutput(const std::string& name) const
{
if (!outputs_.count(name)) return boost::shared_ptr<HandleType>();
return boost::static_pointer_cast<HandleType>(outputs_.at(name));
}
template <typename HandleType> boost::shared_ptr<HandleType> getInput(const std::string& name) const
{
if (!inputs_.count(name)) return boost::shared_ptr<HandleType>();
return boost::static_pointer_cast<HandleType>(inputs_.at(name));
}
template <typename HandleType> typename HandleType::ValueType const* getCommand(const std::string& name) const
{
boost::shared_ptr<HandleType> output = getOutput<HandleType>(name);
if (!output || !output->connected()) return 0;
return &(output->command());
}
virtual const Pose *getPoseCommand() const;
virtual const Twist *getTwistCommand() const;
virtual const Wrench *getWrenchCommand() const;
virtual const MotorCommand *getMotorCommand() const;
bool enabled(const CommandHandle *handle) const;
bool start(const CommandHandle *handle);
void stop(const CommandHandle *handle);
void disconnect(const CommandHandle *handle);
private:
// typedef std::list< CommandHandlePtr > HandleList;
// std::map<const std::type_info *, HandleList> inputs_;
// std::map<const std::type_info *, HandleList> outputs_;
std::map<std::string, CommandHandlePtr> inputs_;
std::map<std::string, CommandHandlePtr> outputs_;
std::map<std::string, const CommandHandle *> enabled_;
};
} // namespace hector_quadrotor_controller
#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_INTERFACE_H

7
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/launch/controller.launch Normal file
View File

@ -0,0 +1,7 @@
<launch>
<rosparam file="$(find hector_quadrotor_controller)/params/controller.yaml" />
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="
controller/twist
--shutdown-timeout 3"/>
</launch>

42
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/package.xml Normal file
View File

@ -0,0 +1,42 @@
<package>
<name>hector_quadrotor_controller</name>
<version>0.3.5</version>
<description>hector_quadrotor_controller provides libraries and a node for quadrotor control using <a href="http://wiki.ros.org/ros_control">ros_control</a>.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_controller</url>
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
<!-- Dependencies which this package needs to build itself. -->
<buildtool_depend>catkin</buildtool_depend>
<!-- Dependencies needed to compile this package. -->
<build_depend>roscpp</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>hector_uav_msgs</build_depend>
<build_depend>std_srvs</build_depend>
<build_depend>hardware_interface</build_depend>
<build_depend>controller_interface</build_depend>
<!-- Dependencies needed after this package is compiled. -->
<run_depend>roscpp</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>hector_uav_msgs</run_depend>
<run_depend>std_srvs</run_depend>
<run_depend>hardware_interface</run_depend>
<run_depend>controller_interface</run_depend>
<!-- Dependencies needed only for running tests. -->
<export>
<controller_interface plugin="${prefix}/plugin.xml"/>
</export>
</package>

50
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/params/controller.yaml Normal file
View File

@ -0,0 +1,50 @@
controller:
pose:
type: hector_quadrotor_controller/PoseController
xy:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 5.0
z:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 5.0
yaw:
k_p: 2.0
k_i: 0.0
k_d: 0.0
limit_output: 1.0
twist:
type: hector_quadrotor_controller/TwistController
linear/xy:
k_p: 5.0
k_i: 1.0
k_d: 0.0
limit_output: 10.0
time_constant: 0.05
linear/z:
k_p: 5.0
k_i: 1.0
k_d: 0.0
limit_output: 10.0
time_constant: 0.05
angular/xy:
k_p: 10.0
k_i: 5.0
k_d: 5.0
time_constant: 0.01
angular/z:
k_p: 5.0
k_i: 2.5
k_d: 0.0
limit_output: 3.0
time_constant: 0.1
limits:
load_factor: 1.5
force/z: 30.0
torque/xy: 10.0
torque/z: 1.0
motor:
type: hector_quadrotor_controller/MotorController

23
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/plugin.xml Normal file
View File

@ -0,0 +1,23 @@
<class_libraries>
<library path="lib/libhector_quadrotor_pose_controller">
<class name="hector_quadrotor_controller/PoseController" type="hector_quadrotor_controller::PoseController" base_class_type="controller_interface::ControllerBase">
<description>
The PoseController controls the quadrotor's pose and outputs velocity commands.
</description>
</class>
</library>
<library path="lib/libhector_quadrotor_twist_controller">
<class name="hector_quadrotor_controller/TwistController" type="hector_quadrotor_controller::TwistController" base_class_type="controller_interface::ControllerBase">
<description>
The TwistController controls the quadrotor's twist (linear and angular velocity) by applying torque and thrust similar to a real quadrotor.
</description>
</class>
</library>
<library path="lib/libhector_quadrotor_motor_controller">
<class name="hector_quadrotor_controller/MotorController" type="hector_quadrotor_controller::MotorController" base_class_type="controller_interface::ControllerBase">
<description>
The MotorController calculates the output voltages of the four motors from the commanded thrust and torque using a simple linearized model.
</description>
</class>
</library>
</class_libraries>

186
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/motor_controller.cpp Normal file
View File

@ -0,0 +1,186 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class MotorController : public controller_interface::Controller<QuadrotorInterface>
{
public:
MotorController()
: node_handle_(0)
{}
~MotorController()
{
if (node_handle_) {
node_handle_->shutdown();
delete node_handle_;
node_handle_ = 0;
}
}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
wrench_input_ = interface->addInput<WrenchCommandHandle>("wrench");
motor_output_ = interface->addOutput<MotorCommandHandle>("motor");
// initialize NodeHandle
delete node_handle_;
node_handle_ = new ros::NodeHandle(root_nh);
// load parameters
controller_nh.getParam("force_per_voltage", parameters_.force_per_voltage = 0.559966216);
controller_nh.getParam("torque_per_voltage", parameters_.torque_per_voltage = 7.98598e-3);
controller_nh.getParam("lever", parameters_.lever = 0.275);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// TODO: calculate these parameters from the quadrotor_propulsion parameters
// quadrotor_propulsion:
// k_t: 0.015336864714397
// k_m: -7.011631909766668e-5
// CT2s: -1.3077e-2
// CT1s: -2.5224e-4
// CT0s: 1.538190483976698e-5
return true;
}
void reset()
{
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
motor_.force.assign(4, 0.0);
motor_.torque.assign(4, 0.0);
motor_.frequency.clear();
motor_.voltage.assign(4, 0.0);
}
void wrenchCommandCallback(const geometry_msgs::WrenchStampedConstPtr& command)
{
wrench_ = *command;
if (wrench_.header.stamp.isZero()) wrench_.header.stamp = ros::Time::now();
// start controller if it not running
if (!isRunning()) this->startRequest(wrench_.header.stamp);
}
void starting(const ros::Time &time)
{
reset();
motor_output_->start();
}
void stopping(const ros::Time &time)
{
motor_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
// Get wrench command input
if (wrench_input_->connected() && wrench_input_->enabled()) {
wrench_.wrench = wrench_input_->getCommand();
}
// Update output
if (wrench_.wrench.force.z > 0.0) {
double nominal_thrust_per_motor = wrench_.wrench.force.z / 4.0;
motor_.force[0] = nominal_thrust_per_motor - wrench_.wrench.torque.y / 2.0 / parameters_.lever;
motor_.force[1] = nominal_thrust_per_motor - wrench_.wrench.torque.x / 2.0 / parameters_.lever;
motor_.force[2] = nominal_thrust_per_motor + wrench_.wrench.torque.y / 2.0 / parameters_.lever;
motor_.force[3] = nominal_thrust_per_motor + wrench_.wrench.torque.x / 2.0 / parameters_.lever;
double nominal_torque_per_motor = wrench_.wrench.torque.z / 4.0;
motor_.voltage[0] = motor_.force[0] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[1] = motor_.force[1] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[2] = motor_.force[2] / parameters_.force_per_voltage + nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.voltage[3] = motor_.force[3] / parameters_.force_per_voltage - nominal_torque_per_motor / parameters_.torque_per_voltage;
motor_.torque[0] = motor_.voltage[0] * parameters_.torque_per_voltage;
motor_.torque[1] = motor_.voltage[1] * parameters_.torque_per_voltage;
motor_.torque[2] = motor_.voltage[2] * parameters_.torque_per_voltage;
motor_.torque[3] = motor_.voltage[3] * parameters_.torque_per_voltage;
if (motor_.voltage[0] < 0.0) motor_.voltage[0] = 0.0;
if (motor_.voltage[1] < 0.0) motor_.voltage[1] = 0.0;
if (motor_.voltage[2] < 0.0) motor_.voltage[2] = 0.0;
if (motor_.voltage[3] < 0.0) motor_.voltage[3] = 0.0;
} else {
reset();
}
// set wrench output
motor_.header.stamp = time;
motor_.header.frame_id = "base_link";
motor_output_->setCommand(motor_);
}
private:
WrenchCommandHandlePtr wrench_input_;
MotorCommandHandlePtr motor_output_;
ros::NodeHandle *node_handle_;
ros::Subscriber wrench_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::WrenchStamped wrench_;
hector_uav_msgs::MotorCommand motor_;
std::string base_link_frame_;
struct {
double force_per_voltage; // coefficient for linearized volts to force conversion for a single motor [N / V]
double torque_per_voltage; // coefficient for linearized volts to force conversion for a single motor [Nm / V]
double lever; // the lever arm from origin to the motor axes (symmetry assumption) [m]
} parameters_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::MotorController, controller_interface::ControllerBase)

152
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/pid.cpp Normal file
View File

@ -0,0 +1,152 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/pid.h>
#include <limits>
namespace hector_quadrotor_controller {
PID::state::state()
: p(std::numeric_limits<double>::quiet_NaN())
, i(0.0)
, d(std::numeric_limits<double>::quiet_NaN())
, input(std::numeric_limits<double>::quiet_NaN())
, dinput(0.0)
, dx(std::numeric_limits<double>::quiet_NaN())
{
}
PID::parameters::parameters()
: enabled(true)
, time_constant(0.0)
, k_p(0.0)
, k_i(0.0)
, k_d(0.0)
, limit_i(std::numeric_limits<double>::quiet_NaN())
, limit_output(std::numeric_limits<double>::quiet_NaN())
{
}
PID::PID()
{}
PID::PID(const parameters& params)
: parameters_(params)
{}
PID::~PID()
{}
void PID::init(const ros::NodeHandle &param_nh)
{
param_nh.getParam("enabled", parameters_.enabled);
param_nh.getParam("k_p", parameters_.k_p);
param_nh.getParam("k_i", parameters_.k_i);
param_nh.getParam("k_d", parameters_.k_d);
param_nh.getParam("limit_i", parameters_.limit_i);
param_nh.getParam("limit_output", parameters_.limit_output);
param_nh.getParam("time_constant", parameters_.time_constant);
}
void PID::reset()
{
state_ = state();
}
template <typename T> static inline T& checknan(T& value)
{
if (std::isnan(value)) value = T();
return value;
}
double PID::update(double input, double x, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
double dt_sec = dt.toSec();
// low-pass filter input
if (std::isnan(state_.input)) state_.input = input;
if (dt_sec + parameters_.time_constant > 0.0) {
state_.dinput = (input - state_.input) / (dt_sec + parameters_.time_constant);
state_.input = (dt_sec * input + parameters_.time_constant * state_.input) / (dt_sec + parameters_.time_constant);
}
return update(state_.input - x, dx, dt);
}
double PID::update(double error, double dx, const ros::Duration& dt)
{
if (!parameters_.enabled) return 0.0;
if (std::isnan(error)) return 0.0;
double dt_sec = dt.toSec();
// integral error
state_.i += error * dt_sec;
if (parameters_.limit_i > 0.0)
{
if (state_.i > parameters_.limit_i) state_.i = parameters_.limit_i;
if (state_.i < -parameters_.limit_i) state_.i = -parameters_.limit_i;
}
// differential error
if (dt_sec > 0.0 && !std::isnan(state_.p) && !std::isnan(state_.dx)) {
state_.d = (error - state_.p) / dt_sec + state_.dx - dx;
} else {
state_.d = -dx;
}
state_.dx = dx;
// proportional error
state_.p = error;
// calculate output...
double output = parameters_.k_p * state_.p + parameters_.k_i * state_.i + parameters_.k_d * state_.d;
int antiwindup = 0;
if (parameters_.limit_output > 0.0)
{
if (output > parameters_.limit_output) { output = parameters_.limit_output; antiwindup = 1; }
if (output < -parameters_.limit_output) { output = -parameters_.limit_output; antiwindup = -1; }
}
if (antiwindup && (error * dt_sec * antiwindup > 0.0)) state_.i -= error * dt_sec;
checknan(output);
return output;
}
double PID::getFilteredControlError(double& filtered_error, double time_constant, const ros::Duration& dt)
{
double dt_sec = dt.toSec();
filtered_error = checknan(filtered_error);
if (dt_sec + time_constant > 0.0) {
filtered_error = (time_constant * filtered_error + dt_sec * state_.p) / (dt_sec + time_constant);
}
return filtered_error;
}
} // namespace hector_quadrotor_controller

204
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/pose_controller.cpp Normal file
View File

@ -0,0 +1,204 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class PoseController : public controller_interface::Controller<QuadrotorInterface>
{
public:
PoseController() {}
~PoseController() {}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
pose_input_ = interface->addInput<PoseCommandHandle>("pose");
twist_input_ = interface->addInput<TwistCommandHandle>("pose/twist");
twist_limit_ = interface->addInput<TwistCommandHandle>("pose/twist_limit");
twist_output_ = interface->addOutput<TwistCommandHandle>("twist");
node_handle_ = root_nh;
// subscribe to commanded pose and velocity
pose_subscriber_ = node_handle_.subscribe<geometry_msgs::PoseStamped>("command/pose", 1, boost::bind(&PoseController::poseCommandCallback, this, _1));
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&PoseController::twistCommandCallback, this, _1));
// initialize PID controllers
pid_.x.init(ros::NodeHandle(controller_nh, "xy"));
pid_.y.init(ros::NodeHandle(controller_nh, "xy"));
pid_.z.init(ros::NodeHandle(controller_nh, "z"));
pid_.yaw.init(ros::NodeHandle(controller_nh, "yaw"));
return true;
}
void reset()
{
pid_.x.reset();
pid_.y.reset();
pid_.z.reset();
pid_.yaw.reset();
}
void poseCommandCallback(const geometry_msgs::PoseStampedConstPtr& command)
{
pose_command_ = *command;
if (!(pose_input_->connected())) *pose_input_ = &(pose_command_.pose);
pose_input_->start();
ros::Time start_time = command->header.stamp;
if (start_time.isZero()) start_time = ros::Time::now();
if (!isRunning()) this->startRequest(start_time);
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
{
twist_command_ = *command;
if (!(twist_input_->connected())) *twist_input_ = &(twist_command_.twist);
twist_input_->start();
ros::Time start_time = command->header.stamp;
if (start_time.isZero()) start_time = ros::Time::now();
if (!isRunning()) this->startRequest(start_time);
}
void starting(const ros::Time &time)
{
reset();
twist_output_->start();
}
void stopping(const ros::Time &time)
{
twist_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
Twist output;
// check command timeout
// TODO
// return if no pose command is available
if (pose_input_->enabled()) {
// control horizontal position
double error_n, error_w;
HorizontalPositionCommandHandle(*pose_input_).getError(*pose_, error_n, error_w);
output.linear.x = pid_.x.update(error_n, twist_->twist().linear.x, period);
output.linear.y = pid_.y.update(error_w, twist_->twist().linear.y, period);
// control height
output.linear.z = pid_.z.update(HeightCommandHandle(*pose_input_).getError(*pose_), twist_->twist().linear.z, period);
// control yaw angle
output.angular.z = pid_.yaw.update(HeadingCommandHandle(*pose_input_).getError(*pose_), twist_->twist().angular.z, period);
}
// add twist command if available
if (twist_input_->enabled())
{
output.linear.x += twist_input_->getCommand().linear.x;
output.linear.y += twist_input_->getCommand().linear.y;
output.linear.z += twist_input_->getCommand().linear.z;
output.angular.x += twist_input_->getCommand().angular.x;
output.angular.y += twist_input_->getCommand().angular.y;
output.angular.z += twist_input_->getCommand().angular.z;
}
// limit twist
if (twist_limit_->enabled())
{
double linear_xy = sqrt(output.linear.x*output.linear.x + output.linear.y*output.linear.y);
double limit_linear_xy = std::max(twist_limit_->get()->linear.x, twist_limit_->get()->linear.y);
if (limit_linear_xy > 0.0 && linear_xy > limit_linear_xy) {
output.linear.x *= limit_linear_xy / linear_xy;
output.linear.y *= limit_linear_xy / linear_xy;
}
if (twist_limit_->get()->linear.z > 0.0 && fabs(output.linear.z) > twist_limit_->get()->linear.z) {
output.linear.z *= twist_limit_->get()->linear.z / fabs(output.linear.z);
}
double angular_xy = sqrt(output.angular.x*output.angular.x + output.angular.y*output.angular.y);
double limit_angular_xy = std::max(twist_limit_->get()->angular.x, twist_limit_->get()->angular.y);
if (limit_angular_xy > 0.0 && angular_xy > limit_angular_xy) {
output.angular.x *= limit_angular_xy / angular_xy;
output.angular.y *= limit_angular_xy / angular_xy;
}
if (twist_limit_->get()->angular.z > 0.0 && fabs(output.angular.z) > twist_limit_->get()->angular.z) {
output.angular.z *= twist_limit_->get()->angular.z / fabs(output.angular.z);
}
}
// set twist output
twist_output_->setCommand(output);
}
private:
PoseHandlePtr pose_;
PoseCommandHandlePtr pose_input_;
TwistHandlePtr twist_;
TwistCommandHandlePtr twist_input_;
TwistCommandHandlePtr twist_limit_;
TwistCommandHandlePtr twist_output_;
geometry_msgs::PoseStamped pose_command_;
geometry_msgs::TwistStamped twist_command_;
ros::NodeHandle node_handle_;
ros::Subscriber pose_subscriber_;
ros::Subscriber twist_subscriber_;
struct {
PID x;
PID y;
PID z;
PID yaw;
} pid_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::PoseController, controller_interface::ControllerBase)

195
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/quadrotor_interface.cpp Normal file
View File

@ -0,0 +1,195 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <cmath>
namespace hector_quadrotor_controller {
QuadrotorInterface::QuadrotorInterface()
{}
QuadrotorInterface::~QuadrotorInterface()
{}
bool QuadrotorInterface::enabled(const CommandHandle *handle) const
{
if (!handle || !handle->connected()) return false;
std::string resource = handle->getName();
return enabled_.count(resource) > 0;
}
bool QuadrotorInterface::start(const CommandHandle *handle)
{
if (!handle || !handle->connected()) return false;
if (enabled(handle)) return true;
std::string resource = handle->getName();
enabled_[resource] = handle;
ROS_DEBUG_NAMED("quadrotor_interface", "Enabled %s control", resource.c_str());
return true;
}
void QuadrotorInterface::stop(const CommandHandle *handle)
{
if (!handle) return;
if (!enabled(handle)) return;
std::string resource = handle->getName();
std::map<std::string, const CommandHandle *>::iterator it = enabled_.lower_bound(resource);
if (it != enabled_.end() && it->second == handle) enabled_.erase(it);
ROS_DEBUG_NAMED("quadrotor_interface", "Disabled %s control", resource.c_str());
}
void QuadrotorInterface::disconnect(const CommandHandle *handle)
{
if (!handle) return;
std::string resource = handle->getName();
if (inputs_.count(resource)) {
const CommandHandlePtr& input = inputs_.at(resource);
if (input.get() != handle) input->reset();
}
if (outputs_.count(resource)) {
const CommandHandlePtr& output = outputs_.at(resource);
if (output.get() != handle) output->reset();
}
}
const Pose *QuadrotorInterface::getPoseCommand() const { return getCommand<PoseCommandHandle>("pose"); }
const Twist *QuadrotorInterface::getTwistCommand() const { return getCommand<TwistCommandHandle>("twist"); }
const Wrench *QuadrotorInterface::getWrenchCommand() const { return getCommand<WrenchCommandHandle>("wrench"); }
const MotorCommand *QuadrotorInterface::getMotorCommand() const { return getCommand<MotorCommandHandle>("motor"); }
void PoseHandle::getEulerRPY(double &roll, double &pitch, double &yaw) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
roll = atan2(2.*y*z + 2.*w*x, z*z - y*y - x*x + w*w);
pitch = -asin(2.*x*z - 2.*w*y);
yaw = atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
double PoseHandle::getYaw() const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
Vector3 PoseHandle::toBody(const Vector3& nav) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
Vector3 body;
body.x = (w*w+x*x-y*y-z*z) * nav.x + (2.*x*y + 2.*w*z) * nav.y + (2.*x*z - 2.*w*y) * nav.z;
body.y = (2.*x*y - 2.*w*z) * nav.x + (w*w-x*x+y*y-z*z) * nav.y + (2.*y*z + 2.*w*x) * nav.z;
body.z = (2.*x*z + 2.*w*y) * nav.x + (2.*y*z - 2.*w*x) * nav.y + (w*w-x*x-y*y+z*z) * nav.z;
return body;
}
Vector3 PoseHandle::fromBody(const Vector3& body) const
{
const Quaternion::_w_type& w = pose().orientation.w;
const Quaternion::_x_type& x = pose().orientation.x;
const Quaternion::_y_type& y = pose().orientation.y;
const Quaternion::_z_type& z = pose().orientation.z;
Vector3 nav;
nav.x = (w*w+x*x-y*y-z*z) * body.x + (2.*x*y - 2.*w*z) * body.y + (2.*x*z + 2.*w*y) * body.z;
nav.y = (2.*x*y + 2.*w*z) * body.x + (w*w-x*x+y*y-z*z) * body.y + (2.*y*z - 2.*w*x) * body.z;
nav.z = (2.*x*z - 2.*w*y) * body.x + (2.*y*z + 2.*w*x) * body.y + (w*w-x*x-y*y+z*z) * body.z;
return nav;
}
void HorizontalPositionCommandHandle::getError(const PoseHandle &pose, double &x, double &y) const
{
getCommand(x, y);
x -= pose.get()->position.x;
y -= pose.get()->position.y;
}
double HeightCommandHandle::getError(const PoseHandle &pose) const
{
return getCommand() - pose.get()->position.z;
}
void HeadingCommandHandle::setCommand(double command)
{
if (get()) {
get()->x = 0.0;
get()->y = 0.0;
get()->z = sin(command / 2.);
get()->w = cos(command / 2.);
}
if (scalar_) {
*scalar_ = command;
}
}
double HeadingCommandHandle::getCommand() const {
if (scalar_) return *scalar_;
const Quaternion::_w_type& w = get()->w;
const Quaternion::_x_type& x = get()->x;
const Quaternion::_y_type& y = get()->y;
const Quaternion::_z_type& z = get()->z;
return atan2(2.*x*y + 2.*w*z, x*x + w*w - z*z - y*y);
}
bool HeadingCommandHandle::update(Pose& command) const {
if (get()) {
command.orientation = *get();
return true;
}
if (scalar_) {
command.orientation.x = 0.0;
command.orientation.y = 0.0;
command.orientation.z = sin(*scalar_ / 2.);
command.orientation.x = cos(*scalar_ / 2.);
return true;
}
return false;
}
double HeadingCommandHandle::getError(const PoseHandle &pose) const {
static const double M_2PI = 2.0 * M_PI;
double error = getCommand() - pose.getYaw() + M_PI;
error -= floor(error / M_2PI) * M_2PI;
return error - M_PI;
}
bool CommandHandle::enabled() { return interface_->enabled(this); }
bool CommandHandle::start() { return interface_->start(this); }
void CommandHandle::stop() { interface_->stop(this); }
void CommandHandle::disconnect() { interface_->disconnect(this); }
} // namespace hector_quadrotor_controller

328
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller (复件).cpp Normal file
View File

@ -0,0 +1,328 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
#include <boost/thread.hpp>
#include <limits>
namespace hector_quadrotor_controller {
using namespace controller_interface;
class TwistController : public controller_interface::Controller<QuadrotorInterface>
{
public:
TwistController()
{}
~TwistController()
{}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
acceleration_ = interface->getAcceleration();
twist_input_ = interface->addInput<TwistCommandHandle>("twist");
wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
node_handle_ = root_nh;
// subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("/fixedwing/cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
// engage/shutdown service servers
engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
// initialize PID controllers
pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
// load other parameters
controller_nh.getParam("auto_engage", auto_engage_ = true);
controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
controller_nh.getParam("limits/force/z", limits_.force.z);
controller_nh.getParam("limits/torque/xy", limits_.torque.x);
controller_nh.getParam("limits/torque/xy", limits_.torque.y);
controller_nh.getParam("limits/torque/z", limits_.torque.z);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// get mass and inertia from QuadrotorInterface
interface->getMassAndInertia(mass_, inertia_);
command_given_in_stabilized_frame_ = false;
return true;
}
void reset()
{
pid_.linear.x.reset();
pid_.linear.y.reset();
pid_.linear.z.reset();
pid_.angular.x.reset();
pid_.angular.y.reset();
pid_.angular.z.reset();
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
linear_z_control_error_ = 0.0;
motors_running_ = false;
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr& command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_ = *command;
if (command_.header.stamp.isZero()) command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = false;
// start controller if it not running
if (!isRunning()) this->startRequest(command_.header.stamp);
}
void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr& command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_.twist = *command;
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = true;
// start controller if it not running
if (!isRunning()) this->startRequest(command_.header.stamp);
}
bool engageCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
motors_running_ = true;
return true;
}
bool shutdownCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
motors_running_ = false;
return true;
}
void starting(const ros::Time &time)
{
reset();
wrench_output_->start();
}
void stopping(const ros::Time &time)
{
wrench_output_->stop();
}
void update(const ros::Time& time, const ros::Duration& period)
{
boost::mutex::scoped_lock lock(command_mutex_);
// Get twist command input
if (twist_input_->connected() && twist_input_->enabled()) {
command_.twist = twist_input_->getCommand();
command_given_in_stabilized_frame_ = false;
}
// Get current state and command
Twist command = command_.twist;
Twist twist = twist_->twist();
Twist twist_body;
twist_body.linear = pose_->toBody(twist.linear);
twist_body.angular = pose_->toBody(twist.angular);
// Transform to world coordinates if necessary (yaw only)
if (command_given_in_stabilized_frame_) {
double yaw = pose_->getYaw();
Twist transformed = command;
transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
command = transformed;
}
// Get gravity and load factor
const double gravity = 9.8065;
double load_factor = 1. / ( pose_->pose().orientation.w * pose_->pose().orientation.w
- pose_->pose().orientation.x * pose_->pose().orientation.x
- pose_->pose().orientation.y * pose_->pose().orientation.y
+ pose_->pose().orientation.z * pose_->pose().orientation.z );
// Note: load_factor could be NaN or Inf...?
if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit)) load_factor = load_factor_limit;
// Auto engage/shutdown
if (auto_engage_) {
if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0) {
motors_running_ = true;
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
} else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */) {
double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
if (linear_z_control_error_ > 0.0) linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit) {
motors_running_ = false;
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
} else {
ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
}
} else {
linear_z_control_error_ = 0.0;
}
// flip over?
if (motors_running_ && load_factor < 0.0) {
motors_running_ = false;
ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
}
}
// Update output
if (motors_running_) {
Vector3 acceleration_command;
acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update( acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update( command.angular.z, twist.angular.z, 0.0, period);
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z) wrench_.wrench.force.z = limits_.force.z;
if (wrench_.wrench.force.z <= std::numeric_limits<double>::min()) wrench_.wrench.force.z = std::numeric_limits<double>::min();
if (limits_.torque.x > 0.0) {
if (wrench_.wrench.torque.x > limits_.torque.x) wrench_.wrench.torque.x = limits_.torque.x;
if (wrench_.wrench.torque.x < -limits_.torque.x) wrench_.wrench.torque.x = -limits_.torque.x;
}
if (limits_.torque.y > 0.0) {
if (wrench_.wrench.torque.y > limits_.torque.y) wrench_.wrench.torque.y = limits_.torque.y;
if (wrench_.wrench.torque.y < -limits_.torque.y) wrench_.wrench.torque.y = -limits_.torque.y;
}
if (limits_.torque.z > 0.0) {
if (wrench_.wrench.torque.z > limits_.torque.z) wrench_.wrench.torque.z = limits_.torque.z;
if (wrench_.wrench.torque.z < -limits_.torque.z) wrench_.wrench.torque.z = -limits_.torque.z;
}
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
} else {
reset();
}
// set wrench output
wrench_.header.stamp = time;
wrench_.header.frame_id = base_link_frame_;
wrench_output_->setCommand(wrench_.wrench);
}
private:
PoseHandlePtr pose_;
TwistHandlePtr twist_;
AccelerationHandlePtr acceleration_;
TwistCommandHandlePtr twist_input_;
WrenchCommandHandlePtr wrench_output_;
ros::NodeHandle node_handle_;
ros::Subscriber twist_subscriber_;
ros::Subscriber cmd_vel_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::TwistStamped command_;
geometry_msgs::WrenchStamped wrench_;
bool command_given_in_stabilized_frame_;
std::string base_link_frame_;
struct {
struct {
PID x;
PID y;
PID z;
} linear, angular;
} pid_;
geometry_msgs::Wrench limits_;
bool auto_engage_;
double load_factor_limit;
double mass_;
double inertia_[3];
bool motors_running_;
double linear_z_control_error_;
boost::mutex command_mutex_;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

372
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller/src/twist_controller.cpp Normal file
View File

@ -0,0 +1,372 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <hector_quadrotor_controller/pid.h>
#include <controller_interface/controller.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <std_srvs/Empty.h>
#include <ros/subscriber.h>
#include <ros/callback_queue.h>
#include <boost/thread.hpp>
#include <limits>
#include <boost/timer.hpp> //add by zhangshuai, used to timing
#include <iostream>
namespace hector_quadrotor_controller
{
using namespace controller_interface;
// using namespace std;
class TwistController : public controller_interface::Controller<QuadrotorInterface>
{
public:
TwistController()
{
}
~TwistController()
{
}
bool init(QuadrotorInterface *interface, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
{
// get interface handles
pose_ = interface->getPose();
twist_ = interface->getTwist();
acceleration_ = interface->getAcceleration();
twist_input_ = interface->addInput<TwistCommandHandle>("twist");
wrench_output_ = interface->addOutput<WrenchCommandHandle>("wrench");
node_handle_ = root_nh;
// subscribe to commanded twist (geometry_msgs/TwistStamped) and cmd_vel (geometry_msgs/Twist)
twist_subscriber_ = node_handle_.subscribe<geometry_msgs::TwistStamped>("command/twist", 1, boost::bind(&TwistController::twistCommandCallback, this, _1));
cmd_vel_subscriber_ = node_handle_.subscribe<geometry_msgs::Twist>("/cmd_vel", 1, boost::bind(&TwistController::cmd_velCommandCallback, this, _1));
// engage/shutdown service servers
engage_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("engage", boost::bind(&TwistController::engageCallback, this, _1, _2));
shutdown_service_server_ = node_handle_.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>("shutdown", boost::bind(&TwistController::shutdownCallback, this, _1, _2));
// initialize PID controllers
pid_.linear.x.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.y.init(ros::NodeHandle(controller_nh, "linear/xy"));
pid_.linear.z.init(ros::NodeHandle(controller_nh, "linear/z"));
pid_.angular.x.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.y.init(ros::NodeHandle(controller_nh, "angular/xy"));
pid_.angular.z.init(ros::NodeHandle(controller_nh, "angular/z"));
// load other parameters
controller_nh.getParam("auto_engage", auto_engage_ = true);
controller_nh.getParam("limits/load_factor", load_factor_limit = 1.5);
controller_nh.getParam("limits/force/z", limits_.force.z);
controller_nh.getParam("limits/torque/xy", limits_.torque.x);
controller_nh.getParam("limits/torque/xy", limits_.torque.y);
controller_nh.getParam("limits/torque/z", limits_.torque.z);
root_nh.param<std::string>("base_link_frame", base_link_frame_, "base_link");
// get mass and inertia from QuadrotorInterface
interface->getMassAndInertia(mass_, inertia_);
command_given_in_stabilized_frame_ = false;
return true;
}
void reset()
{
pid_.linear.x.reset();
pid_.linear.y.reset();
pid_.linear.z.reset();
pid_.angular.x.reset();
pid_.angular.y.reset();
pid_.angular.z.reset();
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = 0.0;
wrench_.wrench.torque.x = 0.0;
wrench_.wrench.torque.y = 0.0;
wrench_.wrench.torque.z = 0.0;
linear_z_control_error_ = 0.0;
motors_running_ = false;
}
void twistCommandCallback(const geometry_msgs::TwistStampedConstPtr &command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_ = *command;
if (command_.header.stamp.isZero())
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = false;
// start controller if it not running
if (!isRunning())
this->startRequest(command_.header.stamp);
}
void cmd_velCommandCallback(const geometry_msgs::TwistConstPtr &command)
{
boost::mutex::scoped_lock lock(command_mutex_);
command_.twist = *command;
command_.header.stamp = ros::Time::now();
command_given_in_stabilized_frame_ = true;
// start controller if it not running
if (!isRunning())
this->startRequest(command_.header.stamp);
}
bool engageCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
motors_running_ = true;
return true;
}
bool shutdownCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
{
boost::mutex::scoped_lock lock(command_mutex_);
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
motors_running_ = false;
return true;
}
void starting(const ros::Time &time)
{
reset();
wrench_output_->start();
}
void stopping(const ros::Time &time)
{
wrench_output_->stop();
}
void update(const ros::Time &time, const ros::Duration &period)
{
boost::mutex::scoped_lock lock(command_mutex_);
// Get twist command input
if (twist_input_->connected() && twist_input_->enabled())
{
command_.twist = twist_input_->getCommand();
command_given_in_stabilized_frame_ = false;
}
// Get current state and command
Twist command = command_.twist;
Twist twist = twist_->twist();
Twist twist_body;
twist_body.linear = pose_->toBody(twist.linear);
twist_body.angular = pose_->toBody(twist.angular);
// Transform to world coordinates if necessary (yaw only)
if (command_given_in_stabilized_frame_)
{
double yaw = pose_->getYaw();
Twist transformed = command;
transformed.linear.x = cos(yaw) * command.linear.x - sin(yaw) * command.linear.y;
transformed.linear.y = sin(yaw) * command.linear.x + cos(yaw) * command.linear.y;
transformed.angular.x = cos(yaw) * command.angular.x - sin(yaw) * command.angular.y;
transformed.angular.y = sin(yaw) * command.angular.x + cos(yaw) * command.angular.y;
command = transformed;
}
// Get gravity and load factor
const double gravity = 9.8065;
double load_factor = 1. / (pose_->pose().orientation.w * pose_->pose().orientation.w - pose_->pose().orientation.x * pose_->pose().orientation.x - pose_->pose().orientation.y * pose_->pose().orientation.y + pose_->pose().orientation.z * pose_->pose().orientation.z);
// Note: load_factor could be NaN or Inf...?
if (load_factor_limit > 0.0 && !(load_factor < load_factor_limit))
load_factor = load_factor_limit;
// Auto engage/shutdown
if (auto_engage_)
{
if (!motors_running_ && command.linear.z > 0.1 && load_factor > 0.0)
{
motors_running_ = true;
ROS_INFO_NAMED("twist_controller", "Engaging motors!");
}
else if (motors_running_ && command.linear.z < -0.1 /* && (twist.linear.z > -0.1 && twist.linear.z < 0.1) */)
{
double shutdown_limit = 0.25 * std::min(command.linear.z, -0.5);
if (linear_z_control_error_ > 0.0)
linear_z_control_error_ = 0.0; // positive control errors should not affect shutdown
if (pid_.linear.z.getFilteredControlError(linear_z_control_error_, 5.0, period) < shutdown_limit)
{
motors_running_ = false;
ROS_INFO_NAMED("twist_controller", "Shutting down motors!");
}
else
{
ROS_DEBUG_STREAM_NAMED("twist_controller", "z control error = " << linear_z_control_error_ << " >= " << shutdown_limit);
}
}
else
{
linear_z_control_error_ = 0.0;
}
// flip over?
if (motors_running_ && load_factor < 0.0)
{
motors_running_ = false;
ROS_WARN_NAMED("twist_controller", "Shutting down motors due to flip over!");
}
}
// Update output
if (motors_running_)
{
Vector3 acceleration_command;
boost::timer t;//声明计时器对象并开始计时 add by zhangshuai
acceleration_command.x = pid_.linear.x.update(command.linear.x, twist.linear.x, acceleration_->acceleration().x, period);
// std::cout<<"运行时间："<<t.elapsed() <<"s"<< std::endl;//输出已流失的时间
acceleration_command.y = pid_.linear.y.update(command.linear.y, twist.linear.y, acceleration_->acceleration().y, period);
acceleration_command.z = pid_.linear.z.update(command.linear.z, twist.linear.z, acceleration_->acceleration().z, period) + gravity;
Vector3 acceleration_command_body = pose_->toBody(acceleration_command);
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist.linear: [" << twist.linear.x << " " << twist.linear.y << " " << twist.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_body.angular: [" << twist_body.angular.x << " " << twist_body.angular.y << " " << twist_body.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.linear: [" << command.linear.x << " " << command.linear.y << " " << command.linear.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "twist_command.angular: [" << command.angular.x << " " << command.angular.y << " " << command.angular.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration: [" << acceleration_->acceleration().x << " " << acceleration_->acceleration().y << " " << acceleration_->acceleration().z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_world: [" << acceleration_command.x << " " << acceleration_command.y << " " << acceleration_command.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "acceleration_command_body: [" << acceleration_command_body.x << " " << acceleration_command_body.y << " " << acceleration_command_body.z << "]");
wrench_.wrench.torque.x = inertia_[0] * pid_.angular.x.update(-acceleration_command_body.y / gravity, 0.0, twist_body.angular.x, period);
wrench_.wrench.torque.y = inertia_[1] * pid_.angular.y.update(acceleration_command_body.x / gravity, 0.0, twist_body.angular.y, period);
wrench_.wrench.torque.z = inertia_[2] * pid_.angular.z.update(command.angular.z, twist.angular.z, 0.0, period);
wrench_.wrench.force.x = 0.0;
wrench_.wrench.force.y = 0.0;
wrench_.wrench.force.z = mass_ * ((acceleration_command.z - gravity) * load_factor + gravity);
if (limits_.force.z > 0.0 && wrench_.wrench.force.z > limits_.force.z)
wrench_.wrench.force.z = limits_.force.z;
if (wrench_.wrench.force.z <= std::numeric_limits<double>::min())
wrench_.wrench.force.z = std::numeric_limits<double>::min();
if (limits_.torque.x > 0.0)
{
if (wrench_.wrench.torque.x > limits_.torque.x)
wrench_.wrench.torque.x = limits_.torque.x;
if (wrench_.wrench.torque.x < -limits_.torque.x)
wrench_.wrench.torque.x = -limits_.torque.x;
}
if (limits_.torque.y > 0.0)
{
if (wrench_.wrench.torque.y > limits_.torque.y)
wrench_.wrench.torque.y = limits_.torque.y;
if (wrench_.wrench.torque.y < -limits_.torque.y)
wrench_.wrench.torque.y = -limits_.torque.y;
}
if (limits_.torque.z > 0.0)
{
if (wrench_.wrench.torque.z > limits_.torque.z)
wrench_.wrench.torque.z = limits_.torque.z;
if (wrench_.wrench.torque.z < -limits_.torque.z)
wrench_.wrench.torque.z = -limits_.torque.z;
}
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.force: [" << wrench_.wrench.force.x << " " << wrench_.wrench.force.y << " " << wrench_.wrench.force.z << "]");
ROS_DEBUG_STREAM_NAMED("twist_controller", "wrench_command.torque: [" << wrench_.wrench.torque.x << " " << wrench_.wrench.torque.y << " " << wrench_.wrench.torque.z << "]");
}
else
{
reset();
}
// set wrench output
wrench_.header.stamp = time;
wrench_.header.frame_id = base_link_frame_;
wrench_output_->setCommand(wrench_.wrench);
}
private:
PoseHandlePtr pose_;
TwistHandlePtr twist_;
AccelerationHandlePtr acceleration_;
TwistCommandHandlePtr twist_input_;
WrenchCommandHandlePtr wrench_output_;
ros::NodeHandle node_handle_;
ros::Subscriber twist_subscriber_;
ros::Subscriber cmd_vel_subscriber_;
ros::ServiceServer engage_service_server_;
ros::ServiceServer shutdown_service_server_;
geometry_msgs::TwistStamped command_;
geometry_msgs::WrenchStamped wrench_;
bool command_given_in_stabilized_frame_;
std::string base_link_frame_;
struct
{
struct
{
PID x;
PID y;
PID z;
} linear, angular;
} pid_;
geometry_msgs::Wrench limits_;
bool auto_engage_;
double load_factor_limit;
double mass_;
double inertia_[3];
bool motors_running_;
double linear_z_control_error_;
boost::mutex command_mutex_;
// std::ostream cout1;
// cout1<<"运行时间："<<t.elapsed() <<"s"<< std::endl;//输出已流失的时间
// printf("%s s",t.elapsed());
// std::cout << "C_mz = " << drag_model_->parameters_.C_mz << std::endl;
};
} // namespace hector_quadrotor_controller
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_quadrotor_controller::TwistController, controller_interface::ControllerBase)

20
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CHANGELOG.rst Normal file
View File

@ -0,0 +1,20 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_quadrotor_controller_gazebo
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.3.5 (2015-03-28)
------------------
0.3.4 (2015-02-22)
------------------
0.3.3 (2014-09-01)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-12-26)
------------------
* New controller implementation using ros_control
* Contributors: Johannes Meyer

122
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller_gazebo/CMakeLists.txt Normal file
View File

@ -0,0 +1,122 @@
cmake_minimum_required(VERSION 2.8.3)
project(hector_quadrotor_controller_gazebo)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
gazebo_ros_control
hector_quadrotor_controller
)
include_directories(include ${catkin_INCLUDE_DIRS})
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
# Depend on system install of Gazebo
find_package(gazebo REQUIRED)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}")
link_directories(${GAZEBO_LIBRARY_DIRS})
include_directories(${GAZEBO_INCLUDE_DIRS})
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
#######################################
## Declare ROS messages and services ##
#######################################
## Generate messages in the 'msg' folder
# add_message_files(
# FILES
# Message1.msg
# Message2.msg
# )
## Generate services in the 'srv' folder
# add_service_files(
# FILES
# Service1.srv
# Service2.srv
# )
## Generate added messages and services with any dependencies listed here
# generate_messages(
# DEPENDENCIES
# std_msgs # Or other packages containing msgs
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES hector_quadrotor_controller_gazebo
CATKIN_DEPENDS gazebo_ros_control hector_quadrotor_controller
# DEPENDS system_lib
)
###########
## Build ##
###########
add_library(hector_quadrotor_controller_gazebo
src/quadrotor_hardware_gazebo.cpp
)
target_link_libraries(hector_quadrotor_controller_gazebo
${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS hector_quadrotor_controller_gazebo
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
)
## Mark other files for installation (e.g. launch and bag files, etc.)
install(FILES
quadrotor_controller_gazebo.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_hector_quadrotor_controller_gazebo.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

109
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller_gazebo/include/hector_quadrotor_controller/quadrotor_hardware_gazebo.h Normal file
View File

@ -0,0 +1,109 @@
//=================================================================================================
// Copyright (c) 2013, Johannes Meyer, TU Darmstadt
// All rights reserved.
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of the Flight Systems and Automatic Control group,
// TU Darmstadt, nor the names of its contributors may be used to
// endorse or promote products derived from this software without
// specific prior written permission.
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//=================================================================================================
#ifndef HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
#define HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H
#include <gazebo_ros_control/robot_hw_sim.h>
#include <hector_quadrotor_controller/quadrotor_interface.h>
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <hector_uav_msgs/MotorStatus.h>
#include <ros/node_handle.h>
#include <ros/callback_queue.h>
namespace hector_quadrotor_controller_gazebo {
using namespace hector_quadrotor_controller;
using namespace hardware_interface;
using namespace gazebo_ros_control;
class QuadrotorHardwareSim : public RobotHWSim, public QuadrotorInterface
{
public:
QuadrotorHardwareSim();
virtual ~QuadrotorHardwareSim();
virtual const ros::Time &getTimestamp() { return header_.stamp; }
virtual PoseHandlePtr getPose() { return PoseHandlePtr(new PoseHandle(this, &pose_)); }
virtual TwistHandlePtr getTwist() { return TwistHandlePtr(new TwistHandle(this, &twist_)); }
virtual AccelerationHandlePtr getAcceleration() { return AccelerationHandlePtr(new AccelerationHandle(this, &acceleration_)); }
virtual ImuHandlePtr getSensorImu() { return ImuHandlePtr(new ImuHandle(this, &imu_)); }
virtual MotorStatusHandlePtr getMotorStatus() { return MotorStatusHandlePtr(new MotorStatusHandle(this, &motor_status_)); }
virtual bool getMassAndInertia(double &mass, double inertia[3]);
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions);
virtual void readSim(ros::Time time, ros::Duration period);
virtual void writeSim(ros::Time time, ros::Duration period);
private:
void stateCallback(const nav_msgs::OdometryConstPtr &state);
void imuCallback(const sensor_msgs::ImuConstPtr &imu);
void motorStatusCallback(const hector_uav_msgs::MotorStatusConstPtr &motor_status);
protected:
std_msgs::Header header_;
Pose pose_;
Twist twist_;
Vector3 acceleration_;
Imu imu_;
MotorStatus motor_status_;
std::string base_link_frame_, world_frame_;
WrenchCommandHandlePtr wrench_output_;
MotorCommandHandlePtr motor_output_;
gazebo::physics::ModelPtr model_;
gazebo::physics::LinkPtr link_;
gazebo::physics::PhysicsEnginePtr physics_;
gazebo::math::Pose gz_pose_;
gazebo::math::Vector3 gz_velocity_, gz_acceleration_, gz_angular_velocity_;
ros::CallbackQueue callback_queue_;
ros::Subscriber subscriber_state_;
ros::Subscriber subscriber_imu_;
ros::Subscriber subscriber_motor_status_;
ros::Publisher publisher_wrench_command_;
ros::Publisher publisher_motor_command_;
};
} // namespace hector_quadrotor_controller_gazebo
#endif // HECTOR_QUADROTOR_CONTROLLER_QUADROTOR_HARDWARE_GAZEBO_H

24
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller_gazebo/package.xml Normal file
View File

@ -0,0 +1,24 @@
<?xml version="1.0"?>
<package>
<name>hector_quadrotor_controller_gazebo</name>
<version>0.3.5</version>
<description>The hector_quadrotor_controller_gazebo package implements the ros_control RobotHWSim interface for the quadrotor controller in package hector_quadrotor_controller.</description>
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_quadrotor_controller_gazebo</url>
<url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
<author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend version_gte="2.3.4">gazebo_ros_control</build_depend>
<build_depend>hector_quadrotor_controller</build_depend>
<run_depend version_gte="2.3.4">gazebo_ros_control</run_depend>
<run_depend>hector_quadrotor_controller</run_depend>
<export>
<gazebo_ros_control plugin="${prefix}/quadrotor_controller_gazebo.xml"/>
</export>
</package>

7
Gazebo_Hector_Test/src/hector_quadrotor/hector_quadrotor_controller_gazebo/quadrotor_controller_gazebo.xml Normal file
View File

@ -0,0 +1,7 @@
<library path="lib/libhector_quadrotor_controller_gazebo">
<class name="hector_quadrotor_controller_gazebo/QuadrotorHardwareSim" type="hector_quadrotor_controller_gazebo::QuadrotorHardwareSim" base_class_type="gazebo_ros_control::RobotHWSim">
<description>
This class represents the quadrotor hardware in Gazebo for the gazebo_ros_control plugin.
</description>
</class>
</library>

Some files were not shown because too many files have changed in this diff Show More