Template for Publisher and Subscriber

我整理的C++版Publisher和Subscriber模板代码，以后不要从零开始码。分别有OO写法和PO写法，面向对象方式扩展性更强！

发布和订阅

#include 
#include 
#include 
#include 

#include 
#include 
#include 
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("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 
#include  //自定义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("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
  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;
  }