jasder
/
XTDrone
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

fix more than 1 gimbal control problem

This commit is contained in:
Peter 2020-10-12 11:03:12 +08:00
parent 8df18e6179
commit 6d36b05f53
2 changed files with 650 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

649
sensing/gimbal/gazebo_gimbal_controller_plugin.cpp Normal file
View File

@ -0,0 +1,649 @@
/*
* Copyright (C) 2012-2015 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo_gimbal_controller_plugin.hh>
using namespace gazebo;
using namespace std;
GZ_REGISTER_MODEL_PLUGIN(GimbalControllerPlugin)
/* Keep these functions in the 'detail' namespace so that they
* can be called from unit tests. */
namespace detail {
/////////////////////////////////////////////////
ignition::math::Vector3d ThreeAxisRot(
double r11, double r12, double r21, double r31, double r32)
{
return ignition::math::Vector3d(
atan2( r31, r32 ),
asin ( r21 ),
atan2( r11, r12 ));
}
/////////////////////////////////////////////////
/// \TODO something to move into Angle class
/// \brief returns _angle1 normalized about
/// (_reference - M_PI, _reference + M_PI]
/// \param[in] _angle1 input angle
/// \param[in] _reference reference input angle for normalization
/// \return normalized _angle1 about _reference
double NormalizeAbout(double _angle, double reference)
{
double diff = _angle - reference;
// normalize diff about (-pi, pi], then add reference
while (diff <= -M_PI)
{
diff += 2.0*M_PI;
}
while (diff > M_PI)
{
diff -= 2.0*M_PI;
}
return diff + reference;
}
/////////////////////////////////////////////////
/// \TODO something to move into Angle class
/// \brief returns shortest angular distance from _from to _to
/// \param[in] _from starting anglular position
/// \param[in] _to end angular position
/// \return distance traveled from starting to end angular positions
double ShortestAngularDistance(double _from, double _to)
{
return NormalizeAbout(_to, _from) - _from;
}
/////////////////////////////////////////////////
ignition::math::Vector3d QtoZXY(
const ignition::math::Quaterniond &_q)
{
// taken from
// http://bediyap.com/programming/convert-quaternion-to-euler-rotations/
// case zxy:
ignition::math::Vector3d result = detail::ThreeAxisRot(
-2*(_q.X()*_q.Y() - _q.W()*_q.Z()),
_q.W()*_q.W() - _q.X()*_q.X() + _q.Y()*_q.Y() - _q.Z()*_q.Z(),
2*(_q.Y()*_q.Z() + _q.W()*_q.X()),
-2*(_q.X()*_q.Z() - _q.W()*_q.Y()),
_q.W()*_q.W() - _q.X()*_q.X() - _q.Y()*_q.Y() + _q.Z()*_q.Z());
return result;
}
}
/////////////////////////////////////////////////
GimbalControllerPlugin::GimbalControllerPlugin()
:status("closed")
{
/// defaults if sdf xml doesn't contain any pid gains
this->pitchPid.Init(kPIDPitchP, kPIDPitchI, kPIDPitchD, kPIDPitchIMax, kPIDPitchIMin, kPIDPitchCmdMax, kPIDPitchCmdMin);
this->rollPid.Init(kPIDRollP, kPIDRollI, kPIDRollD, kPIDRollIMax, kPIDRollIMin, kPIDRollCmdMax, kPIDRollCmdMin);
this->yawPid.Init(kPIDYawP, kPIDYawI, kPIDYawD, kPIDYawIMax, kPIDYawIMin, kPIDYawCmdMax, kPIDYawCmdMin);
this->pitchCommand = 0.5* M_PI;
this->rollCommand = 0;
this->yawCommand = 0;
this->lastImuYaw = 0;
this->rDir = kRollDir;
this->pDir = kPitchDir;
this->yDir = kYawDir;
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::Load(physics::ModelPtr _model,
sdf::ElementPtr _sdf)
{
this->model = _model;
this->sdf = _sdf;
// Create axis name -> pid map. It may be empty or not fully defined
std::map<std::string, common::PID> pids_;
if (_sdf->HasElement("control_gimbal_channels"))
{
sdf::ElementPtr control_channels = _sdf->GetElement("control_gimbal_channels");
sdf::ElementPtr channel = control_channels->GetElement("channel");
while(channel)
{
if (channel->HasElement("joint_axis"))
{
std::string joint_axis = channel->Get<std::string>("joint_axis");
// setup joint control pid to control joint
if (channel->HasElement("joint_control_pid"))
{
sdf::ElementPtr pid = channel->GetElement("joint_control_pid");
double p = 0;
if (pid->HasElement("p"))
p = pid->Get<double>("p");
double i = 0;
if (pid->HasElement("i"))
i = pid->Get<double>("i");
double d = 0;
if (pid->HasElement("d"))
d = pid->Get<double>("d");
double iMax = 0;
if (pid->HasElement("iMax"))
iMax = pid->Get<double>("iMax");
double iMin = 0;
if (pid->HasElement("iMin"))
iMin = pid->Get<double>("iMin");
double cmdMax = 0;
if (pid->HasElement("cmdMax"))
cmdMax = pid->Get<double>("cmdMax");
double cmdMin = 0;
if (pid->HasElement("cmdMin"))
cmdMin = pid->Get<double>("cmdMin");
// insert pid gains into map for the respective named joint axis
pids_.insert(std::pair<std::string, common::PID>(joint_axis, common::PID(p, i, d, iMax, iMin, cmdMax, cmdMin)));
}
channel = channel->GetNextElement("channel");
}
}
}
else
{
gzwarn << "Control channels for gimbal not found. Using default pid gains\n";
}
std::string yawJointName = "cgo3_vertical_arm_joint";
this->yawJoint = this->model->GetJoint(yawJointName);
if (this->sdf->HasElement("joint_yaw"))
{
// Add names to map
yawJointName = sdf->Get<std::string>("joint_yaw");
if (this->model->GetJoint(yawJointName))
{
this->yawJoint = this->model->GetJoint(yawJointName);
// Try to find yaw rotation direction
sdf::ElementPtr sdfElem = this->yawJoint->GetSDF();
if(sdfElem->HasElement("axis"))
{
// Rotation is found
#if GAZEBO_MAJOR_VERSION >= 9
yDir = this->yawJoint->LocalAxis(0)[2];
#else
yDir = this->yawJoint->GetLocalAxis(0)[2];
#endif
}
else
{
// If user do not defines axis for yaw joint explicitly
// then display warning
gzwarn << "joint_yaw [" << yawJointName << "] axis do not defined?\n";
}
// Try to find respective pid for the named axis control
std::map<std::string, common::PID>::iterator it = pids_.find("joint_yaw");
if(it != pids_.end())
{
// Found pid for this axis (and therefore for this joint)
this->yawPid = it->second;
}
else
{
// If user defines control channels for gimbal but don't define yaw gains explicitly
// then display warning
gzwarn << "joint_yaw [" << yawJointName << "] pid control gains do not defined?\n";
}
}
else
{
gzwarn << "joint_yaw [" << yawJointName << "] does not exist?\n";
}
}
if (!this->yawJoint)
{
gzerr << "GimbalControllerPlugin::Load ERROR! Can't get yaw joint '"
<< yawJointName << "' " << endl;
}
std::string rollJointName = "cgo3_horizontal_arm_joint";
this->rollJoint = this->model->GetJoint(rollJointName);
if (this->sdf->HasElement("joint_roll"))
{
// Add names to map
rollJointName = sdf->Get<std::string>("joint_roll");
if (this->model->GetJoint(rollJointName))
{
this->rollJoint = this->model->GetJoint(rollJointName);
// Try to find roll rotation direction
sdf::ElementPtr sdfElem = this->rollJoint->GetSDF();
if(sdfElem->HasElement("axis"))
{
// Rotation is found
#if GAZEBO_MAJOR_VERSION >= 9
rDir = this->rollJoint->LocalAxis(0)[0];
#else
rDir = this->rollJoint->GetLocalAxis(0)[0];
#endif
}
else
{
// If user do not defines axis for roll joint explicitly
// then display warning
gzwarn << "joint_roll [" << rollJointName << "] axis do not defined?\n";
}
// Try to find respective pid for the named axis control
std::map<std::string, common::PID>::iterator it = pids_.find("joint_roll");
if(it != pids_.end())
{
// Found pid for this axis (and therefore for this joint)
this->rollPid = it->second;
}
else
{
// If user defines control channels for gimbal but don't define roll gains explicitly
// then display warning
gzwarn << "joint_roll [" << rollJointName << "] pid control gains do not defined?\n";
}
}
else
{
gzwarn << "joint_roll [" << rollJointName << "] does not exist?\n";
}
}
if (!this->rollJoint)
{
gzerr << "GimbalControllerPlugin::Load ERROR! Can't get roll joint '"
<< rollJointName << "' " << endl;
}
std::string pitchJointName = "cgo3_camera_joint";
this->pitchJoint = this->model->GetJoint(pitchJointName);
if (this->sdf->HasElement("joint_pitch"))
{
// Add names to map
pitchJointName = sdf->Get<std::string>("joint_pitch");
if (this->model->GetJoint(pitchJointName))
{
this->pitchJoint = this->model->GetJoint(pitchJointName);
// Try to find pitch rotation direction
sdf::ElementPtr sdfElem = this->pitchJoint->GetSDF();
if(sdfElem->HasElement("axis"))
{
// Rotation is found
#if GAZEBO_MAJOR_VERSION >= 9
pDir = this->pitchJoint->LocalAxis(0)[1];
#else
pDir = this->pitchJoint->GetLocalAxis(0)[1];
#endif
}
else
{
// If user do not defines axis for pitch joint explicitly
// then display warning
gzwarn << "joint_pitch [" << pitchJointName << "] axis do not defined?\n";
}
// Try to find respective pid for the named axis
std::map<std::string, common::PID>::iterator it = pids_.find("joint_pitch");
if(it != pids_.end())
{
// Found pid for this axis (and therefore for this joint)
this->pitchPid = it->second;
}
else
{
// If user defines control channels for gimbal but don't define pitch gains explicitly
// then display warning
gzwarn << "joint_pitch [" << pitchJointName << "] pid control gains do not defined?\n";
}
}
else
{
gzwarn << "joint_pitch [" << pitchJointName << "] does not exist?\n";
}
}
if (!this->pitchJoint)
{
gzerr << "GimbalControllerPlugin::Load ERROR! Can't get pitch joint '"
<< pitchJointName << "' " << endl;
}
// get imu sensors
std::string cameraImuSensorName = "camera_imu";
if (this->sdf->HasElement("gimbal_imu"))
{
// Add names to map
cameraImuSensorName = sdf->Get<std::string>("gimbal_imu");
}
#if GAZEBO_MAJOR_VERSION >= 7
this->cameraImuSensor = std::static_pointer_cast<sensors::ImuSensor>(
sensors::SensorManager::Instance()->GetSensor(_model->SensorScopedName(cameraImuSensorName)[0]));
#elif GAZEBO_MAJOR_VERSION >= 6
this->cameraImuSensor = boost::static_pointer_cast<sensors::ImuSensor>(
sensors::SensorManager::Instance()->GetSensor(cameraImuSensorName));
#endif
if (!this->cameraImuSensor)
{
gzerr << "GimbalControllerPlugin::Load ERROR! Can't get imu sensor '"
<< cameraImuSensorName << "' " << endl;
}
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::Init()
{
this->node = transport::NodePtr(new transport::Node());
#if GAZEBO_MAJOR_VERSION >= 9
this->node->Init(this->model->GetWorld()->Name());
this->lastUpdateTime = this->model->GetWorld()->SimTime();
#else
this->node->Init(this->model->GetWorld()->GetName());
this->lastUpdateTime = this->model->GetWorld()->GetSimTime();
#endif
// receive pitch command via gz transport
std::string pitchTopic = std::string("~/") + this->model->GetName() +
"/gimbal_pitch_cmd";
this->pitchSub = this->node->Subscribe(pitchTopic,
&GimbalControllerPlugin::OnPitchStringMsg, this);
// receive roll command via gz transport
std::string rollTopic = std::string("~/") + this->model->GetName() +
"/gimbal_roll_cmd";
this->rollSub = this->node->Subscribe(rollTopic,
&GimbalControllerPlugin::OnRollStringMsg, this);
// receive yaw command via gz transport
std::string yawTopic = std::string("~/") + this->model->GetName() +
"/gimbal_yaw_cmd";
this->yawSub = this->node->Subscribe(yawTopic,
&GimbalControllerPlugin::OnYawStringMsg, this);
// plugin update
this->connections.push_back(event::Events::ConnectWorldUpdateBegin(
boost::bind(&GimbalControllerPlugin::OnUpdate, this)));
// publish pitch status via gz transport
pitchTopic = std::string("~/") + this->model->GetName()
+ "/gimbal_pitch_status";
// Although gazebo above 7.4 support Any, still use GzString instead
this->pitchPub = node->Advertise<gazebo::msgs::GzString>(pitchTopic);
// publish roll status via gz transport
rollTopic = std::string("~/") + this->model->GetName()
+ "/gimbal_roll_status";
// Although gazebo above 7.4 support Any, still use GzString instead
this->rollPub = node->Advertise<gazebo::msgs::GzString>(rollTopic);
// publish yaw status via gz transport
yawTopic = std::string("~/") + this->model->GetName()
+ "/gimbal_yaw_status";
// Although gazebo above 7.4 support Any, still use GzString instead
this->yawPub = node->Advertise<gazebo::msgs::GzString>(yawTopic);
imuSub = node->Subscribe("~/" + model->GetName() + "/imu", &GimbalControllerPlugin::ImuCallback, this);
gzmsg << "GimbalControllerPlugin::Init" << std::endl;
}
void GimbalControllerPlugin::ImuCallback(ImuPtr& imu_message)
{
this->lastImuYaw = ignition::math::Quaterniond(imu_message->orientation().w(),
imu_message->orientation().x(),
imu_message->orientation().y(),
imu_message->orientation().z()).Euler()[2];
}
#if GAZEBO_MAJOR_VERSION > 7 || (GAZEBO_MAJOR_VERSION == 7 && GAZEBO_MINOR_VERSION >= 4)
/// only gazebo 7.4 and above support Any
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnPitchStringMsg(ConstAnyPtr &_msg)
{
// gzdbg << "pitch command received " << _msg->double_value() << std::endl;
this->pitchCommand = _msg->double_value();
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnRollStringMsg(ConstAnyPtr &_msg)
{
// gzdbg << "roll command received " << _msg->double_value() << std::endl;
this->rollCommand = _msg->double_value();
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnYawStringMsg(ConstAnyPtr &_msg)
{
// gzdbg << "yaw command received " << _msg->double_value() << std::endl;
this->yawCommand = _msg->double_value();
}
#else
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnPitchStringMsg(ConstGzStringPtr &_msg)
{
// gzdbg << "pitch command received " << _msg->data() << std::endl;
this->pitchCommand = atof(_msg->data().c_str());
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnRollStringMsg(ConstGzStringPtr &_msg)
{
// gzdbg << "roll command received " << _msg->data() << std::endl;
this->rollCommand = atof(_msg->data().c_str());
}
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnYawStringMsg(ConstGzStringPtr &_msg)
{
// gzdbg << "yaw command received " << _msg->data() << std::endl;
this->yawCommand = atof(_msg->data().c_str());
}
#endif
/////////////////////////////////////////////////
void GimbalControllerPlugin::OnUpdate()
{
if (!this->pitchJoint || !this->rollJoint || !this->yawJoint)
return;
#if GAZEBO_MAJOR_VERSION >= 9
common::Time time = this->model->GetWorld()->SimTime();
#else
common::Time time = this->model->GetWorld()->GetSimTime();
#endif
if (time < this->lastUpdateTime)
{
gzerr << "time reset event\n";
this->lastUpdateTime = time;
return;
}
else if (time > this->lastUpdateTime)
{
double dt = (time - this->lastUpdateTime).Double();
// We want yaw to control in body frame, not in global.
this->yawCommand += this->lastImuYaw;
// truncate command inside joint angle limits
#if GAZEBO_MAJOR_VERSION >= 9
double rollLimited = ignition::math::clamp(this->rollCommand,
rDir*this->rollJoint->UpperLimit(0),
rDir*this->rollJoint->LowerLimit(0));
double pitchLimited = ignition::math::clamp(this->pitchCommand,
pDir*this->pitchJoint->UpperLimit(0),
pDir*this->pitchJoint->LowerLimit(0));
double yawLimited = ignition::math::clamp(this->yawCommand,
yDir*this->yawJoint->LowerLimit(0),
yDir*this->yawJoint->UpperLimit(0));
#else
double rollLimited = ignition::math::clamp(this->rollCommand,
rDir*this->rollJoint->GetUpperLimit(0).Radian(),
rDir*this->rollJoint->GetLowerLimit(0).Radian());
double pitchLimited = ignition::math::clamp(this->pitchCommand,
pDir*this->pitchJoint->GetUpperLimit(0).Radian(),
pDir*this->pitchJoint->GetLowerLimit(0).Radian());
double yawLimited = ignition::math::clamp(this->yawCommand,
yDir*this->yawJoint->GetLowerLimit(0).Radian(),
yDir*this->yawJoint->GetUpperLimit(0).Radian());
#endif
/// currentAngleYPRVariable is defined in roll-pitch-yaw-fixed-axis
/// and gimbal is constructed using yaw-roll-pitch-variable-axis
ignition::math::Vector3d currentAngleYPRVariable(
this->cameraImuSensor->Orientation().Euler());
#if GAZEBO_MAJOR_VERSION >= 8
ignition::math::Vector3d currentAnglePRYVariable(
detail::QtoZXY(ignition::math::Quaterniond(currentAngleYPRVariable)));
#else
ignition::math::Vector3d currentAnglePRYVariable(
detail::QtoZXY(currentAngleYPRVariable));
#endif
/// get joint limits (in sensor frame)
/// TODO: move to Load() if limits do not change
#if GAZEBO_MAJOR_VERSION >= 9
ignition::math::Vector3d lowerLimitsPRY
(pDir*this->pitchJoint->LowerLimit(0),
rDir*this->rollJoint->LowerLimit(0),
yDir*this->yawJoint->LowerLimit(0));
ignition::math::Vector3d upperLimitsPRY
(pDir*this->pitchJoint->UpperLimit(0),
rDir*this->rollJoint->UpperLimit(0),
yDir*this->yawJoint->UpperLimit(0));
#else
ignition::math::Vector3d lowerLimitsPRY
(pDir*this->pitchJoint->GetLowerLimit(0).Radian(),
rDir*this->rollJoint->GetLowerLimit(0).Radian(),
yDir*this->yawJoint->GetLowerLimit(0).Radian());
ignition::math::Vector3d upperLimitsPRY
(pDir*this->pitchJoint->GetUpperLimit(0).Radian(),
rDir*this->rollJoint->GetUpperLimit(0).Radian(),
yDir*this->yawJoint->GetUpperLimit(0).Radian());
#endif
// normalize errors
double pitchError = detail::ShortestAngularDistance(
pitchLimited, currentAnglePRYVariable.X());
double rollError = detail::ShortestAngularDistance(
rollLimited, currentAnglePRYVariable.Y());
double yawError = detail::ShortestAngularDistance(
yawLimited, currentAnglePRYVariable.Z());
// Clamp errors based on current angle and estimated errors from rotations:
// given error = current - target, then
// if target (current angle - error) is outside joint limit, truncate error
// so that current angle - error is within joint limit, i.e.:
// lower limit < current angle - error < upper limit
// or
// current angle - lower limit > error > current angle - upper limit
// re-expressed as clamps:
// hardcoded negative joint axis for pitch and roll
if (lowerLimitsPRY.X() < upperLimitsPRY.X())
{
pitchError = ignition::math::clamp(pitchError,
currentAnglePRYVariable.X() - upperLimitsPRY.X(),
currentAnglePRYVariable.X() - lowerLimitsPRY.X());
}
else
{
pitchError = ignition::math::clamp(pitchError,
currentAnglePRYVariable.X() - lowerLimitsPRY.X(),
currentAnglePRYVariable.X() - upperLimitsPRY.X());
}
if (lowerLimitsPRY.Y() < upperLimitsPRY.Y())
{
rollError = ignition::math::clamp(rollError,
currentAnglePRYVariable.Y() - upperLimitsPRY.Y(),
currentAnglePRYVariable.Y() - lowerLimitsPRY.Y());
}
else
{
rollError = ignition::math::clamp(rollError,
currentAnglePRYVariable.Y() - lowerLimitsPRY.Y(),
currentAnglePRYVariable.Y() - upperLimitsPRY.Y());
}
if (lowerLimitsPRY.Z() < upperLimitsPRY.Z())
{
yawError = ignition::math::clamp(yawError,
currentAnglePRYVariable.Z() - upperLimitsPRY.Z(),
currentAnglePRYVariable.Z() - lowerLimitsPRY.Z());
}
else
{
yawError = ignition::math::clamp(yawError,
currentAnglePRYVariable.Z() - lowerLimitsPRY.Z(),
currentAnglePRYVariable.Z() - upperLimitsPRY.Z());
}
// apply forces to move gimbal
double pitchForce = this->pitchPid.Update(pitchError, dt);
this->pitchJoint->SetForce(0, pDir*pitchForce);
double rollForce = this->rollPid.Update(rollError, dt);
this->rollJoint->SetForce(0, rDir*rollForce);
double yawForce = this->yawPid.Update(yawError, dt);
this->yawJoint->SetForce(0, yDir*yawForce);
// ignition::math::Vector3d angles = this->imuSensor->Orientation().Euler();
// gzerr << "ang[" << angles.X() << ", " << angles.Y() << ", " << angles.Z()
// << "] cmd[ " << this->rollCommand
// << ", " << this->pitchCommand << ", " << this->yawCommand
// << "] err[ " << rollError
// << ", " << pitchError << ", " << yawError
// << "] frc[ " << rollForce
// << ", " << pitchForce << ", " << yawForce << "]\n";
this->lastUpdateTime = time;
}
static int i =1000;
if (++i>100)
{
i = 0;
// There is bug when use gazebo::msgs::Any m, so still use GzString instead
// Although gazebo above 7.4 support Any, still use GzString instead
std::stringstream ss;
gazebo::msgs::GzString m;
#if GAZEBO_MAJOR_VERSION >= 9
ss << this->pitchJoint->Position(0);
m.set_data(ss.str());
this->pitchPub->Publish(m);
ss << this->rollJoint->Position(0);
m.set_data(ss.str());
this->rollPub->Publish(m);
ss << this->yawJoint->Position(0);
m.set_data(ss.str());
this->yawPub->Publish(m);
#else
ss << this->pitchJoint->GetAngle(0).Radian();
m.set_data(ss.str());
this->pitchPub->Publish(m);
ss << this->rollJoint->GetAngle(0).Radian();
m.set_data(ss.str());
this->rollPub->Publish(m);
ss << this->yawJoint->GetAngle(0).Radian();
m.set_data(ss.str());
this->yawPub->Publish(m);
#endif
}
}

2
sensing/gimbal/run.py
View File

@ -11,7 +11,7 @@ rospy.init_node('gimbal_control')
mountCnt = rospy.Publisher(vehicle_type+'_'+vehicle_id+'/mavros/mount_control/command', MountControl, queue_size=10)
mountConfig = rospy.ServiceProxy(vehicle_type+'_'+vehicle_id+'/mavros/mount_control/configure', MountConfigure)
rate=rospy.Rate(100)
gimbal_pitch_ = -45
gimbal_pitch_ = -90
gimbal_yaw_ = 0.0
gimbal_roll_ = 0.0
srvheader=std_msgs.msg.Header()