ros编程
ros编程
ros概念
topic
service
action
param:
是一种通过ROS计算网络访问的共享多变量字典，节点可以使用该服务器在运行时候进行存储和检索参数。
童工XMLRPC进行实现，斌在ROS主服务器内部进行运行，意味着他的API可以通过XMLRPC库访问，支持32位整数、布尔、字符串、双精度(DOUBLE)、ISO8601日期、List、Base64的二进制数据。
rosparam set//给参数设定一定值 rosparam get//获取给定参数的值 rosparam load//从YAML文件中记加载YAML文件 rosparam dump//把现有的ROS参数存储到YAML文件里面 rosparam delete//删除给定参数 rosparam list//列出现有参数的名称 //扩展名：.bag<消息记录包> rosbag record rosbag -a rosbag play
ros命令
roscore
rosrun//启动一个节点
roslaunch//启动多个节点
rosclean//删除或者查看日志 rosclean check rosclean purge信息命令
rostopic:
image-20250310100115429
rosservice:image-20250310101224524 rosparam :
image-20250310101400110 rosmsg:
image-20250310101433347 rosrv:
: image-20250310101459110 image-20250310101637307 catkin 命令 :
image-20250310101745650
catkin_make:catkin构建系统的构建
catkin_create_pkg:创建软件包rospack //线程包信息 //rospack depends-on //rospack find //rospack list //rospack depends //rospack profile rosinstall //安装ros附加功能包 rosdep roslocate roscreate-pkg rosmake //旧版的rosbuild系统构建rostopic //确认话题信息 rosservice //确认服务信息 rosnode //确认节点信息 //rosnode list ->列出所有的节点 //rosnode ping-》对指定节点进行链接测试 //rosenode info-》检查节点信息 //rosnode kill -》杀死指定节点 //rosnode machine-》查看当前pc上面的所有节点 //rosnode cleanup -》删除无法验证的虚拟节点的注册信息 rosparam //确认和修改ros参数信息 rosbag //记录和回访ros信息 rosmsg //显示ros消息类型
工具
RVIZ
显示屏（display):
image-20250310103400448 image-20250310103420596
编程
标准单位的使用：
//坐标表示的时候
//ros x:forward y:left z:up
//旋转方向符合右手定则，以x轴向正向为卷手方向topic《发布者和订阅者创建》
创建发布者：
#include<ros/ros.h>
#include<包名/消息文件名>//这个相当于一个类，定义一个由简单类型组成的类
int main(int argc ,char** argv )
{
//首先初始化ros
ros::init(argc ,argv ,"节点名称");
ros::NodeHandle nh;//创建操作句柄
//创建发布者和话题，rostopic list 可以查看 rosnode info
ros::Publiser ros_pub = nh.advertise<包名::消息文件名不带后缀>("topic的名称"，100);
//设置消息发布的频率，这个单位是赫兹，表示一秒发布多少次消息
ros::Rate loop_rate(10);//ros::timer，基于事件的时间回调
节点名称::消息文件名 msg;
while(ros::ok())//循环条件
{
//msg.[成员]可以在这里操作msg的数据
ros_pub.publish(msg);
loop_rate.sleep();//让频率更加稳定，防止超负荷的情况
//处理一些数据
}
return 0;
}创建订阅者
#include<ros/ros.h>
#include<包名/消息文件名>//这个相当于一个类，定义一个由简单类型组成的类
//订阅者是回调模式，首先定一个回调函数，参数是 节点名称::消息文件名&
void MsgCallback(const 节点名称::消息文件名::ConstPtr & msg)
{
//消息处理逻辑
}
int main(int argc ,char** argv)
{
ros:init(argc ,argv ,"节点名称");
ros::NodeHandle nh ;
//回调注册
ros::Subscriber ros_sub = nh.subscribe("topic的名称（和发布者保持一致)",100,MsgCallback);
ros::spin()//持续监听topic上的消息，有消息，执行回调
//ros::spinOnce()，回调一次后执行后续代码
//ros::AsyncSpinner(),可以使用多线程对topic进行监听，用于计算密集型和高频回调函数
}service服务端和客户端
创建service服务端
#include<ros/ros.h> #include<包名/服务文件名> //服务端作为一个继续运行的节点，通过接受请求，然后返回回复，客户端一次性的，接受到回复之后就会断开链接。 //服务端有一个回调函数，参数是 包名::服务文件名::request& request,包名::服务文件名::response & response //一元断言 bool cal(ros_server::srvfile::request & req,ros_server::srvfile::response & rep) { //回调函数执行对请求处理的函数 //ROS_INFO->ros的标准输出宏，类似printf的格式进行输出 rep.result = req.a+req.b; return true; } int main(int argc ,char** argv) { ros::init(argc,argv,"节点名称"); ros::NodeHandle nh; //创建服务并注册回调 ros::ServiceServer srv = nh.advertiseService("服务名",cal); ros::spin();//持续运行监听请求，会持续阻塞 return 0; }创建service客户端
#include<ros/ros.h> #include<包名/服务文件名> int main(int argc ,char** argv) { ros::init(argc.argv,"节点名"); if(argc!=3)//说明传入参数不对，这个依赖于具体实现来考虑是否需要做判断 { return 1; } ros::NodeHandle nh; //句柄建立 ros::ServieClient client = nh.serviceClient<包名::服务名>("服务端设置的服务名"); //手动创建一个服务类型，并对服务请求进行相应的处理 包名::服务名 srv ; //处理srv.request; //手动发送信息 //call if(client.call(srv))//为真发送成功 { //打印输出相关信息 } else { //输出错误信息 return 1; } return 0; }
action服务端和客户端
image-20250311095100152 动作服务端:包括设置任务执行方式，接受客户端指令，返回对应的目标。
#include<ros/ros.h> #include<actionlib/server/simple_action_server.h> #include"包名/动作名Action.h" //动作也是一个回调的过程。书本模板中，创建了一个动作类，主要包括了如下：action、result、feedback class Action { public: //定义一个构造函数 action(std::string action_name) :_ac(_nh,action_name,std::bind(&Action::exeto,this,placeholders::_1),flase) ,_name(action_name) { _ac.start(); } void exeto(const 包名::动作名GoalConstPtr& goal) { //初始化feedback.sequence //设置回调里面的发送频率 ros::Rate rate(1); bool success; _feed.dequence.clear(); _feed.sequence.push_back(0); _feed.sequence.push_back(1); for(int i=1;i<goal->order;++i) { if(!ros::ok()||_ac.isPreemptRequest()) { ROS_INFO("be preempted"); _ac.setPreempted(); success=false; break; } _feed.sequence.push_back(_feed.sequence[i]+_feed.sequence[i-1]); _ac.publishFeedback(_feed); rate.sleep(); } if(success) { _result.sequence = _feed.sequence; _ac.setSucceeded(_result); } return ; } private: ros::NodeHandle _nh; actiolib::SimpleActionServer<包名::动作名Action> _ac // 可以在/home/rxx/devel/include/对应包下面查看生成的.h文件 std::string _name; 包名::动作名Feedback _feed; 包名::动作名Result _result; }; int main(int argc ,char** argv) { ros::init(agc,argv,"节点名称"); Action action("action_server");//名字自己选 ros::spin(); return 0; }动作客户端:
#include<ros/ros.h> #include<actionlib/client/simple_action_client.h> #include<actionlib/client/terminal_state.h> #include"包名/动作名Action.h" int main(int argc,char** argv) { ros::init(argc,argv ,"节点"); actionlib::SimpleActionClient<包名::动作名Action> ac ("action_server"); //等待服务器 ac.waitForServer(); 包名::动作名Goal goal ; goal.order=20; ac.sendGoal(goal); bool not_out_time = ac.waitForResult(ros::Duration(30,0)); if(not_out_time) { actionlib::SimpleClientGoalState state= ac.getstate(); //ROS_INFO("%s: action finish",state.toString().c_str()); } else { ROS_INFO("out of time"); } return 0; }
动态参数与静态参数和roslaunch
//静态参数 ros::NodeHanle nh; nh.setParam("param_name","被修改的值"); nh.getParam("param_name","被修改的值"); //从外部有 rosparam set /param_name "设置的值"动态参数
//.cfg //通过python语言编译,写一个.cfg文件，主要是调用add加参数、设置参数上下限 //首先在.cfg文件里面定义包名： PACKAGE = "packagename" from dynamic_reconfigure.parameter_generator_catkin import * //创造一个参数生成器 gen = ParameterGenerator() //gen.add(name(参数名),type(定义存储值的类型),level(动态回调的掩码),description,default,min ,max) //对于枚举类型 size_enum = gen.enum([gen.const("Low,",int_t,0,"Low:0") gen.const("Medium",int_t , 1,"Medium:1") gen.const("High",int_t ,2,"High:2")] ,"Selection List" ) gen.add("SIZE",int_t,0,"Selection List",1,0,2,edit_method = size_enum) //exit()用于退出并且生成所需的文件 exit(gen.generate(PACKAGE,"节点名称"，"生成的头文件名称"))//.cpp //静态参数有getparam(),动态参数类似消息一样 #include<ros/ros.h> #include<dynamic_reconfigure/server.h> #include"包名/.cfg中生成的头文件名称Config.h" //回调函数 void callback(包名::cfg中生成的头文件名称Config & config,unint32_t level) { ROS_INFO()//打印参数改变之后的数值 } int main(int argc ,char** argv) { ros::init(argc ,argv ,"node_name"); dynamic_reconfigure::Server<包名::cfg中生成的头文件名称Config> server; dynamic_reconfigure::Server<包名::cfg中生成的头文件名称Config> ::CallbackType f; f=boost::bind(&callback,_1,_2); server.callback(f); //进入监听回调 ros::spin(); }roslaunch:
image-20250312100436950 //新建launch文件夹新建.launch文件 //文件格式采用xml格式 <launch> <arg name="sim_mode" default="true" /> <group if="$(arg sim_mode)"> <node pkg="gazebo_ros" type="gazebo" name="gazebo" respawn="true" /> </group> <node pkg="move_base" type="move_base" name="nav"> <remap from="cmd_vel" to="robot/cmd_vel" /> <rosparam command="load" file="$(find nav_pkg)/config/costmap.yaml" /> </node> <param name="robot_description" command="$(find xacro)/xacro $(find urdf_pkg)/urdf/robot.urdf.xacro" /> </launch> //param的用法 <param name="config"> <value type="yaml"> {key1: value1, key2: [1, 2, 3]} </value> </param> //test <test test-name="test_node" pkg="pkg" type="test_node.py" /> //设置节点环境 <env name="ROS_IP" value="192.168.1.100" /> //重映射话题名 <node pkg="pkg" type="node" name="node_name"> <remap from="old_topic" to="new_topic" /> </node> //分组管理节点 <group ns="robot1"> <node pkg="pkg" type="node" name="controller" /> </group> //rosparam(可以直接加载yaml文件或者直接输入参数) <rosparam command="load" file="$(find pkg)/config/params.yaml" /> //include(可以包含另一个launch文件) <include file="$(find pkg_name)/launch/other.launch" /> //param 引用传递参数 <param name="sim_mode" value="$(arg use_sim)" /> //可以经由命令行输入参数 //roslaunch package_name file.launch use_sim:=false max_speed:=2.5
TF库进行坐标转换
坐标转换看作是
旋转+移动,在机器人方向叫做“位姿”变换。考虑旋转：求取旋转矩阵：视作各轴在对应轴上的投影
旋转矩阵表示有当前坐标转移到目的坐标的矩阵，每行表示对应的的轴在目的坐标三个轴方向上的投影。
位移：每个基于坐标系 B 的向量先进性旋转变换，再与向量 O1O2 求和即可得到向量 P 再坐标系 A 中的实际映射。
坐标变换 = 旋转 + 平移，因此坐标变换表达式如下所示：
这样不利于矩阵运算，我们可以改写为如下形式：
其中 O1O2 是从 O1 指向 O2 的向量。
欧拉角
欧拉角遵循的是右手系规则，即大拇指指向坐标轴正方向，四指旋转的方向即为转动的正方向，欧拉角包含三个自由量：yaw(偏航角，z)、pitch(俯仰角,y)、roll(翻滚角,x)。
滚转角(x)、偏航角(z)、俯仰角(y)
我们将三次旋转分开讨论，我们以绕 Z 轴旋转为例来进行说明：
绕 Z 轴旋转的三维立体图如上所示，为了方便，我们查看一下二维旋转图：
欧拉角可以转化为旋转矩阵，并且利用旋转矩阵的知识进行旋转操作。向量在二维平面上旋转可以让我们联想到向量在复平面中的旋转：
设向量 P 的坐标为 (x,y)，则 XOY 平面变换公式如下所示：
由于 “绕谁谁不变” 的原则，因此 Z 轴坐标不会发生改变，最终 Z 轴旋转变换公式如下所示：
同理，我们可以按照同样的方式得出到 X/Y 轴旋转的公式：
旋转矩阵的旋转顺序分为外旋（x->y->z）和内旋（z->y->x），我们一般使用外旋的顺序：
但是欧拉角存在万向节死锁问题：由于某一轴旋转了90°，某些时刻，造成了自由度的缺失。
因为欧拉采用的是矩阵计算的方式当连续两次为90°的整数的旋转的时候，这时候角度偏差的向量积接近于0，容易导致第三个向量的结果失效。
四元数
利用四元数
四元数的可视化
上述视频链接如下所示：四元数的可视化_哔哩哔哩_bilibili
https://www.bilibili.com/video/BV1SW411y7W1?from=search&seid=2286694305504614618&spm_id_from=333.337.0.0 上述四元数的物理解释较难理解，而我喜欢把它理解为一种旋转算法。数学之美就在于其揭示了许多新的计算方法，幸运的是，四元数就是一种用于物体旋转的计算方法。
tf坐标代码示例
#include<ros/ros.h> #include<tf/transformbroadcaster.h> #include<turtulesim/Pose.h> std::string turtlename; void Posecallback(const turtlesim::PoseConstPtr& msg) { static tf::TransformBroadCaster br; tf::Transform transform; transform.setOrigin(tf::Vector3(msg->x,msg->y,0.0)); tf::Quaternion q; q.setRPY(0,0,,msg->theta); transform.setTotation(q); br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"world",turtlename)); int main(int argc ,char**argv ) { //if(argc !=2){//处理错误} turtlename = aargv[1]; ros::init(argc ,argv,"node"); ros::NodeHandle nh; ros::SUbscriber sub = nh.subscribe("topic_name",10,&Posecallback); ros::spin(); return 0; } }listener:
#include<ros/ros.h> #include<tf/transform_listener.h> #include<geometry_msgs/Twist.h> #include<turtlesim/Spamn.h> int main(int argc ,char**argv) { ros::init(argc ,argv ,"node"); ros::NodeHandle nh; ros::service::waitForService("spawn"); ros::ServiceClient add_turtle = nh.serviceClient<turtlesim::Spawn>("spawn"); turtlesim::Spawn srv; add_turtle.call(srv); ros::Publisher turtle_vel = n.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10); tf::TransformListener listener; ros::Rate(10); while(nh.ok()) { tf::StampedTransform transform ; try { lsitener.lookupTransform("/turtle2","turtle1",ros::Time(0),tranform); } catch (tf::TransformException& ex) { ROS_ERROR("wrong"); ros::DUration(1.0).sleep(); continue; } geometry_msgs::Twist vel_msg; vel_msg.angular.z= 4.0* atan2(transform.getOrigin().y(),transform.getOrigin().x()); vel_msg.linear.x=0.5*sqrt(pow(transform.getOrigin().x(),2),pow(transform.getOrigin().y(),2)); turtle_vel.pushlish(vel_msg); rate.sleep(); return 0; } }
gdb
catkin_make _DCMAKE_BUILD_TYPE=debug,生成可执行debug文件
在.launch文件里面，launch-prefix =" xterm -e gdb -ex --args"
日志
DEBUG INFO WARN ERROR FATAL 阻止程序继续运行的错误 rqt_console、rqt_logger_level
可以在.launch文件里面包含环境
<env name ="ROSCONSOLE_CONFIG_FILE" value ="***.config"/>在.config文件里面写上
log4j.logger.ros.packname=ERROR日志还可以设置命名消息
ROS_INFO_STREAM_NAMED("name","mesage");//带有stream 的支持stl样式的输出 //修改配置文件加上对命名信息定义级别 log4j.logger.ros.packname.named_msg=ERROR日志可以设置条件消息
ROS_INFO_STREAM_COND(val<0.,"message" <<value<<"message" );//支持stl样式输出日志还可以设置
ROS_INFO_STREAM_ONCE()//控制显示输出的次数 //但是以计时器作为辅助的打印更加优秀 ROS_INFO_STREAM_THROTTLE(周期,"message");ros::Duration(1).sleep());
代码
codescan
src文件解释
error_code
// 利用unordered_map<enum errorcode , string error>记录对应枚举值的报错信息cmd_channel
//这个文件较为复杂
enum CmdChannel {}//枚举了cmd_channel状态
enum CmdType {}//枚举了命令执行的类别
enum CmdStatus {}//枚举了命令执行的状态
enum TaskStatus {}//枚举请求的状态
enum HearStatus {}//枚举了心跳的状态
//有几个关键的类型别名
using ResBack = std::pair<std::string,CmdStatus>;//便于封装和隐藏
using TaskDeScQueue = std::list<TaskDesc> ;
using CmdMap = std::map<CmdType,TaskDescQueue>;
typedef std::function<void(ResBack) CallType;
using CallFunc = std::shared_ptr<CallType>;
class ScanTool {}//里面只有一个函数WaitFor(func,starttime,outtime);将函数封装在这个列别里面，一个类对应一个回调函数
class TaskDesc {
std::atomic<TaskStatus> _status;//以原子量来存储一个简单任务状态的枚举类型来做任务分类判断
std::promise<ResBack> _res; //一种异步的通信类，可以实现线程同步，采用异步的方式通过
//promise.get_future;futrue.set_value;来实现异步通信，可以看到这个托管的类是一个状态的标志位置，也是用于逻辑判断的
std::shared_ptr<std::function<void(ResBack)>> _callback;//回调函数注册并且用智能指针托管
CmdType CmdType;//任务类别
}
//主要类之一，包括类创建时间，超时时间，初始化的时候设置任务的初始状态为无效
class MessageInfo
{
bool endflag;
vector<CmdType> ;
}//todo
class CmdChannel //核心类
{
//建立链接
//存储信息ip、端口、链接状态、类自身标记、锁
std::atomic<CmdChannelStatus>;
std::list<std::future<void>> defer_queue;
defer_queue.push_back(std::async(std::launch::async(),[iter,res]
{
if(iter->callback) (*iter->callback)(res);
iter->status.store(Taskstatus::done);
});
//这个涉及一个std::futrue;std::promise
std::atomic<HeartStatus> heart_status;
//函数包括：构造、链接、循环
//任务命令实现：扫描、聚焦、复位（这三个任务具体是否需要实现根据设备不同而有所区别，具体以协议为准。
std::shared_future<ResBack> scan();
CmdMap cmd_m ;//存储所有任务
void tellallinfo(CmdType , ResBack)//遍历上面的codemap，并且需要两个参数分别是CmdType和ResBack，用来修改状态，如果命令类型相同就执行任务并且，把状态修改，同时在遍历的时候检测如果任务已经取消就从list里面去掉，同时给所有对应命令的任务加上ResBack的属性。
static unordered_map<string,struct messageinfo> hash;
void receivedata();//关键函数
static unordered_map<CmdType,string> cache;
//函数内部设置静态的哈希表检测通信请求方发来的是那种消息，同时使用slecte监听链接，确认selecte 创建读事件成功，确认fd的可读成立，接受数据放到string，通过哈希表确认消息字段里面是否有哈希表的first，如果有再继续处理，然后通过struct message 里面的end_flag来判断是否需要往本函数中声明的一个静态的map中存放数据,注册退出处理函数修改原子量的CmdChannelStatus，然后触发continue再继续循环判断hash的string字段是否再消息里面出现如果出现并且end_flag不假的话走到对当前cache的string判断空的逻辑，如果监测到有值的话，那么置换内容并发布消息(调用tellainfo)，这个地方有好些逻辑，如果是LON的回复如果是正常的情况就可以直接发送接受到消息，如果是ERROR就说明LON(开始读取)成功LOFF(读取成功)成功状态启动,TUNE的三种状态逻辑可以仔细看看。
//心跳任务注册
void HeartBeat();//判断心跳的状态，如果是wait状态，那么尝试改变心跳状态并且执行docmd(string,time,callback);callback是函数中利用std::function封装的对HeartBeatCalllback(res)的执行，如果执行成功就返回，如果执行失败就会继续往下执行，如果是心跳检测成功就检测是否到达的心跳检测时间，如果是错误就该改变心跳状态之后重试一次。
void HeartBeatCallback(ResBack res);//修改心跳时间，如果res的状态是成功那么就可以将心跳状态设置为陈工，如果不是的话就会将心跳状态设置为失败。
std::shaerd_future<Resback> scancode();
std::shaerd_future<Resback> foucs();
std::shaerd_future<Resback> reset();
std::shaerd_future<Resback> docmd(string,time , callbcak);//这个函数内部生产Taskesc并且存放到这个类的存储区CmdMap里面，并在这里面创建scantool类对象，执行waitfor()，waitfor要求一个可调用对象类型，通过lambda表达式进行设置，逻辑是这个整个,检查到返回之后进行判断是否是超时或者有链接，有链接进开展读任务，根据任务类型发送消息。
auto flag_timeout = tool.waitfor([this]->bool
{
auto statu = this->status.load();
if(statu == CmdChannelStatus::CONNECTED)
{
//尝试修改
auto item = CmdChannelStatus::CONNECTED;
bool out_time=this->status.compare_exchange_strong(Cmd::CONNECTED,Cmd::BUSY)
if(out_time)
{
return false;
}
return true;
}
})
//这个地方compare_exchange_strong 是集合条件和交换一体的。
void loopcheck()//检测已完成并且超出时间，检测未完成但是已经超时的就发消息说超时。
void dealdeferqueue();
}uart_gpio
gpio.h
start()
- 首先在main里面构造单例的gpio并调用start函数启动
- 在启动函数里面加载yaml文件里面的参数
- 初始化输出端口状态,并且启动串口SerialPort()(同样单例)
- 加载串口类设置的yaml文件，打开fd[^端口地址由配置文件决定/dev/ttyso]
- fd的DTR位设置低电压，然后去设置串口的波特率以及对应的c_cflag 、c_iflag 、c_lflag 等等，成功后改变串口类的设置状态
- 循环判断串口类的设置状态，如果没有成功就等待一段时间之后进行重新判断。
- 成功之后所有串口设置完毕，记录下端口的晶体管类型
- 根据数据源选择是利用PLC还是UART更改设置串口属性（收消息、判断接口状态、修改fd）
- 同时订阅屏蔽信号
out_port_controller
- 首先加载配置文件。
- 初始化端口状态（调用GPIO）里面的设置输出端口。
- 订阅自动充电话题，出入回调
- 订阅设置输出端口话题，设置回调
- 订阅顶升话题设置回调
- 订阅急停话题设置回调
- 订阅正常端口修改话题并注册回调
- 订阅后向叉车话题，并注册回调
light_manager
- 加载参数，给所有的灯光进行初始化。
- 单例创建DmxLIghtManager ，这个类继承了LightDeviceManager，添加一些对外的接口函数，继承了LightDeviceManager的大部分功能
- 订阅vehicle_state，这个topic控制的是灯光的显示效果。
- 然后建立一个服务，并注册回调函数LightConfig_Callback .
- 启动一个线程，线程运行ThreadFunc函数，这个函数利用stdtime_point/std::chrono::system_clock,std::chrono::nanoseconds/记录当前系统时间，然后进入循环条件，通过sleep_for()控制线程运行的频率，
- 检测状态变化，如果变化记录新的基准时间，然后进入对于光设备的便利根据灯光的设置的模式控制灯光的显示效果，stdduration_cast/std::chrono::milliseconds/(a-b).count();(可以记录时间间隔的标准函数）
- 比较间隔时间与设备的每亮一次的时间的倍数关系，如果可以整说明时间同步否则不同步。其他的设置模式就具体看细节。
- 把模式设置到对应的设备组类
state_publisher
- 加载配置文件
- 初始化所有的topic，然后进入线程对于所有接口状态进行判断，然后每次循环中依次对每个状态进行修改。
- 发布对应的信息