引言:在ROS项目开发中,有些数据需要在回调函数中处理后再发布出去。这种情况下,通常需要发布者定义成全局变量,否则无法在汉调函数中调用发布者。
步骤一:定义全局变量发布者,必须定义成全局变量
![请添加图片描述](https://i-blog.csdnimg.cn/blog_migrate/95e487c546a536b4cbc00282a8d50aff.png)
步骤二:在回调函数或者任意子函数中调用即可
![请添加图片描述](https://i-blog.csdnimg.cn/blog_migrate/78298090439548a87cbed6b7f846f6da.png)
步骤三:代码参考
#include <ros/ros.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <cmath>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <mavros_msgs/CommandLong.h>
#include <string>
#define MAX_ERROR 0.20
#define VEL_SET 0.10
#define ALTITUDE 0.40
using namespace std;
float target_x_angle = 0;
float target_distance = 2000;
float linear_x_p = 0.5;
float linear_x_d = 0.33;
float yaw_rate_p = 4.0;
float yaw_rate_d = 15;
geometry_msgs::PointStamped object_pos;
nav_msgs::Odometry local_pos;
mavros_msgs::State current_state;
mavros_msgs::PositionTarget setpoint_raw;
string current_frame_id = "no_object";
double current_position_x = 0;
double current_position_y = 0;
double current_distance = 0;
ros::Subscriber state_sub;
ros::Subscriber local_pos_sub;
ros::Subscriber object_pos_sub;
ros::Publisher mavros_setpoint_pos_pub;
ros::ServiceClient arming_client;
ros::ServiceClient set_mode_client;
void pid_control()
{
static float last_error_x_angle = 0;
static float last_error_distance = 0;
float x_angle;
float distance;
if(current_position_x == 0 && current_position_y == 0 && current_distance == 0)
{
x_angle = target_x_angle;
distance = target_distance;
}
else
{
x_angle = current_position_x / current_distance;
distance = current_distance;
}
float error_x_angle = x_angle - target_x_angle;
float error_distance = distance - target_distance;
if(error_x_angle > -0.01 && error_x_angle < 0.01)
{
error_x_angle = 0;
}
if(error_distance > -80 && error_distance < 80)
{
error_distance = 0;
}
setpoint_raw.velocity.x = error_distance*linear_x_p/1000 + (error_distance - last_error_distance)*linear_x_d/1000;
if(setpoint_raw.velocity.x < -0.3)
{
setpoint_raw.velocity.x = -0.3;
}
else if(setpoint_raw.velocity.x > 0.3)
{
setpoint_raw.velocity.x = 0.3;
}
setpoint_raw.yaw_rate = error_x_angle*yaw_rate_p + (error_x_angle - last_error_x_angle)*yaw_rate_d;
if(setpoint_raw.yaw_rate < -0.5)
{
setpoint_raw.yaw_rate = -0.5;
}
else if(setpoint_raw.yaw_rate > 0.5)
{
setpoint_raw.yaw_rate = 0.5;
}
mavros_setpoint_pos_pub.publish(setpoint_raw);
last_error_x_angle = error_x_angle;
last_error_distance = error_distance;
}
void state_cb(const mavros_msgs::State::ConstPtr& msg)
{
current_state = *msg;
}
void local_pos_cb(const nav_msgs::Odometry::ConstPtr& msg)
{
local_pos = *msg;
}
void object_pos_cb(const geometry_msgs::PointStamped::ConstPtr& msg)
{
object_pos = *msg;
current_position_x = object_pos.point.x*(-1000);
current_position_y = object_pos.point.y*(-1000);
current_distance = object_pos.point.z*1000;
current_frame_id = object_pos.header.frame_id;
pid_control();
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "follow_pid");
ros::NodeHandle nh;
state_sub = nh.subscribe<mavros_msgs::State>("mavros/state", 100, state_cb);
local_pos_sub = nh.subscribe<nav_msgs::Odometry>("/mavros/local_position/odom", 100, local_pos_cb);
object_pos_sub = nh.subscribe<geometry_msgs::PointStamped>("object_position", 100, object_pos_cb);
mavros_setpoint_pos_pub = nh.advertise<mavros_msgs::PositionTarget>("/mavros/setpoint_raw/local", 100);
arming_client = nh.serviceClient<mavros_msgs::CommandBool>("mavros/cmd/arming");
set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("mavros/set_mode");
ros::Rate rate(20.0);
ros::param::get("linear_x_p",linear_x_p);
ros::param::get("linear_x_d",linear_x_d);
ros::param::get("yaw_rate_p",yaw_rate_p);
ros::param::get("yaw_rate_d",yaw_rate_d);
ros::param::get("target_x_angle", target_x_angle);
ros::param::get("target_distance",target_distance);
setpoint_raw.type_mask = + 64 + 128 + 256 + 512 ;
setpoint_raw.coordinate_frame = 1;
setpoint_raw.position.x = 0;
setpoint_raw.position.y = 0;
setpoint_raw.position.z = 0 + ALTITUDE;
mavros_setpoint_pos_pub.publish(setpoint_raw);
for(int i = 100; ros::ok() && i > 0; --i)
{
mavros_setpoint_pos_pub.publish(setpoint_raw);
ros::spinOnce();
rate.sleep();
}
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
ros::Time last_request = ros::Time::now();
while(ros::ok())
{
ros::spinOnce();
rate.sleep();
}
}