Move Model Using Topic System Plugin¶
In these plugin logic & system api are same as Move Model System Plugin & only addition is we are going to use gz sim topic to control the velocity. We will write a gazebo transport node which will subscribe to gazebo sim topic and base on the value will move the model.
gazebo transport
- publisher/subscriber c++ example
- publisher/subscriber python example
Plugin¶
.sdf plugin part
.hh file
#ifndef SYSTEM_PLUGIN_MODEL_HH_
#define SYSTEM_PLUGIN_MODEL_HH_
//! [header]
#include <gz/sim/System.hh> // to inherit system
#include "gz/sim/Model.hh" // for Model component
#include "gz/sim/components/LinearVelocity.hh" // for linear velocity
#include "gz/sim/components/LinearVelocityCmd.hh" // for LinearVelocityCmd component
#include "gz/sim/components/Name.hh" // for Name component
#include <gz/plugin/Register.hh> //for GZ_ADD_PLUGIN_ALIAS()
#include <gz/msgs/Utility.hh> //for msg
#include <gz/transport/Node.hh> //for gz transport node
#include <gz/math/Vector3.hh>
#include <gz/msgs/double.pb.h> // Include the Double message type
namespace gz
{
namespace sim
{
namespace systems
{
/// \brief plugin to move a model
/// plugin interface.
class MoveModelTopicWay :
// This class is a system.
public gz::sim::System,
public gz::sim::ISystemConfigure,
// This class also implements the ISystemPreUpdate interface.
public gz::sim::ISystemUpdate
{
public:
MoveModelTopicWay();
~MoveModelTopicWay() override;
void Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &_eventMgr) override;
void Update(const UpdateInfo &_info,
EntityComponentManager &_ecm) override;
private:
std::string modelName;
std::string topicName;
double zVelocity{0.0};
Entity targetEntity{kNullEntity};
gz::transport::Node node; // GZ Transport node
/* transport msg callback */
void OnTransportMsg(const gz::msgs::Double & _msg);
};
}
}
}
//! [header]
#endif
.cc file
#include "MoveModelTopicWay.hh"
using namespace gz;
using namespace sim;
using namespace systems;
MoveModelTopicWay::MoveModelTopicWay() {
std::cout<<"MoveModelTopicWay Plugin Started!!"<<std::endl;
}
MoveModelTopicWay::~MoveModelTopicWay(){
std::cout<<"MoveModelTopicWay Plugin stopped!!"<<std::endl;
}
void MoveModelTopicWay::Configure(const Entity &_entity,
const std::shared_ptr<const sdf::Element> &_sdf,
EntityComponentManager &_ecm,
EventManager &/*_eventMgr*/)
{
// 1. Read the target model name from SDF
if (!_sdf->HasElement("model_name"))
{
gzerr << "MoveModel plugin requires a <model_name> element." << std::endl;
return;
}
this->modelName = _sdf->Get<std::string>("model_name");
gzmsg << "Target Model Name: " << this->modelName << std::endl;
if (!_sdf->HasElement("topic_name"))
{
gzerr << "MoveModel plugin requires a <topic_name> element." << std::endl;
return;
}
this->topicName = _sdf->Get<std::string>("topic_name");
gzmsg << "Cmd vel z Topic Name: " << this->topicName << std::endl;
this->node.Subscribe(this->topicName, &MoveModelTopicWay::OnTransportMsg, this);
if (_sdf->HasElement("z_velocity"))
{
this->zVelocity = _sdf->Get<double>("z_velocity");
}
gzmsg << "Initial Z Velocity: " << this->zVelocity << " m/s" << std::endl;
}
void MoveModelTopicWay::OnTransportMsg(const gz::msgs::Double & _msg){
// Directly extract the double value from the message
this->zVelocity = _msg.data();
gzdbg << "Received new Z-velocity command: " << this->zVelocity << std::endl;
}
void MoveModelTopicWay::Update(const UpdateInfo &_info,
EntityComponentManager &_ecm)
{
// Only run if the simulation is not paused
if (_info.paused)
return;
// 1. Find the target model entity by name (if not found yet)
if (this->targetEntity == kNullEntity)
{
auto entityOpt = _ecm.EntityByName(this->modelName);
if (!entityOpt.has_value())
{
gzdbg << "Model [" << this->modelName
<< "] not found yet. Skipping velocity application." << std::endl;
return;
}
this->targetEntity = entityOpt.value();
gzmsg << "Found target model entity: " << this->targetEntity << std::endl;
}
//method 1
// 2.Get / create LinearVelocity component
// auto velComp =
// _ecm.Component<components::LinearVelocityCmd>(this->targetEntity);
// if (!velComp)
// {
// velComp = _ecm.CreateComponent(this->targetEntity,
// components::LinearVelocityCmd());
// gzmsg << "Added LinearVelocity component to model: "
// << this->modelName << std::endl;
// }
// if (!velComp)
// {
// gzerr << "Failed to create/get LinearVelocity component for model ["
// << this->modelName << "]." << std::endl;
// return;
// }
// // 3.Set the Z-axis linear velocity: (0, 0, zVelocity)
// const gz::math::Vector3d vel(0.0, 0.0, this->zVelocity);
// velComp->Data() = vel;
/// or
// method 2
//instate of step 2 & 3 directly run
const gz::math::Vector3d vel(0.0, 0.0, this->zVelocity);
_ecm.SetComponentData<components::LinearVelocityCmd>(this->targetEntity,{vel});
}
// Register the plugin with Gazebo Sim
GZ_ADD_PLUGIN(gz::sim::systems::MoveModelTopicWay,
gz::sim::System,
gz::sim::ISystemConfigure,
gz::sim::ISystemUpdate)
GZ_ADD_PLUGIN_ALIAS(gz::sim::systems::MoveModelTopicWay,
"gz::sim::systems::MoveModelTopicWay")
Gazebo Messages¶
gz-msgs use google Protobuf messages
gz msgs Double Class Reference
Gazebo Transport Node¶
topic callback function
void MoveModelTopicWay::OnTransportMsg(const gz::msgs::Double & _msg){
// Directly extract the double value from the message
this->zVelocity = _msg.data();
gzdbg << "Received new Z-velocity command: " << this->zVelocity << std::endl;
}
Gazebo tools (command line)¶
list topic
check topic msg type
send msg