我整理的C++版Publisher和Subscriber模板代码,以后不要从零开始码。分别有OO写法和PO写法,面向对象方式扩展性更强!
发布和订阅
#include <iostream>
#include <ros/ros.h>
#include <stdio.h>
#include <math.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>
using namespace std;
namespace hector_quadrotor
{
class Teleop
{
private:
// 用的的变量,都放到private里。包括消息变量和发布订阅句柄
geometry_msgs::Twist twist_cmd;
geometry_msgs::Twist pos_current;
geometry_msgs::Twist pos_target;
geometry_msgs::Point euler_current;
geometry_msgs::Quaternion quaternion_current;
double yaw;
double p_xy, p_z, p_yaw;
ros::NodeHandle nh_;
ros::Subscriber get_target_goal;
ros::Publisher pub_twist_cmd;
public:
// 构造函数
Teleop(ros::NodeHandle& nh):nh_(nh)
{
ros::NodeHandle pnh("~");
// 参数服务器获取参数
pnh.param("p_xy", p_xy, 0.20);
pnh.param("p_z", p_z, 0.20);
pnh.param("p_yaw", p_yaw, 0.20);
get_target_goal = nh_.subscribe("pose_cmd", 1, &Teleop::PositionControllerCB,this);
pub_twist_cmd = nh_.advertise<geometry_msgs::Twist>("velocity_cmd", 1);
ros::spin();
}
// 析构函数
~Teleop()
{
}
// 其他函数
void Quat2Euler(geometry_msgs::Quaternion &quat, geometry_msgs::Point &euler)
{
...
}
// subscribe的回调函数
void PositionControllerCB(const geometry_msgs::Twist &msg)
{
pos_target = msg;
double e_x,e_y,e_z,e_yaw;
double vx_n, vy_n; //reference coordinate frame
vx_n = p_xy * (pos_target.linear.x - pos_current.linear.x);
vy_n = p_xy * (pos_target.linear.y - pos_current.linear.y);
twist_cmd.linear.x = cos(yaw) * vx_n + sin(yaw) * vy_n;
twist_cmd.linear.y = -sin(yaw) * vx_n + cos(yaw) * vy_n;
twist_cmd.linear.z = p_z * (pos_target.linear.z - pos_current.linear.z);
twist_cmd.angular.z = p_yaw * (pos_target.angular.z - pos_current.angular.z);
pub_twist_cmd.publish(twist_cmd);
}
};
} // namespace hector_quadrotor
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_target_goal");
ros::NodeHandle nh;
hector_quadrotor::Teleop teleop(nh);
return 0;
}
单纯的发布
#include <ros/ros.h>
#include <topic_demo/gps.h> //自定义msg产生的头文件
int main(int argc, char **argv)
{
ros::init(argc, argv, "talker"); //用于解析ROS参数,第三个参数为本节点名
ros::NodeHandle nh; //实例化句柄,初始化node
topic_demo::gps msg; //自定义gps消息并初始化
...
ros::Publisher pub = nh.advertise<topic_demo::gps>("gps_info", 1); //创建publisher,往"gps_info"话题上发布消息
ros::Rate loop_rate(1.0); //定义发布的频率,1HZ
while (ros::ok()) //循环发布msg
{
... //处理msg
pub.publish(msg); //以1Hz的频率发布msg
loop_rate.sleep(); //根据前面的定义的loop_rate,设置1s的暂停
}
return 0;
}
ros::NodeHandle::param(param_name, param_val, default_val)比ros::NodeHandle::getParam()好用。不信看源码
template<typename T>
bool param(const std::string& param_name, T& param_val, const T& default_val) const
{
if (hasParam(param_name))
{
if (getParam(param_name, param_val))
{
return true;
}
}
param_val = default_val;
return false;
}