做了几个控制插件对gazebo的模型进行控制然后进行仿真,但是发现好像插件识别不了gazebo的宏,打开gazebo的时候加载不出插件,请问有什么注意的点吗?以下是其中一个插件
#include "flapping_controller.hpp"
#include <gazebo/physics/Joint.hh>
#include <chrono>
#include <rclcpp/rclcpp.hpp>
using namespace gazebo;
FlappingController::FlappingController()
{
std::cout << "[FlappingController] Constructor called." << std::endl;
}
FlappingController::~FlappingController() {}
void FlappingController::Load(physics::ModelPtr model, sdf::ElementPtr sdf)
{
std::cout << "[FlappingController] Load() start." << std::endl;
if (!model)
{
std::cout << "[FlappingController] Model pointer is null!" << std::endl;
return;
}
this->model_ = model;
this->start_time_ = model->GetWorld()->SimTime().Double();
this->last_time_ = this->start_time_;
if (sdf->HasElement("namespace_model"))
this->ns_ = sdf->Getstd::string("namespace_model");
else
this->ns_ = "bird2";
try
{
this->ros_node_ = rclcpp::Node::make_shared(this->ns_ + "_flap_node");
}
catch (const std::exception &e)
{
std::cout << "[FlappingController] Failed to create ROS2 node: " << e.what() << std::endl;
return;
}
RCLCPP_INFO(this->ros_node_->get_logger(), "Creating subscriptions and publishers...");
this->flap_freq_sub_ = this->ros_node_->create_subscription<std_msgs::msg::Float32>(
"/" + ns_ + "/frequency", 10,
std::bind(&FlappingController::OnFlapFreqMsg, this, std::placeholders::_1));
this->height_sub_ = this->ros_node_->create_subscription<std_msgs::msg::Float32>(
"/" + ns_ + "/height", 10,
std::bind(&FlappingController::OnHeightMsg, this, std::placeholders::_1));
this->speed_pub_ = this->ros_node_->create_publisher<std_msgs::msg::Float32>(
"/" + ns_ + "/speed", 10);
this->update_connection_ = event::Events::ConnectWorldUpdateBegin(
std::bind(&FlappingController::OnUpdate, this));
RCLCPP_INFO(this->ros_node_->get_logger(), "FlappingController loaded.");
}
void FlappingController::OnFlapFreqMsg(const std_msgs::msg::Float32::SharedPtr msg)
{
this->SetFlappingFrequency(msg->data);
}
void FlappingController::SetFlappingFrequency(double freq)
{
this->flapping_freq_ = freq;
}
void FlappingController::OnHeightMsg(const std_msgs::msg::Float32::SharedPtr msg)
{
if (msg->data < 0.1)
this->flap_enabled_ = false;
else
this->flap_enabled_ = true;
}
void FlappingController::OnUpdate()
{
if (!this->model_)
return;
double now = model_->GetWorld()->SimTime().Double();
if (!flap_enabled_)
return;
double elapsed = now - this->start_time_;
double left = flapping_amp_ * sin(2 * M_PI * flapping_freq_ * elapsed);
double right = -left;
auto joint_left = model_->GetJoint("joint_4");
auto joint_right = model_->GetJoint("joint_6");
if (!joint_left || !joint_right)
{
std::cerr << "[FlappingController] Failed to find joint_4 or joint_6!" << std::endl;
return;
}
joint_left->SetPosition(0, left);
joint_right->SetPosition(0, right);
std_msgs::msg::Float32 speed_msg;
speed_msg.data = left;
this->speed_pub_->publish(speed_msg);
}
GZ_REGISTER_MODEL_PLUGIN(FlappingController)