1. xml_configuration branch merged
2. sdformat updated with distribution 3. hector test project (ROS)
This commit is contained in:
parent
a57eba035e
commit
18b2f5223e
|
@ -0,0 +1,2 @@
|
||||||
|
build
|
||||||
|
.vscode
|
|
@ -131,5 +131,6 @@ set(headers
|
||||||
gazebo.hh
|
gazebo.hh
|
||||||
Master.hh
|
Master.hh
|
||||||
Server.hh
|
Server.hh
|
||||||
|
countTime.hh
|
||||||
)
|
)
|
||||||
gz_install_includes("" ${headers})
|
gz_install_includes("" ${headers})
|
||||||
|
|
|
@ -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
|
|
@ -85,6 +85,8 @@ set (sources ${sources}
|
||||||
World.cc
|
World.cc
|
||||||
WorldState.cc
|
WorldState.cc
|
||||||
TcpCommunication.cc #zhangshuai 2019.03.19
|
TcpCommunication.cc #zhangshuai 2019.03.19
|
||||||
|
Distribution.cc #zhangshuai 2019.03.28
|
||||||
|
GazeboID.cc #zhangshuai 2019.03.28
|
||||||
)
|
)
|
||||||
|
|
||||||
set (headers
|
set (headers
|
||||||
|
@ -139,6 +141,8 @@ set (headers
|
||||||
World.hh
|
World.hh
|
||||||
WorldState.hh
|
WorldState.hh
|
||||||
TcpCommunication.hh #zhangshuai 2019.03.19
|
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)
|
set (physics_headers "" CACHE INTERNAL "physics headers" FORCE)
|
||||||
|
|
|
@ -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();
|
||||||
|
}
|
|
@ -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
|
|
@ -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();
|
||||||
|
}
|
|
@ -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
|
|
@ -16,9 +16,9 @@
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifdef _WIN32
|
#ifdef _WIN32
|
||||||
// Ensure that Winsock2.h is included before Windows.h, which can get
|
// Ensure that Winsock2.h is included before Windows.h, which can get
|
||||||
// pulled in by anybody (e.g., Boost).
|
// pulled in by anybody (e.g., Boost).
|
||||||
#include <Winsock2.h>
|
#include <Winsock2.h>
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#include <boost/algorithm/string.hpp>
|
#include <boost/algorithm/string.hpp>
|
||||||
|
@ -85,8 +85,7 @@ void Link::Load(sdf::ElementPtr _sdf)
|
||||||
{
|
{
|
||||||
this->SetSelfCollide(this->GetModel()->GetSelfCollide());
|
this->SetSelfCollide(this->GetModel()->GetSelfCollide());
|
||||||
}
|
}
|
||||||
this->sdf->GetElement("self_collide")->GetValue()->SetUpdateFunc(
|
this->sdf->GetElement("self_collide")->GetValue()->SetUpdateFunc(std::bind(&Link::GetSelfCollide, this));
|
||||||
std::bind(&Link::GetSelfCollide, this));
|
|
||||||
|
|
||||||
// Parse visuals from SDF
|
// Parse visuals from SDF
|
||||||
this->ParseVisuals();
|
this->ParseVisuals();
|
||||||
|
@ -103,45 +102,43 @@ void Link::Load(sdf::ElementPtr _sdf)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
//zhangshuai
|
// Modified by zhangshuai 2019.04.02 ----Begin
|
||||||
std::string bebop_0 = "bebop_0";
|
// Judge if the model to load is simulated in this gazebo
|
||||||
std::cout << "bebop_0:" << bebop_0 << std::endl; //zhangshuai
|
for (unsigned int i = 0; i < this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelCount(); i++)
|
||||||
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)
|
|
||||||
{
|
{
|
||||||
//zhangshuai
|
if (this->GetModel()->GetName() == this->GetWorld()->GetDistribution()->GetGazeboIDPtr(this->GetWorld()->GetGazeboLocalID())->GetModelName(i))
|
||||||
if (this->sdf->HasElement("sensor"))
|
|
||||||
{
|
|
||||||
sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor");
|
|
||||||
while (sensorElem)
|
|
||||||
{
|
{
|
||||||
/// \todo This if statement is a hack to prevent Links from creating
|
// Original part
|
||||||
/// a force torque sensor. We should make this more generic.
|
if (this->sdf->HasElement("sensor"))
|
||||||
if (sensorElem->Get<std::string>("type") == "force_torque")
|
|
||||||
{
|
{
|
||||||
gzerr << "A link cannot load a [" <<
|
sdf::ElementPtr sensorElem = this->sdf->GetElement("sensor");
|
||||||
sensorElem->Get<std::string>("type") << "] sensor.\n";
|
while (sensorElem)
|
||||||
}
|
{
|
||||||
else if (sensorElem->Get<std::string>("type") != "__default__")
|
/// \todo This if statement is a hack to prevent Links from creating
|
||||||
{
|
/// a force torque sensor. We should make this more generic.
|
||||||
// This must match the implementation in Sensors::GetScopedName
|
if (sensorElem->Get<std::string>("type") == "force_torque")
|
||||||
std::string sensorName = this->GetScopedName(true) + "::" +
|
{
|
||||||
sensorElem->Get<std::string>("name");
|
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.
|
// Tell the sensor library to create a sensor.
|
||||||
event::Events::createSensor(sensorElem,
|
event::Events::createSensor(sensorElem,
|
||||||
this->GetWorld()->GetName(), this->GetScopedName(), this->GetId());
|
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
|
// Modified by zhangshuai 2019.04.02 ----End
|
||||||
}
|
|
||||||
//zhangshuai
|
|
||||||
|
|
||||||
// Load the lights
|
// Load the lights
|
||||||
if (this->sdf->HasElement("light"))
|
if (this->sdf->HasElement("light"))
|
||||||
|
@ -187,16 +184,16 @@ void Link::Load(sdf::ElementPtr _sdf)
|
||||||
if (!collisionNames.empty())
|
if (!collisionNames.empty())
|
||||||
{
|
{
|
||||||
for (std::vector<std::string>::iterator iter = collisionNames.begin();
|
for (std::vector<std::string>::iterator iter = collisionNames.begin();
|
||||||
iter != collisionNames.end(); ++iter)
|
iter != collisionNames.end(); ++iter)
|
||||||
{
|
{
|
||||||
(*iter) = this->GetScopedName() + "::" + (*iter);
|
(*iter) = this->GetScopedName() + "::" + (*iter);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::string topic =
|
std::string topic =
|
||||||
this->world->GetPhysicsEngine()->GetContactManager()->CreateFilter(
|
this->world->GetPhysicsEngine()->GetContactManager()->CreateFilter(
|
||||||
this->GetScopedName() + "/audio_collision", collisionNames);
|
this->GetScopedName() + "/audio_collision", collisionNames);
|
||||||
this->audioContactsSub = this->node->Subscribe(topic,
|
this->audioContactsSub = this->node->Subscribe(topic,
|
||||||
&Link::OnCollision, this);
|
&Link::OnCollision, this);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -249,7 +246,7 @@ void Link::Init()
|
||||||
}
|
}
|
||||||
if ((*iter)->HasType(Base::LIGHT))
|
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?
|
// TODO add to lights var?
|
||||||
// this->lights.push_back(light);
|
// this->lights.push_back(light);
|
||||||
light->Init();
|
light->Init();
|
||||||
|
@ -290,7 +287,7 @@ void Link::Fini()
|
||||||
for (auto iter : this->visuals)
|
for (auto iter : this->visuals)
|
||||||
{
|
{
|
||||||
auto deleteMsg = msgs::CreateRequest("entity_delete",
|
auto deleteMsg = msgs::CreateRequest("entity_delete",
|
||||||
std::to_string(iter.second.id()));
|
std::to_string(iter.second.id()));
|
||||||
this->requestPub->Publish(*deleteMsg, true);
|
this->requestPub->Publish(*deleteMsg, true);
|
||||||
|
|
||||||
msgs::Visual msg;
|
msgs::Visual msg;
|
||||||
|
@ -373,8 +370,7 @@ void Link::UpdateParameters(sdf::ElementPtr _sdf)
|
||||||
|
|
||||||
this->sdf->GetElement("gravity")->GetValue()->SetUpdateFunc(
|
this->sdf->GetElement("gravity")->GetValue()->SetUpdateFunc(
|
||||||
boost::bind(&Link::GetGravityMode, this));
|
boost::bind(&Link::GetGravityMode, this));
|
||||||
this->sdf->GetElement("kinematic")->GetValue()->SetUpdateFunc(
|
this->sdf->GetElement("kinematic")->GetValue()->SetUpdateFunc(boost::bind(&Link::GetKinematic, this));
|
||||||
boost::bind(&Link::GetKinematic, this));
|
|
||||||
|
|
||||||
if (this->sdf->Get<bool>("gravity") != this->GetGravityMode())
|
if (this->sdf->Get<bool>("gravity") != this->GetGravityMode())
|
||||||
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
|
this->SetGravityMode(this->sdf->Get<bool>("gravity"));
|
||||||
|
@ -442,13 +438,13 @@ void Link::SetCollideMode(const std::string &_mode)
|
||||||
|
|
||||||
if (_mode == "all")
|
if (_mode == "all")
|
||||||
{
|
{
|
||||||
categoryBits = GZ_ALL_COLLIDE;
|
categoryBits = GZ_ALL_COLLIDE;
|
||||||
collideBits = GZ_ALL_COLLIDE;
|
collideBits = GZ_ALL_COLLIDE;
|
||||||
}
|
}
|
||||||
else if (_mode == "none")
|
else if (_mode == "none")
|
||||||
{
|
{
|
||||||
categoryBits = GZ_NONE_COLLIDE;
|
categoryBits = GZ_NONE_COLLIDE;
|
||||||
collideBits = GZ_NONE_COLLIDE;
|
collideBits = GZ_NONE_COLLIDE;
|
||||||
}
|
}
|
||||||
else if (_mode == "sensors")
|
else if (_mode == "sensors")
|
||||||
{
|
{
|
||||||
|
@ -514,7 +510,8 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
|
||||||
|
|
||||||
// Update all the audio sources
|
// Update all the audio sources
|
||||||
for (std::vector<util::OpenALSourcePtr>::iterator iter =
|
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)->SetPose(this->GetWorldPose().Ign());
|
||||||
(*iter)->SetVelocity(this->GetWorldLinearVel().Ign());
|
(*iter)->SetVelocity(this->GetWorldLinearVel().Ign());
|
||||||
|
@ -522,7 +519,7 @@ void Link::Update(const common::UpdateInfo & /*_info*/)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// FIXME: race condition on factory-based model loading!!!!!
|
// FIXME: race condition on factory-based model loading!!!!!
|
||||||
/*if (this->GetEnabled() != this->enabled)
|
/*if (this->GetEnabled() != this->enabled)
|
||||||
{
|
{
|
||||||
this->enabled = this->GetEnabled();
|
this->enabled = this->GetEnabled();
|
||||||
this->enabledSignal(this->enabled);
|
this->enabledSignal(this->enabled);
|
||||||
|
@ -567,8 +564,8 @@ Link_V Link::GetChildJointsLinks() const
|
||||||
{
|
{
|
||||||
Link_V links;
|
Link_V links;
|
||||||
for (std::vector<JointPtr>::const_iterator iter = this->childJoints.begin();
|
for (std::vector<JointPtr>::const_iterator iter = this->childJoints.begin();
|
||||||
iter != this->childJoints.end();
|
iter != this->childJoints.end();
|
||||||
++iter)
|
++iter)
|
||||||
{
|
{
|
||||||
if ((*iter)->GetChild())
|
if ((*iter)->GetChild())
|
||||||
links.push_back((*iter)->GetChild());
|
links.push_back((*iter)->GetChild());
|
||||||
|
@ -581,8 +578,8 @@ Link_V Link::GetParentJointsLinks() const
|
||||||
{
|
{
|
||||||
Link_V links;
|
Link_V links;
|
||||||
for (std::vector<JointPtr>::const_iterator iter = this->parentJoints.begin();
|
for (std::vector<JointPtr>::const_iterator iter = this->parentJoints.begin();
|
||||||
iter != this->parentJoints.end();
|
iter != this->parentJoints.end();
|
||||||
++iter)
|
++iter)
|
||||||
{
|
{
|
||||||
if ((*iter)->GetParent())
|
if ((*iter)->GetParent())
|
||||||
links.push_back((*iter)->GetParent());
|
links.push_back((*iter)->GetParent());
|
||||||
|
@ -595,13 +592,13 @@ void Link::LoadCollision(sdf::ElementPtr _sdf)
|
||||||
{
|
{
|
||||||
CollisionPtr collision;
|
CollisionPtr collision;
|
||||||
std::string geomType =
|
std::string geomType =
|
||||||
_sdf->GetElement("geometry")->GetFirstElement()->GetName();
|
_sdf->GetElement("geometry")->GetFirstElement()->GetName();
|
||||||
|
|
||||||
if (geomType == "heightmap" || geomType == "map")
|
if (geomType == "heightmap" || geomType == "map")
|
||||||
this->SetStatic(true);
|
this->SetStatic(true);
|
||||||
|
|
||||||
collision = this->GetWorld()->GetPhysicsEngine()->CreateCollision(geomType,
|
collision = this->GetWorld()->GetPhysicsEngine()->CreateCollision(geomType,
|
||||||
boost::static_pointer_cast<Link>(shared_from_this()));
|
boost::static_pointer_cast<Link>(shared_from_this()));
|
||||||
|
|
||||||
if (!collision)
|
if (!collision)
|
||||||
gzthrow("Unknown Collisionetry Type[" + geomType + "]");
|
gzthrow("Unknown Collisionetry Type[" + geomType + "]");
|
||||||
|
@ -683,7 +680,7 @@ math::Vector3 Link::GetRelativeLinearVel() const
|
||||||
math::Vector3 Link::GetRelativeAngularVel() const
|
math::Vector3 Link::GetRelativeAngularVel() const
|
||||||
{
|
{
|
||||||
return this->GetWorldPose().rot.RotateVectorReverse(
|
return this->GetWorldPose().rot.RotateVectorReverse(
|
||||||
this->GetWorldAngularVel());
|
this->GetWorldAngularVel());
|
||||||
}
|
}
|
||||||
|
|
||||||
//////////////////////////////////////////////////
|
//////////////////////////////////////////////////
|
||||||
|
@ -702,7 +699,7 @@ math::Vector3 Link::GetWorldLinearAccel() const
|
||||||
math::Vector3 Link::GetRelativeAngularAccel() const
|
math::Vector3 Link::GetRelativeAngularAccel() const
|
||||||
{
|
{
|
||||||
return this->GetWorldPose().rot.RotateVectorReverse(
|
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
|
// L: angular momentum of CoG in world frame
|
||||||
// w: angular velocity in world frame
|
// w: angular velocity in world frame
|
||||||
// return I^-1 * (T - w x L)
|
// return I^-1 * (T - w x L)
|
||||||
return this->GetWorldInertiaMatrix().Inverse() * (this->GetWorldTorque()
|
return this->GetWorldInertiaMatrix().Inverse() * (this->GetWorldTorque() - this->GetWorldAngularVel().Cross(this->GetWorldAngularMomentum()));
|
||||||
- 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";
|
gzwarn << "Link::SetMass is empty\n";
|
||||||
}
|
}
|
||||||
|
@ -813,8 +809,8 @@ void Link::AddChildJoint(JointPtr _joint)
|
||||||
void Link::RemoveParentJoint(const std::string &_jointName)
|
void Link::RemoveParentJoint(const std::string &_jointName)
|
||||||
{
|
{
|
||||||
for (std::vector<JointPtr>::iterator iter = this->parentJoints.begin();
|
for (std::vector<JointPtr>::iterator iter = this->parentJoints.begin();
|
||||||
iter != this->parentJoints.end();
|
iter != this->parentJoints.end();
|
||||||
++iter)
|
++iter)
|
||||||
{
|
{
|
||||||
/// @todo: can we assume there are no repeats?
|
/// @todo: can we assume there are no repeats?
|
||||||
if ((*iter)->GetName() == _jointName)
|
if ((*iter)->GetName() == _jointName)
|
||||||
|
@ -829,8 +825,8 @@ void Link::RemoveParentJoint(const std::string &_jointName)
|
||||||
void Link::RemoveChildJoint(const std::string &_jointName)
|
void Link::RemoveChildJoint(const std::string &_jointName)
|
||||||
{
|
{
|
||||||
for (std::vector<JointPtr>::iterator iter = this->childJoints.begin();
|
for (std::vector<JointPtr>::iterator iter = this->childJoints.begin();
|
||||||
iter != this->childJoints.end();
|
iter != this->childJoints.end();
|
||||||
++iter)
|
++iter)
|
||||||
{
|
{
|
||||||
/// @todo: can we assume there are no repeats?
|
/// @todo: can we assume there are no repeats?
|
||||||
if ((*iter)->GetName() == _jointName)
|
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_iyz(this->inertial->GetIYZ());
|
||||||
_msg.mutable_inertial()->set_izz(this->inertial->GetIZZ());
|
_msg.mutable_inertial()->set_izz(this->inertial->GetIZZ());
|
||||||
msgs::Set(_msg.mutable_inertial()->mutable_pose(),
|
msgs::Set(_msg.mutable_inertial()->mutable_pose(),
|
||||||
this->inertial->GetPose().Ign());
|
this->inertial->GetPose().Ign());
|
||||||
|
|
||||||
for (auto &child : this->children)
|
for (auto &child : this->children)
|
||||||
{
|
{
|
||||||
|
@ -901,7 +897,7 @@ void Link::FillMsg(msgs::Link &_msg)
|
||||||
this->UpdateVisualMsg();
|
this->UpdateVisualMsg();
|
||||||
|
|
||||||
for (Visuals_M::iterator iter = this->visuals.begin();
|
for (Visuals_M::iterator iter = this->visuals.begin();
|
||||||
iter != this->visuals.end(); ++iter)
|
iter != this->visuals.end(); ++iter)
|
||||||
{
|
{
|
||||||
msgs::Visual *vis = _msg.add_visual();
|
msgs::Visual *vis = _msg.add_visual();
|
||||||
vis->CopyFrom(iter->second);
|
vis->CopyFrom(iter->second);
|
||||||
|
@ -976,7 +972,7 @@ void Link::ProcessMsg(const msgs::Link &_msg)
|
||||||
if (coll)
|
if (coll)
|
||||||
coll->ProcessMsg(_msg.collision(i));
|
coll->ProcessMsg(_msg.collision(i));
|
||||||
}
|
}
|
||||||
if (_msg.collision_size()>0)
|
if (_msg.collision_size() > 0)
|
||||||
this->UpdateSurface();
|
this->UpdateSurface();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1015,8 +1011,8 @@ void Link::DetachStaticModel(const std::string &_modelName)
|
||||||
{
|
{
|
||||||
if (this->attachedModels[i]->GetName() == _modelName)
|
if (this->attachedModels[i]->GetName() == _modelName)
|
||||||
{
|
{
|
||||||
this->attachedModels.erase(this->attachedModels.begin()+i);
|
this->attachedModels.erase(this->attachedModels.begin() + i);
|
||||||
this->attachedModelsOffset.erase(this->attachedModelsOffset.begin()+i);
|
this->attachedModelsOffset.erase(this->attachedModelsOffset.begin() + i);
|
||||||
break;
|
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();
|
std::string topic = "~/" + this->GetScopedName();
|
||||||
this->dataPub = this->node->Advertise<msgs::LinkData>(topic);
|
this->dataPub = this->node->Advertise<msgs::LinkData>(topic);
|
||||||
this->connections.push_back(
|
this->connections.push_back(
|
||||||
event::Events::ConnectWorldUpdateEnd(
|
event::Events::ConnectWorldUpdateEnd(
|
||||||
boost::bind(&Link::PublishData, this)));
|
boost::bind(&Link::PublishData, this)));
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
@ -1128,9 +1124,9 @@ void Link::PublishData()
|
||||||
msgs::Set(this->linkDataMsg.mutable_time(), this->world->GetSimTime());
|
msgs::Set(this->linkDataMsg.mutable_time(), this->world->GetSimTime());
|
||||||
linkDataMsg.set_name(this->GetScopedName());
|
linkDataMsg.set_name(this->GetScopedName());
|
||||||
msgs::Set(this->linkDataMsg.mutable_linear_velocity(),
|
msgs::Set(this->linkDataMsg.mutable_linear_velocity(),
|
||||||
this->GetWorldLinearVel().Ign());
|
this->GetWorldLinearVel().Ign());
|
||||||
msgs::Set(this->linkDataMsg.mutable_angular_velocity(),
|
msgs::Set(this->linkDataMsg.mutable_angular_velocity(),
|
||||||
this->GetWorldAngularVel().Ign());
|
this->GetWorldAngularVel().Ign());
|
||||||
this->dataPub->Publish(this->linkDataMsg);
|
this->dataPub->Publish(this->linkDataMsg);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -1227,7 +1223,7 @@ bool Link::SetVisualPose(const uint32_t _id,
|
||||||
while (visualElem)
|
while (visualElem)
|
||||||
{
|
{
|
||||||
std::string visName = linkName + "::" +
|
std::string visName = linkName + "::" +
|
||||||
visualElem->Get<std::string>("name");
|
visualElem->Get<std::string>("name");
|
||||||
|
|
||||||
// update visual msg if it exists
|
// update visual msg if it exists
|
||||||
if (msg.name() == visName)
|
if (msg.name() == visName)
|
||||||
|
@ -1266,12 +1262,13 @@ void Link::OnCollision(ConstContactsPtr &_msg)
|
||||||
GZ_ASSERT(pos1 != std::string::npos, "Invalid collision name");
|
GZ_ASSERT(pos1 != std::string::npos, "Invalid collision name");
|
||||||
GZ_ASSERT(pos2 != std::string::npos, "Invalid collision name");
|
GZ_ASSERT(pos2 != std::string::npos, "Invalid collision name");
|
||||||
|
|
||||||
collisionName1 = collisionName1.substr(pos1+2);
|
collisionName1 = collisionName1.substr(pos1 + 2);
|
||||||
collisionName2 = collisionName2.substr(pos2+2);
|
collisionName2 = collisionName2.substr(pos2 + 2);
|
||||||
|
|
||||||
#ifdef HAVE_OPENAL
|
#ifdef HAVE_OPENAL
|
||||||
for (std::vector<util::OpenALSourcePtr>::iterator iter =
|
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) ||
|
if ((*iter)->HasCollisionName(collisionName1) ||
|
||||||
(*iter)->HasCollisionName(collisionName2))
|
(*iter)->HasCollisionName(collisionName2))
|
||||||
|
@ -1351,7 +1348,7 @@ void Link::UpdateVisualGeomSDF(const math::Vector3 &_scale)
|
||||||
math::Vector3 size =
|
math::Vector3 size =
|
||||||
geomElem->GetElement("box")->Get<math::Vector3>("size");
|
geomElem->GetElement("box")->Get<math::Vector3>("size");
|
||||||
geomElem->GetElement("box")->GetElement("size")->Set(
|
geomElem->GetElement("box")->GetElement("size")->Set(
|
||||||
_scale/this->scale*size);
|
_scale / this->scale * size);
|
||||||
}
|
}
|
||||||
else if (geomElem->HasElement("sphere"))
|
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 radius = geomElem->GetElement("sphere")->Get<double>("radius");
|
||||||
double newRadius = std::max(_scale.z, std::max(_scale.x, _scale.y));
|
double newRadius = std::max(_scale.z, std::max(_scale.x, _scale.y));
|
||||||
double oldRadius = std::max(this->scale.Z(),
|
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(
|
geomElem->GetElement("sphere")->GetElement("radius")->Set(
|
||||||
newRadius/oldRadius*radius);
|
newRadius / oldRadius * radius);
|
||||||
}
|
}
|
||||||
else if (geomElem->HasElement("cylinder"))
|
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 oldRadius = std::max(this->scale.X(), this->scale.Y());
|
||||||
|
|
||||||
double length = geomElem->GetElement("cylinder")->Get<double>("length");
|
double length = geomElem->GetElement("cylinder")->Get<double>("length");
|
||||||
geomElem->GetElement("cylinder")->GetElement("radius")->Set(
|
geomElem->GetElement("cylinder")->GetElement("radius")->Set(newRadius / oldRadius * radius);
|
||||||
newRadius/oldRadius*radius);
|
geomElem->GetElement("cylinder")->GetElement("length")->Set(_scale.z / this->scale.Z() * length);
|
||||||
geomElem->GetElement("cylinder")->GetElement("length")->Set(
|
|
||||||
_scale.z/this->scale.Z()*length);
|
|
||||||
}
|
}
|
||||||
else if (geomElem->HasElement("mesh"))
|
else if (geomElem->HasElement("mesh"))
|
||||||
geomElem->GetElement("mesh")->GetElement("scale")->Set(_scale);
|
geomElem->GetElement("mesh")->GetElement("scale")->Set(_scale);
|
||||||
|
@ -1402,7 +1397,7 @@ void Link::UpdateVisualMsg()
|
||||||
for (auto &iter : this->visuals)
|
for (auto &iter : this->visuals)
|
||||||
{
|
{
|
||||||
std::string visName = linkName + "::" +
|
std::string visName = linkName + "::" +
|
||||||
visualElem->Get<std::string>("name");
|
visualElem->Get<std::string>("name");
|
||||||
if (iter.second.name() == visName)
|
if (iter.second.name() == visName)
|
||||||
{
|
{
|
||||||
iter.second.mutable_geometry()->CopyFrom(msg.geometry());
|
iter.second.mutable_geometry()->CopyFrom(msg.geometry());
|
||||||
|
@ -1424,7 +1419,7 @@ void Link::UpdateVisualMsg()
|
||||||
|
|
||||||
auto iter = this->visuals.find(msg.id());
|
auto iter = this->visuals.find(msg.id());
|
||||||
if (iter != this->visuals.end())
|
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;
|
this->visuals[msg.id()] = msg;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -1479,15 +1474,14 @@ double Link::GetWorldEnergy() const
|
||||||
void Link::MoveFrame(const math::Pose &_worldReferenceFrameSrc,
|
void Link::MoveFrame(const math::Pose &_worldReferenceFrameSrc,
|
||||||
const math::Pose &_worldReferenceFrameDst)
|
const math::Pose &_worldReferenceFrameDst)
|
||||||
{
|
{
|
||||||
math::Pose targetWorldPose = (this->GetWorldPose() - _worldReferenceFrameSrc)
|
math::Pose targetWorldPose = (this->GetWorldPose() - _worldReferenceFrameSrc) + _worldReferenceFrameDst;
|
||||||
+ _worldReferenceFrameDst;
|
|
||||||
this->SetWorldPose(targetWorldPose);
|
this->SetWorldPose(targetWorldPose);
|
||||||
this->SetWorldTwist(math::Vector3(0, 0, 0), math::Vector3(0, 0, 0));
|
this->SetWorldTwist(math::Vector3(0, 0, 0), math::Vector3(0, 0, 0));
|
||||||
}
|
}
|
||||||
|
|
||||||
/////////////////////////////////////////////////
|
/////////////////////////////////////////////////
|
||||||
bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
|
bool Link::FindAllConnectedLinksHelper(const LinkPtr &_originalParentLink,
|
||||||
Link_V &_connectedLinks, bool _fistLink)
|
Link_V &_connectedLinks, bool _fistLink)
|
||||||
{
|
{
|
||||||
// debug
|
// debug
|
||||||
// std::string pn;
|
// 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
|
// loop through all joints where this link is a parent link of the joint
|
||||||
for (Link_V::iterator li = childLinks.begin();
|
for (Link_V::iterator li = childLinks.begin();
|
||||||
li != childLinks.end(); ++li)
|
li != childLinks.end(); ++li)
|
||||||
{
|
{
|
||||||
// gzerr << "debug: checking " << (*li)->GetName() << "\n";
|
// 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
|
// if it returns false, it looped back to parent, mark flag and break
|
||||||
// from current for-loop.
|
// from current for-loop.
|
||||||
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
|
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
|
||||||
_connectedLinks))
|
_connectedLinks))
|
||||||
{
|
{
|
||||||
// one of the recursed link is the parent link
|
// one of the recursed link is the parent link
|
||||||
return false;
|
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
|
// loop through all joints where this link is a parent link of the joint
|
||||||
for (Link_V::iterator li = parentLinks.begin();
|
for (Link_V::iterator li = parentLinks.begin();
|
||||||
li != parentLinks.end(); ++li)
|
li != parentLinks.end(); ++li)
|
||||||
{
|
{
|
||||||
// check child link of each child joint recursively
|
// check child link of each child joint recursively
|
||||||
if ((*li).get() == _originalParentLink.get())
|
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
|
// if it returns false, it looped back to parent, mark flag and break
|
||||||
// from current for-loop.
|
// from current for-loop.
|
||||||
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
|
if (!(*li)->FindAllConnectedLinksHelper(_originalParentLink,
|
||||||
_connectedLinks))
|
_connectedLinks))
|
||||||
{
|
{
|
||||||
// one of the recursed link is the parent link
|
// one of the recursed link is the parent link
|
||||||
return false;
|
return false;
|
||||||
|
|
|
@ -66,6 +66,10 @@ namespace gazebo
|
||||||
class LightState;
|
class LightState;
|
||||||
class LinkState;
|
class LinkState;
|
||||||
class JointState;
|
class JointState;
|
||||||
|
// Added by zhangshuai 2019.03.28 ----Begin
|
||||||
|
class GazeboID;
|
||||||
|
class Distribution;
|
||||||
|
// Added by zhangshuai 2019.03.28 ----End
|
||||||
|
|
||||||
/// \def BasePtr
|
/// \def BasePtr
|
||||||
/// \brief Boost shared pointer to a Base object
|
/// \brief Boost shared pointer to a Base object
|
||||||
|
@ -235,6 +239,24 @@ namespace gazebo
|
||||||
/// \brief Map of joint state
|
/// \brief Map of joint state
|
||||||
typedef std::map<std::string, JointState> JointState_M;
|
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
|
#ifndef GZ_COLLIDE_BITS
|
||||||
|
|
||||||
/// \def GZ_ALL_COLLIDE
|
/// \def GZ_ALL_COLLIDE
|
||||||
|
|
|
@ -84,6 +84,10 @@
|
||||||
#include "gazebo/physics/ContactManager.hh"
|
#include "gazebo/physics/ContactManager.hh"
|
||||||
#include "gazebo/physics/Population.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 gazebo;
|
||||||
using namespace physics;
|
using namespace physics;
|
||||||
|
|
||||||
|
@ -91,6 +95,22 @@ using namespace physics;
|
||||||
/// This will be replaced with a class member variable in Gazebo 3.0
|
/// This will be replaced with a class member variable in Gazebo 3.0
|
||||||
bool g_clearModels;
|
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
|
class ModelUpdate_TBB
|
||||||
{
|
{
|
||||||
public:
|
public:
|
||||||
|
@ -253,15 +273,30 @@ void World::Load(sdf::ElementPtr _sdf)
|
||||||
"~/factory/light");
|
"~/factory/light");
|
||||||
|
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
// 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");
|
sdf::ElementPtr distributionElem = this->dataPtr->sdf->GetElement("distribution");
|
||||||
type_ = eventElem->Get<int>("type");
|
|
||||||
ip = eventElem->Get<std::string>("ip");
|
gazeboLocalID = distributionElem->Get<unsigned int>("gazebo_local_ID");
|
||||||
port = eventElem->Get<int>("port");
|
port = distributionElem->Get<int>("port");
|
||||||
flag = eventElem->Get<int>("flag");
|
flag = distributionElem->Get<int>("flag");
|
||||||
simula_entity_num = eventElem->Get<int>("simula_entity_num");
|
|
||||||
shadow_entity_num = eventElem->Get<int>("shadow_entity_num");
|
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
|
//Added by zhangshuai based on zenglei 2019.03.19 ----End
|
||||||
|
|
||||||
|
@ -419,18 +454,16 @@ void World::Run(unsigned int _iterations)
|
||||||
this->dataPtr->stopIterations = _iterations;
|
this->dataPtr->stopIterations = _iterations;
|
||||||
|
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
// 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 (1 == flag)
|
||||||
{
|
{
|
||||||
if (0 == type_)
|
if (0 == this->gazeboLocalID)
|
||||||
{
|
{
|
||||||
gazeboZero.SetPort(port);
|
gazeboZero.SetPort(port);
|
||||||
gazeboZero.StartServer();
|
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.SetPort(port);
|
||||||
gazeboOne.ConnectServer();
|
gazeboOne.ConnectServer();
|
||||||
}
|
}
|
||||||
|
@ -519,11 +552,11 @@ void World::RunLoop()
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
||||||
if (1 == flag)
|
if (1 == flag)
|
||||||
{
|
{
|
||||||
if (0 == type_)
|
if (0 == this->gazeboLocalID)
|
||||||
{
|
{
|
||||||
gazeboZero.Close();
|
gazeboZero.Close();
|
||||||
}
|
}
|
||||||
else if (1 == type_)
|
else if (1 == this->gazeboLocalID)
|
||||||
{
|
{
|
||||||
gazeboOne.Close();
|
gazeboOne.Close();
|
||||||
}
|
}
|
||||||
|
@ -720,6 +753,18 @@ void World::Step(unsigned int _steps)
|
||||||
//////////////////////////////////////////////////
|
//////////////////////////////////////////////////
|
||||||
void World::Update()
|
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");
|
DIAG_TIMER_START("World::Update");
|
||||||
|
|
||||||
if (this->dataPtr->needsReset)
|
if (this->dataPtr->needsReset)
|
||||||
|
@ -737,20 +782,77 @@ void World::Update()
|
||||||
|
|
||||||
this->dataPtr->updateInfo.simTime = this->GetSimTime();
|
this->dataPtr->updateInfo.simTime = this->GetSimTime();
|
||||||
this->dataPtr->updateInfo.realTime = this->GetRealTime();
|
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);
|
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");
|
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
|
// Update all the models
|
||||||
(*this.*dataPtr->modelUpdateFunc)();
|
(*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");
|
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 must be called before PhysicsEngine::UpdatePhysics.
|
||||||
this->dataPtr->physicsEngine->UpdateCollision();
|
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");
|
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.
|
// Wait for logging to finish, if it's running.
|
||||||
if (util::LogRecord::Instance()->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
|
// Give clients a possibility to react to collisions before the physics
|
||||||
// gets updated.
|
// gets updated.
|
||||||
this->dataPtr->updateInfo.realTime = this->GetRealTime();
|
this->dataPtr->updateInfo.realTime = this->GetRealTime();
|
||||||
event::Events::beforePhysicsUpdate(this->dataPtr->updateInfo);
|
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");
|
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
|
// Update the physics engine
|
||||||
if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
|
if (this->dataPtr->enablePhysicsEngine && this->dataPtr->physicsEngine)
|
||||||
{
|
{
|
||||||
|
@ -800,102 +930,133 @@ void World::Update()
|
||||||
DIAG_TIMER_LAP("World::Update", "SetWorldPose(dirtyPoses)");
|
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
|
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
||||||
if (1 == flag)
|
if (1 == flag)
|
||||||
{
|
{
|
||||||
// if (this->dataPtr->iterations % 10 == 0)
|
// 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;
|
std::string tmp_model_name = this->distribution->GetGazeboIDPtr(gazeboLocalID)->GetModelName(i);
|
||||||
ss << k;
|
unsigned int j = 0;
|
||||||
std::string ks = ss.str();
|
for (j = 0; j < this->GetModel(tmp_model_name)->GetLink("base_link")->GetName().length(); j++)
|
||||||
std::string tmp_model_name = "bebop_" + ks;
|
infor[i].link_name[j] = this->GetModel(tmp_model_name)->GetLink("base_link")->GetName()[j];
|
||||||
// std::cout << "model_name:" << tmp_model_name << std::endl;
|
infor[i].link_name[j] = '\0';
|
||||||
for (i = 0; i < this->GetModel(tmp_model_name)->GetLink("base_link")->GetName().length(); i++)
|
infor[i].ID = this->GetModel(tmp_model_name)->GetLink("base_link")->GetId();
|
||||||
infor[k].link_name[i] = this->GetModel(tmp_model_name)->GetLink("base_link")->GetName()[i];
|
infor[i].link_pose = this->GetModel(tmp_model_name)->GetLink("base_link")->GetWorldPose();
|
||||||
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;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
CommunicationData temp[50];
|
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;
|
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;
|
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;
|
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;
|
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.
|
// Only update state information if logging data.
|
||||||
if (util::LogRecord::Instance()->Running())
|
if (util::LogRecord::Instance()->Running())
|
||||||
|
@ -910,6 +1071,28 @@ void World::Update()
|
||||||
event::Events::worldUpdateEnd();
|
event::Events::worldUpdateEnd();
|
||||||
|
|
||||||
DIAG_TIMER_STOP("World::Update");
|
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)
|
if (model != nullptr)
|
||||||
{
|
{
|
||||||
model->Init();
|
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 (...)
|
catch (...)
|
||||||
|
@ -3095,11 +3289,18 @@ std::string World::UniqueModelName(const std::string &_name)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Added by zhangshuai 2019.04.02 ----Begin
|
||||||
//////////////////////////////////////////////////
|
//////////////////////////////////////////////////
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----Begin
|
/// add for read the element of "distribution"
|
||||||
/// add for Tcp communication
|
DistributionPtr World::GetDistribution()
|
||||||
// unsigned int World::GetLengthOfCommunicationData()
|
{
|
||||||
// {
|
return this->distribution;
|
||||||
// return sizeof(infor);
|
}
|
||||||
// }
|
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----End
|
//////////////////////////////////////////////////
|
||||||
|
/// add for read the element of "gazeboLocalID"
|
||||||
|
unsigned int World::GetGazeboLocalID()
|
||||||
|
{
|
||||||
|
return this->gazeboLocalID;
|
||||||
|
}
|
||||||
|
// Added by zhangshuai 2019.04.02 ----End
|
|
@ -52,6 +52,10 @@
|
||||||
#include "gazebo/physics/TcpCommunication.hh"
|
#include "gazebo/physics/TcpCommunication.hh"
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----End
|
// 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 gazebo
|
||||||
{
|
{
|
||||||
namespace physics
|
namespace physics
|
||||||
|
@ -716,11 +720,11 @@ private:
|
||||||
private:
|
private:
|
||||||
TcpServer gazeboZero;
|
TcpServer gazeboZero;
|
||||||
|
|
||||||
private:
|
// private:
|
||||||
std::string ip;
|
// std::string ip;
|
||||||
|
|
||||||
private:
|
// private:
|
||||||
int type_; //0:server:gazeboZero; 1:client:gazeboOne;
|
// int type_; //0:server:gazeboZero; 1:client:gazeboOne;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
int port;
|
int port;
|
||||||
|
@ -728,18 +732,32 @@ private:
|
||||||
private:
|
private:
|
||||||
int flag;
|
int flag;
|
||||||
|
|
||||||
private:
|
// private:
|
||||||
unsigned int simula_entity_num;
|
// unsigned int simula_entity_num;
|
||||||
|
|
||||||
private:
|
// private:
|
||||||
unsigned int shadow_entity_num;
|
// unsigned int shadow_entity_num;
|
||||||
|
|
||||||
private:
|
private:
|
||||||
CommunicationData infor[50];
|
CommunicationData infor[50];
|
||||||
|
|
||||||
// public:
|
|
||||||
// unsigned int GetLengthOfCommunicationData();
|
|
||||||
// Added by zhangshuai based on zenglei 2019.03.19 ----End
|
// 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
|
} // namespace physics
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
# This file currently only serves to mark the location of a catkin workspace for tool integration
|
|
@ -0,0 +1,2 @@
|
||||||
|
build
|
||||||
|
devel
|
|
@ -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
|
|
@ -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"
|
||||||
|
|
||||||
|
|
||||||
|
|
|
@ -0,0 +1 @@
|
||||||
|
/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake
|
|
@ -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.
|
|
@ -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})
|
||||||
|
|
|
@ -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"))
|
|
@ -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"))
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float c_error_;
|
||||||
|
float c_integrator_;
|
||||||
|
|
||||||
|
float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts);
|
||||||
|
float r_error_;
|
||||||
|
float r_integrator;
|
||||||
|
|
||||||
|
float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts);
|
||||||
|
float p_error_;
|
||||||
|
float p_integrator_;
|
||||||
|
|
||||||
|
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float at_error_;
|
||||||
|
float at_integrator_;
|
||||||
|
float at_differentiator_;
|
||||||
|
|
||||||
|
float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||||
|
float a_error_;
|
||||||
|
float a_integrator_;
|
||||||
|
float a_differentiator_;
|
||||||
|
|
||||||
|
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, 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
|
|
@ -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 ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float c_error_;
|
||||||
|
float c_integrator_;
|
||||||
|
|
||||||
|
float roll_hold(float phi_c, float phi, float p, const struct params_s ¶ms, float Ts);
|
||||||
|
float r_error_;
|
||||||
|
float r_integrator;
|
||||||
|
|
||||||
|
float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, float Ts);
|
||||||
|
float p_error_;
|
||||||
|
float p_integrator_;
|
||||||
|
|
||||||
|
float airspeed_with_pitch_hold(float Va_c, float Va, const struct params_s ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float decend_airspeed_oil(float h_c, float h, const params_s ¶ms, float Ts);
|
||||||
|
|
||||||
|
float airspeed_with_throttle_hold(float Va_c, float Va, const struct params_s ¶ms, float Ts);
|
||||||
|
float at_error_;
|
||||||
|
float at_integrator_;
|
||||||
|
float at_differentiator_;
|
||||||
|
|
||||||
|
float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||||
|
float a_error_;
|
||||||
|
float a_integrator_;
|
||||||
|
float a_differentiator_;
|
||||||
|
|
||||||
|
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, 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
|
|
@ -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 ¶ms, 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
|
|
@ -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 //
|
|
@ -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 //
|
|
@ -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 //
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, const struct input_s &input, struct output_s &output);
|
||||||
|
};
|
||||||
|
|
||||||
|
} //end namespace
|
||||||
|
#endif // PATH_FOLLOWER_EXAMPLE_H
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, const struct input_s &input, struct output_s &output);
|
||||||
|
|
||||||
|
void manage_line(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||||
|
void manage_star(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);//add by kobe
|
||||||
|
void manage_fillet(const struct params_s ¶ms, const struct input_s &input, struct output_s &output);
|
||||||
|
fillet_state fil_state_;
|
||||||
|
void manage_dubins(const struct params_s ¶ms, 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
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, 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
|
|
@ -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 ¶ms, 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 ¶ms, float Ts);
|
||||||
|
//float course_hold(float chi_c, float chi, float phi_ff, float r, const struct params_s ¶ms, 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 ¶ms, float Ts);
|
||||||
|
//float r_error_;
|
||||||
|
//float r_integrator_;
|
||||||
|
|
||||||
|
//float pitch_hold(float theta_c, float theta, float q, const struct params_s ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float pseudo_pitch_hold(float h_c, float h, const struct params_s ¶ms, 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 ¶ms, float Ts);
|
||||||
|
float pseudo_throttle_hold(float Va_c, float Va, const params_s ¶ms, float Ts);
|
||||||
|
float at_error_;
|
||||||
|
float at_integrator_;
|
||||||
|
float at_differentiator_;
|
||||||
|
|
||||||
|
//float altitiude_hold(float h_c, float h, const struct params_s ¶ms, float Ts);
|
||||||
|
float pseudo_altitiude_hold(float h_c, float h, const params_s ¶ms, float Ts);
|
||||||
|
float a_error_;
|
||||||
|
float a_integrator_;
|
||||||
|
float a_differentiator_;
|
||||||
|
|
||||||
|
// float cooridinated_turn_hold(float v, const struct params_s ¶ms, 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
|
|
@ -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
|
|
@ -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
|
|
@ -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>
|
|
@ -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
|
|
@ -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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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
|
|
@ -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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 ¶ms, 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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 ¶ms, 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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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 ¶ms, 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
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
|
@ -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;
|
||||||
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
}
|
|
@ -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
|
||||||
|
)
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -0,0 +1,5 @@
|
||||||
|
int32 status_word
|
||||||
|
float32 longitude
|
||||||
|
float32 latitude
|
||||||
|
float32 altitude
|
||||||
|
float32 plane_speed #desired speed
|
|
@ -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)
|
|
@ -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)
|
||||||
|
|
|
@ -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)
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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>
|
|
@ -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}
|
|
@ -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
|
|
@ -0,0 +1,4 @@
|
||||||
|
cmake_minimum_required(VERSION 2.8.3)
|
||||||
|
project(hector_quadrotor)
|
||||||
|
find_package(catkin REQUIRED)
|
||||||
|
catkin_metapackage()
|
|
@ -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>
|
|
@ -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.
|
|
@ -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}
|
||||||
|
)
|
||||||
|
|
|
@ -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
|
|
@ -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 ¶m_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
|
|
@ -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
|
|
@ -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>
|
|
@ -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>
|
|
@ -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
|
|
@ -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>
|
|
@ -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)
|
|
@ -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 ¶m_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
|
||||||
|
|
|
@ -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)
|
|
@ -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
|
|
@ -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)
|
|
@ -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)
|
|
@ -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
|
|
@ -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)
|
|
@ -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
|
|
@ -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>
|
|
@ -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
Loading…
Reference in New Issue