roscpp模式总结

最新推荐文章于 2023-03-07 17:53:34 发布

紫电清

最新推荐文章于 2023-03-07 17:53:34 发布

阅读量761

点赞数 2

分类专栏： ROS 文章标签： ros-代码 cpp 模版

本文链接：https://blog.csdn.net/vcdanda/article/details/77050062

版权

ROS 专栏收录该内容

2 篇文章 0 订阅

订阅专栏

综述

在写代码的过程中，一直苦于没有一个比较通用标准的代码模版。为了形成一个良好的代码规范，所以在这里博主把这段时间写的代码提取出来，写成下面的模版。希望以后可以提高编写roscpp的效率。

1. Publisher

代码模版展示：

#include "ros/ros.h"

#include "std_msgs/String.h"//某些message

#include <tf/transform_listener.h>//某些ros中使用的包

#include <sstream>//某些cpp中使用的库

int main(int argc, char **argv){
ros::init(argc,char)；
ros::NodeHandle n;
ros::Publisher chatter_pub=n.advertise<std_msgs::String>("chatter",1000);
ros::Rate r(10);
int count=0;
while(ros::ok()){
std_msgs::String msg;//初始化一个msg

std::stringstream ss;
ss<<"hello world"<<count;
chatter_pub.publish(msg);

ros::spinOnce();
r.sleep();
++count;

}
return 0;
}

这里的publisher是用来发送topic“chatter”的。

2.subscriber

#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "geometry_msgs/Pose.h"
#include "basic_msgs/gridpose.h"//自定义message
#include <time.h>
#include "math.h"

void callback(const geometry_msgs::Pose::ConstPtr &msg){
//可以用全局变量保存想要保存的内容

}

int main(int argh, char **argv){
ros::init(argh, argue, "talker");
ros::NodeHangle n;
ros::Subscriber sub=n.subscribe("robot_pose",2,callback);
ros::Rate r(2);
int count =0;
int i=1;
while( ros::ok()){
std_msgs::Int32 msg;//初始化msg
//可能会有pub.publish(<message>)；
ros::spinOnce();
r.sleep();
}
return 0;

}

3.service

分为两种，一种是serviceserver,即为提供service的，一种是serviceclient，请求得到响应的客户端。

3.1 serviceserver

#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include "operation/mapping_status.h"//同一目录下的自定义service
#include <iostream>


bool server(operation::mapping_status::Request &req, operation::mapping_status::Response &res){
    ...

}

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

    ros::param::param<std::string>("mapsaver_bringup_path",mapsaver_bringup_path,"/.../path/file");//ros param
    ros::ServiceServer service=n.advertiseService("/mapping_status",server);//ros service server.

    ros::spin();

    return 0;


}

3.2 serviceclient

#include "ros/ros.h"
#include "operation/mapping_status.h"
#include <cstdlib>

int main(int argc, char **argv)
{
    ros::init(argc,argv,"mapping_status_client");
    ros::NodeHandle n;
    ros::ServiceClient client=n.serviceClient<opeartion::mapping_status>("/mapping_status");
    srv.request.data=atoll(argv[1]);//第一个值为request的输入值
    if(client.call(srv)){

    ROS_INFO("successed");
    }else{
    ROS_INFO("Failed");

    }

    return 0;

}

4.transform

分为两种：broadcaster和listener.

4.1 broadcaster:

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
    ros::init(argc, argv, "my_tf_broadcaster");
    ros::NodeHandle n;
    tf::TransformBroadcaster br;

    ros::Rate r(20);
    while(n::ok()){
      tf::Transform transform;
      transform.setOrigin(tf::Vector3(x,y,z));//x,y,z 自己添
      transform.setRotation(tf::Quaternion(q_x,q_y,q_z,q_w));//同理
      br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"parent frame","child frame"));//child frame 在parent frame 的位置就是上面的姿态值。
      ros::spinOnce();


    }
    return 0;
};

4.2 listener:

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Pose.h>

geometry_msgs::Pose pose_listen(tf::StampedTransform &transform_listener){
    geometry_msgs::Pose pose;
    pose.position.x=transform_listener.getOrigin().x()*20;
    pose.position.y=transform_listener.getOrigin().y()*20;
    ...
    return pose;

}


int main(int argc,char** argv){
    ros::init(argc,argv,"my_tf_listener");
    ros::NodeHandle n;
    tf::TransformListener listener;
    ros::Rate r(1);
    while(n.ok()){
    tf::StampedTransform transform_listener;

    try{
    listener.waitForTransform("/frame1","/frame2",ros::Time(0),ros::Duration(10.0));//ros::Time(0)即使时间可能不同步，但是不会报错，如果是ros::Time.now()由于延时，会发生报错的。
    listener.loopupTransform("/frame1","/frame2",ros::Time(0),transform_listener);//frame1是主frame
    }catch(tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    ros::Duration(1.0).sleep();
    continue;
    }

    geometry_msgs::Pose pose;//对于之前得到的转换进行处理
    pose=pose_listener(transform_listener);

    ros::spinOnce();
    r.sleep();
    }
    return 0;
}

4.3 transform point

最后，是让点转换到某个坐标系中去。代码如下：


#include <ros/ros.h>
#include <geometry_msgs/PointStamped.h>
#include <tf/transform_listener.h>
#include <basic_msgs/laser_points.h>
#include <basic_msgs/points.h>
#include <vector>
#include <iostream>

class Scan_info{
public:
    sensor_msgs:LaserScan ranges;

    Scan_info();
    ~Scan_info();
    void scancallback(const sensor_msgs::LaserScanConstPtr &msg);


};

Scan_info::Scan_info(){
    ROS_INFO("Constructor started");
}

Scan_info::~Scan_info(){
    ROS_INFO("Deconstructor started");
}

void Scan_info::scancallback(const sensor_msgs::LaserScanConstPtr &msg){

    Scan_info::laser_point_vector.data.clear();
    ...

    Scan_info::laser_point_vector.data.push_back(test_msg);//这是一个vector，会存到实例化后的对象上。
}

void transformPoint(const tf::TransformListener& listener, geometry_msgs::PointStamped& laser_point,...,int i){
    ...
    try{
    geometry_msgs::PointStamped base_point;
    base_msgs::points point;
    listener.transformPoint("picture_frame",laser_point,base_point);//得到base_point.
    }catch(tf::TransformException& ex){
    ROS_ERROR("...");
    }
}

int main(int argc,char** argv){
    ros::init(argc,argv,"robot_tf_listener");
    ros::NodeHandle n;
    Scan_info *scaninfo=new Scan_info();
    ros::Subscriber scan_sub=n.subscribe("base_scan",100,&Scan_info::scancallback,scaninfo);//这里的scaninfo是实例化的类，这样subsriber里面就可以返回值到主函数里面了。

    tf::TransformListener listener(ros::Duration(10));
    ros::Timer timer1=n.createTimer(ros::Duration(1.0),boost::bind(&transformPoint,boost::ref(listener),boost::ref(scaninfo->laser_point),int i);//这里的boost::ref里面是对象的静态数据成员，或者对象。boost::bind的第一个参数必须是&transformPoint,来作为transformpoint函数处理点的转换。

}

5.关于CMakelists与package.xml

原则：不要在一个workspace里面自定义了message，srv，同时在不同的package里面的cpp里面使用他们。如果使用，可以先拿出cpp的package，catkin_make workspace里面的内容，然后再把包含这些cpp的package放进去，进行编译。

需要生成新的message包里面的package.xml中，需要加入

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

CMakelists
里面

find package(catkin REQUIRED COMPONENTS
roscpp
...
message_generation
)

generate_messages(
DEPENDENCIES
std_msgs
geometry_msgs
)

catkin_package(
CATKIN_DEPENDS message_runtime
)

6.linux管理

在ros中，有时候需要在cpp中调用系统命令行。采用多线程方法，在子线程中调用命令行，而在别的线程中仍继续运行，等待service从客户端调用。
举个例子

int parameter =-1;
pthread_t id;
int i,ret;
struct Param{
int type;

};

void *thread(void *ptr){
    Param *param=(Param *)ptr;//强制类型转换
    ／／...
    ／／子线程程序

}

bool server(...){

    ／／这里在一个serviceserver里面用了多线程
    Param *param=new Param();
    param->type=req.data;
    ret=pthread_create(&id,NULL,thread,param);//进入子线程程序
    //。。。主线程
    sleep(1);
    return true

综上