gazebo publish pose

在GimbalSmall2dPlugin.cc 基础上修改

origin https://github.com/khancyr/ardupilot_gazebo (fetch)

public: transport::PublisherPtr pub,posepub;

this->dataPtr->posepub =
this->dataPtr->node->Advertise<gazebo::msgs::Pose>(std::string("~/") +
this->dataPtr->model->GetName() + "/copter_pose");

gazebo::msgs::Pose p=gazebo::msgs::Convert(this->dataPtr->model->GetWorldPose().Ign () );

this->dataPtr->posepub->Publish(p);

参考api

http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1math_1_1Pose.html

http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/group__gazebo__msgs.html

http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/classgazebo_1_1physics_1_1Entity.html#a3fb9c33d26b0c46dd6f735f39bbd15de

http://osrf-distributions.s3.amazonaws.com/gazebo/api/7.1.0/namespacegazebo_1_1msgs.html

/** Copyright (C) 2016 Open Source Robotics Foundation** Licensed under the Apache License, Version 2.0 (the "License");* you may not use this file except in compliance with the License.* You may obtain a copy of the License at**     http://www.apache.org/licenses/LICENSE-2.0** Unless required by applicable law or agreed to in writing, software* distributed under the License is distributed on an "AS IS" BASIS,* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.* See the License for the specific language governing permissions and* limitations under the License.*
*/
#include <string>
#include <vector>#include "gazebo/common/PID.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/transport.hh"
#include "GimbalSmall2dPlugin.hh"
using namespace gazebo;
using namespace std;GZ_REGISTER_MODEL_PLUGIN(GimbalSmall2dPlugin)/// \brief Private data class
class gazebo::GimbalSmall2dPluginPrivate
{/// \brief Callback when a command string is received./// \param[in] _msg Mesage containing the command stringpublic: void OnStringMsg(ConstGzStringPtr &_msg);/// \brief A list of event connectionspublic: std::vector<event::ConnectionPtr> connections;/// \brief Subscriber to the gimbal command topicpublic: transport::SubscriberPtr sub;/// \brief Publisher to the gimbal status topicpublic: transport::PublisherPtr pub,posepub;/// \brief Parent model of this pluginpublic: physics::ModelPtr model;/// \brief Joint for tilting the gimbalpublic: physics::JointPtr tiltJoint;/// \brief Command that updates the gimbal tilt anglepublic: double command = IGN_PI_2;/// \brief Pointer to the transport nodepublic: transport::NodePtr node;/// \brief PID controller for the gimbalpublic: common::PID pid;/// \brief Last update sim timepublic: common::Time lastUpdateTime;
};/
GimbalSmall2dPlugin::GimbalSmall2dPlugin(): dataPtr(new GimbalSmall2dPluginPrivate)
{this->dataPtr->pid.Init(1, 0, 0, 0, 0, 1.0, -1.0);
}/
void GimbalSmall2dPlugin::Load(physics::ModelPtr _model,sdf::ElementPtr _sdf)
{this->dataPtr->model = _model;std::string jointName = "tilt_joint";if (_sdf->HasElement("joint")){jointName = _sdf->Get<std::string>("joint");}this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(jointName);if (!this->dataPtr->tiltJoint){std::string scopedJointName = _model->GetScopedName() + "::" + jointName;gzwarn << "joint [" << jointName<< "] not found, trying again with scoped joint name ["<< scopedJointName << "]\n";this->dataPtr->tiltJoint = this->dataPtr->model->GetJoint(scopedJointName);}if (!this->dataPtr->tiltJoint){gzerr << "GimbalSmall2dPlugin::Load ERROR! Can't get joint '"<< jointName << "' " << endl;}
}/
void GimbalSmall2dPlugin::Init()
{this->dataPtr->node = transport::NodePtr(new transport::Node());this->dataPtr->node->Init(this->dataPtr->model->GetWorld()->GetName());this->dataPtr->lastUpdateTime =this->dataPtr->model->GetWorld()->GetSimTime();std::string topic = std::string("~/") +  this->dataPtr->model->GetName() +"/gimbal_tilt_cmd";this->dataPtr->sub = this->dataPtr->node->Subscribe(topic,&GimbalSmall2dPluginPrivate::OnStringMsg, this->dataPtr.get());this->dataPtr->connections.push_back(event::Events::ConnectWorldUpdateBegin(std::bind(&GimbalSmall2dPlugin::OnUpdate, this)));topic = std::string("~/") +this->dataPtr->model->GetName() + "/gimbal_tilt_status";this->dataPtr->pub =this->dataPtr->node->Advertise<gazebo::msgs::GzString>(topic);this->dataPtr->posepub =this->dataPtr->node->Advertise<gazebo::msgs::Pose>(std::string("~/") +this->dataPtr->model->GetName() + "/copter_pose");//virtual const math::Pose
}/
void GimbalSmall2dPluginPrivate::OnStringMsg(ConstGzStringPtr &_msg)
{this->command = atof(_msg->data().c_str());
}/
void GimbalSmall2dPlugin::OnUpdate()
{if (!this->dataPtr->tiltJoint)return;double angle = this->dataPtr->tiltJoint->GetAngle(0).Radian();common::Time time = this->dataPtr->model->GetWorld()->GetSimTime();if (time < this->dataPtr->lastUpdateTime){this->dataPtr->lastUpdateTime = time;return;}else if (time > this->dataPtr->lastUpdateTime){double dt = (this->dataPtr->lastUpdateTime - time).Double();double error = angle - this->dataPtr->command;double force = this->dataPtr->pid.Update(error, dt);this->dataPtr->tiltJoint->SetForce(0, force);this->dataPtr->lastUpdateTime = time;}static int i = 1000;if (++i > 100){i = 0;std::stringstream ss;ss << angle;gazebo::msgs::GzString m;m.set_data(ss.str());this->dataPtr->pub->Publish(m);//Vector3 v=this->dataPtr->model->GetWorldPose().pos;
gazebo::msgs::Pose p=gazebo::msgs::Convert(this->dataPtr->model->GetWorldPose().Ign () );this->dataPtr->posepub->Publish(p);}
}

gazebo publish pose相关推荐

解决Ardupilot+gazebo+mavros在仿真状态下无人机能解锁，但是不能起飞的问题
1. 问题在进行无人机仿真时,我采用的是Ardupilot+gazebo+mavros和Ubuntu 18.04环境.在通过编写ros程序控制无人机在gazebo中起飞时,无人机能解锁,它的桨叶可以 ...
PX4无人机-Gazebo仿真实现移动物体的跟踪
原文链接PX4无人机-Gazebo仿真实现移动物体的跟踪末尾有演示视频这个学期我们有一个智能机器人系统的课设,我们组分配到的题目是<仿真环境下使用无人机及相机跟踪移动物体>,本文主要记录 ...
在ROS Kinetic和Gazebo 8中使用智能汽车仿真演示
在ROS Kinetic和Gazebo 8中使用智能汽车仿真演示智能车无人驾驶技术是目前人工智能和机器人技术的研究热点,有许多开源平台可以使我们零基础零成本入门无人驾驶技术.本文分享一下目前ROS官 ...
ROS中在gazebo中模拟GPS数据
前面的文章写过了在robot_pose_ekf中添加GPS的方法,但是GPS在gazebo中使用会有两个问题: 1.GPS使用需要经过UTM包进行坐标转换,转换完成之后会出现一个GPS坐标很大的问题, ...
realsense D435i gazebo slam(px4)仿真（转载）
转载自:https://blog.csdn.net/weixin_41469272/article/details/117919845?utm_medium=distribute.pc_relevan ...
基于ROS的PX4+Gazebo仿真——PX4一键起飞及飞行控制
一键起飞参考及引用 1. CSDN博主「战争果子」的原创文章,遵循CC 4.0 BY-SA版权协议. 原文:https://blog.csdn.net/EnthusiasmZing/article/ ...
无人机仿真—PX4编译，gazebo仿真及简单off board控制模式下无人机起飞
无人机仿真-PX4编译,gazebo仿真及简单off board控制模式下无人机起飞前言在上篇记录中,已经对整体的PX4仿真环境有了一定的了解,现如今就要开始对无人机进行起飞等仿真环境工作,在整体 ...
PX4+Offboard模式+代码控制无人机起飞（Gazebo）
参考PX4自动驾驶用户指南 https://docs.px4.io/main/zh/ros/mavros_offboard_cpp.html 我的另一篇博客写了键盘控制PX4无人机飞行 PX4无人机 ...
[gazebo仿真1]Kinova机械臂gazebo控制
gazebo加载完kinova机械臂后,通过joint_state_publisher_gui节点控制机械臂在gazebo中的姿态 1.编写转换节点 joint_state_publisher_gui ...
Gazebo機器人仿真學習探索筆記（三）機器人模型
gazebo_models:https://bitbucket.org/osrf/gazebo_models 模型庫下載,可以參考如下命令: ~/Rob_Soft/Gazebo7$ hg clone ...

gazebo publish pose

gazebo publish pose相关推荐

最新文章

热门文章