C++类对象化编写ROS的sub&pub

按照官方教程,写一个简单的ROS接收/发送是很容易的。但是C++倡导对象化,把ROS节点注册、topic接收/发送写到类里,还是一个很好的做法。一方面工程大了之后,方便管理;另一方面个人看起来也更舒服。

1、C++ class

class_demo.cpp

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

class RobotMove {
 public:
  RobotMove();

 private:
  geometry_msgs::Twist cmd;
  nav_msgs::Odometry odom;

  ros::NodeHandle nh;   // declare nodehandle
  ros::Subscriber sub;  // declare sub
  ros::Publisher pub;   // declare pub

  void odomCallback(const nav_msgs::Odometry& msg);  // declare callback_func
  void loopCtl(void);
};

// difine sub&pub
RobotMove::RobotMove() {
  sub = nh.subscribe("odom", 1000, &RobotMove::odomCallback, this);  //
  pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 50);
  loopCtl();
}

// difine callback_func
void RobotMove::odomCallback(const nav_msgs::Odometry& msg) { odom = msg; }

// loop
void RobotMove::loopCtl() {
  ros::Rate rate(30);
  while (ros::ok()) {
    pub.publish(cmd);  // loop cmd
    ros::spinOnce();
    rate.sleep();
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "robotmove");
  RobotMove robotobj;
  ros::spin();
  return 0;
}

2、normal

normal_demo.cpp

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

geometry_msgs::Twist cmd;
nav_msgs::Odometry odom;

void odomCallback(const nav_msgs::Odometry& msg) { odom = msg; }

int main(int argc, char** argv) {
  ros::init(argc, argv, "robotmove");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe("odom", 1000, odomCallback);
  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 50);

  ros::Rate r(30);
  while (ros::ok()) {
    pub.publish(cmd);
    ros::spinOnce();
    r.sleep();
  }
  return 0;
}

常规写法代码量比class写法更少,但是class写法更符合对象化思维吧。

3、private_nh写法

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <iostream>

class RobotMove {
 public:
  RobotMove();

 private:
  geometry_msgs::Twist cmd;
  nav_msgs::Odometry odom;

  ros::NodeHandle nh;   // declare nodehandle
  ros::Subscriber sub;  // declare sub
  ros::Publisher pub;   // declare pub
  std::string odom_topic, cmd_vel_topic;

  void odomCallback(const nav_msgs::Odometry& msg);  // declare callback_func
  void loopCtl(void);
};

// difine sub&pub
RobotMove::RobotMove() {
  ros::NodeHandle private_nh("~");  // declare private nodehandle
  private_nh.param("odom_topic", odom_topic, std::string("odom"));
  private_nh.param("cmd_vel_topic", cmd_vel_topic, std::string("cmd_vel"));
  sub = nh.subscribe(odom_topic, 1000, &RobotMove::odomCallback, this);  //
  pub = nh.advertise<geometry_msgs::Twist>(cmd_vel_topic, 50);
  loopCtl();
}

// difine callback_func
void RobotMove::odomCallback(const nav_msgs::Odometry& msg) { odom = msg; }

// loop
void RobotMove::loopCtl() {
  ros::Rate rate(30);
  while (ros::ok()) {
    pub.publish(cmd);  // loop cmd
    ros::spinOnce();
    rate.sleep();
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "robotmove");
  RobotMove robotobj;
  ros::spin();
  return 0;
}

4、多线程

thread

#include <ros/ros.h>

#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>

#include <thread>
#include <boost/thread/thread.hpp>

class RobotMove {
 public:
  RobotMove();
  ~RobotMove();
 private:
  geometry_msgs::Twist cmd;
  nav_msgs::Odometry odom;

  ros::NodeHandle nh;   // declare nodehandle
  ros::Subscriber sub;  // declare sub
  ros::Publisher pub;   // declare pub

  void odomCallback(const nav_msgs::Odometry& msg);  // declare callback_func
  void loopCtl(void);
  boost::thread *callback_thread_;
};

// difine sub&pub
RobotMove::RobotMove() {
  sub = nh.subscribe("odom", 1000, &RobotMove::odomCallback, this);  
  pub = nh.advertise<geometry_msgs::Twist>("cmd_vel", 50);
  callback_thread_ = new boost::thread(boost::bind(&RobotMove::callbackHandler, this));
  loopCtl();
}

RobotMove::~RobotMove() {
  callback_thread_->interrupt();
  callback_thread_->join();
  delete callback_thread_;
}
// difine callback_func
void RobotMove::odomCallback(const nav_msgs::Odometry& msg) { odom = msg; }

void RobotRegister::callbackHandler()
{
   //ros::Rate rate(1);
  //while (ros::ok())
  //{
   // ros::spinOnce();
    //rate.sleep();
 // }
}

// loop
void RobotMove::loopCtl() {
  ros::Rate rate(30);
  while (ros::ok()) {
    pub.publish(cmd);  // loop cmd
    ros::spinOnce();
    rate.sleep();
  }
}

int main(int argc, char** argv) {
  ros::init(argc, argv, "robotmove");
  RobotMove robotobj;
  ros::spin();
  return 0;
}
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 大白 设计师: CSDN官方博客
应支付0元
点击重新获取
扫码支付

支付成功即可阅读