文章目录
【ROS基础】
1 常用命令
rosbag
循环播放
rosbag play -l xx.bag
对于一个bag，如何抽取其图像话题
rosbag play clxt02.bag
rosbag record -O only_img /usb_cam/image_raw/compressed
rosbag record -O only_lidar /lslidar_point_cloud
//保存only_lidar.bag
查看话题内容
rostopic list
rostopic echo xx
cmath
int abs(int i);//返回整型参数i的绝对值
double fabs(double x);//返回双精度参数x的绝对值
long labs(long n);//返回长整型参数n的绝对值
double exp(double x);//返回指数函数e^x的值
double log(double x);//返回logex的值
double log10(double x) 返回log10x的值
double pow(double x,double y) 返回x^y的值
double pow10(int p) 返回10^p的值
double sqrt(double x) 返回+√x的值
double acos(double x) 返回x的反余弦arccos(x)值,x为弧度
double asin(double x) 返回x的反正弦arcsin(x)值,x为弧度
double atan(double x) 返回x的反正切arctan(x)值,x为弧度
double cos(double x) 返回x的余弦cos(x)值,x为弧度
double sin(double x) 返回x的正弦sin(x)值,x为弧度
double tan(double x) 返回x的正切tan(x)值,x为弧度
double hypot(double x,double y) 返回直角三角形斜边的长度(z),
x和y为直角边的长度,z^2=x^2+y^2
【ROS模板】
节点模板
一个节点同时订阅多个话题和发布多个话题
C++
1 cpp
#include <ros/ros.h>
#include <time.h>
#include <vector>
#include <cmath>
#include <iostream>
using namespace std;
class sub_pub
{
private:
//创建节点句柄
ros::NodeHandle nh;
//声明订阅器变量，未初始化，可声明多个
ros::Subscriber sub;
//声明发布器变量，未初始化，可声明多个
ros::Publisher pub;
//定时器
ros::Timer o_timer;
public:
//构造函数
sub_pub()
{
//订阅器初始化
sub = nh.subscribe("话题名称", 缓冲区数据大小, &sub_pub::Callback, this);
//发布器初始化
pub = nh.advertise<数据类型>("话题名称",缓冲区数据大小);
//定时器初始化;
o_timer=nh.createTimer(ros::Duration(1), &obu_to_xy::o_timer_callback, this);
}
//回调函数，实现自己的算法
void Callback();
void o_timer_callback(const ros::TimerEvent& event);
};
void sub_pub::o_timer_callback(const ros::TimerEvent& event)
{
// pub_markers_obu.publish(markers_all_this_frame);
// std::cout<<"pub one markers all"<<endl;
if(o_add_flag==1)
{
double tmp=ros::Time::now().toSec();
int delta_time=tmp-o_time;
if(delta_time>o_life_time)
{
o_add_flag=0;
}
}
}
int main(int argc, char **argv)
{
//节点初始化
ros::init(argc, argv, "sub_pub");
//类的实例化
sub_pub sub_puber;
//进入自循环，调用所有的回调函数（当接收到消息）
ros::spin();
return 0;
}
#include <ros/ros.h>
#include <time.h>
#include <vector>
#include <cmath>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Float64.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <pcl/filters/passthrough.h> //直通滤波器头文件
#include <pcl/filters/voxel_grid.h> //体素滤波器头文件
#include <pcl/filters/statistical_outlier_removal.h> //统计滤波器头文件
#include <pcl/filters/conditional_removal.h> //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h> //半径滤波器头文件
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <Eigen/Core>
#include <time.h>
#include <vector>
#include <cmath>
using namespace std;
2 CMakeLists
pcl
omp
opencv
eigen
cmake_minimum_required(VERSION 2.8.3)
project(your_name)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
# sensor_msgs CATKIN_DEPENDS
# pcl_ros CATKIN_DEPENDS
)
# find_package(PCL 1.7 REQUIRED)
catkin_package(
INCLUDE_DIRS include
# 和find package对上，和xml对上
CATKIN_DEPENDS roscpp rospy std_msgs
)
# set(EIGEN_INC_DIR /usr/include/eigen3/)
# find_package(OpenCV)
# find_package(OpenMP)
# if (OPENMP_FOUND)
# set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
# set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
# set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
# endif()
include_directories(
include
${catkin_INCLUDE_DIRS}
# ${OpenCV_INCLUDE_DIRS}
# ${EIGEN_INC_DIR}
)
# link_directories(${PCL_LIBRARY_DIRS})
#######################################一个lib
#add_library(your_name
# src/classA.cpp
#)
#add_dependencies(your_name ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#######################################一个lib
#######################################一个cpp
#①添加cpp文件
add_executable(your_name src/your_name.cpp src/your_name.cpp)
#②链接库
target_link_libraries(your_name
${catkin_LIBRARIES}
# ${PCL_LIBRARIES}
# ${OpenCV_LIBS}
)
#③先编译这个节点的依赖，再编译这个节点
add_dependencies(your_name ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
#######################################一个cpp
3 xml
<?xml version="1.0"?>
<package format="2">
<name>your_name</name>
<version>0.0.0</version>
<description>The your_name package</description>
<maintainer email="qm@todo.todo">qm</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<!-- must pack -->
<build_depend>roscpp</build_depend>
<build_export_depend>roscpp</build_export_depend>
<exec_depend>roscpp</exec_depend>
<build_depend>rospy</build_depend>
<build_export_depend>rospy</build_export_depend>
<exec_depend>rospy</exec_depend>
<build_depend>std_msgs</build_depend>
<build_export_depend>std_msgs</build_export_depend>
<exec_depend>std_msgs</exec_depend>
<!-- system pack -->
<!-- <build_depend>qm_txt</build_depend>
<build_export_depend>qm_txt</build_export_depend>
<exec_depend>qm_txt</exec_depend> -->
<!-- your pack -->
<!-- <build_depend>qm_txt</build_depend>
<build_export_depend>qm_txt</build_export_depend>
<exec_depend>qm_txt</exec_depend> -->
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
4 类 当前包调用
add_executable(xx_node src/class_A.cpp src/xx_node.cpp)
#其中class_A是要用的类
5 类 全局调用
方法一（推荐）
- hpp写好类
- 写好该hpp需要CMakLists和xml添加什么东西的readme
- 系统的node最好放一个节点里面
include下写一个hpp文件，src的node里面直接include这个hpp即可
好处：什么都不用改
坏处：当前包有用
建议建立一个util文件夹，里面都是hpp文件和hpp的readme文件。（readme主要写要用这个hpp需要什么样的cmakelist和xml）
哪个ros包需要，直接复制一个到include里面去
方法二 （最好！！）
如果
// #include <pcl/visualization/pcl_visualizer.h>
切cmake声明了pcl，则巨慢无比
在B_package下使用A_package的类(类文件）
A_package
add_library(A_package
#include/A_package/classA.h
src/classA.cpp)
classA.h 包括全部函数与变量
classA.cpp只有一句话
#include<A_package/classA.h>
B_package
find_package(catkin REQUIRED COMPONENTS
A_package
)
<build_depend>A_package</build_depend>
<exec_depend>A_package</exec_depend>
python
基础格式
#! /usr/bin/env python3
#python2
import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
# from ros_numpy import msgify
# from cv_bridge import CvBridge
import sys
sys.path.remove('/opt/ros/kinetic/lib/python2.7/dist-packages')
#python3
class SubscribeAndPublish:
def __init__(self):
self.pub1 = rospy.Publisher('pub1',String,queue_size=20)
self.pub2 = rospy.Publisher('pub2',String,queue_size=20)
self.sub1 = rospy.Subscriber('pub1',String, self.callback1)
self.sub1 = rospy.Subscriber('pub2',String, self.callback2)
self.timer1 =rospy.Timer(rospy.Duration(1), self.timer1_callback)
def callback1(self,data):
print('rec1',data)
def callback2(self,data):
print('rec2',data)
def timer1_callback(self,event):
print("i am timer")
msg1=String()
msg1.data = "1111111111111111111"
msg2=String()
msg2.data = "2222222222222222222"
self.pub1.publish(msg1)
self.pub2.publish(msg2)
# self.pub2.publish('222222222222222222')
def main():
rospy.init_node('node_python_template1', anonymous=True)
SAPObject = SubscribeAndPublish()
rospy.spin()
if __name__ == "__main__":
main()
发布图像（无须 s b cv_bradge)
同时使用 python 3 于2
#! /usr/bin/env python3
#python2
import roslib
import rospy
from std_msgs.msg import Header
from std_msgs.msg import String
from sensor_msgs.msg import CompressedImage
from sensor_msgs.msg import Image
# from ros_numpy import msgify
import sys
sys.path.remove('/opt/ros/melodic/lib/python2.7/dist-packages')
#python3
import numpy as np
import cv2
class SubscribeAndPublish:
def __init__(self):
self.width=rospy.get_param("~width")
self.height=rospy.get_param("~height")
self.fps=rospy.get_param("~fps")
self.camera_id=rospy.get_param("~camera_id")
print('launch info :',self.width ,self.height,self.fps,self.camera_id)
self.cap=None
self.init_camera()
self.frame=0
self.pub1 = rospy.Publisher('/usb_cam/image_raw', Image,queue_size=20)
self.timer1 =rospy.Timer(rospy.Duration(0.02), self.timer1_callback)
self.timer12=rospy.Timer(rospy.Duration(0.033), self.timer2_callback)
def init_camera(self):
#cap = cv2.VideoCapture(1,cv2.CAP_DSHOW)
self.cap = cv2.VideoCapture(self.camera_id)
self.cap.set(6,cv2.VideoWriter.fourcc('M','J','P','G'))# 视频流格式
self.cap.set(5, self.fps);# 帧率
self.cap.set(3, self.width)# 帧宽
self.cap.set(4, self.height)# 帧高
tmp_width = self.cap.get(3)
tmp_height = self.cap.get(4)
tmp_frame = self.cap.get(5) #帧率只对视频有效，因此返回值为0
#打印信息
print('camera info get:',tmp_width ,tmp_height,tmp_frame)
def timer1_callback(self,event):
_, self.frame = self.cap.read()
def timer2_callback(self,event):
image_temp=Image()
header = Header(stamp=rospy.Time.now())
header.frame_id = 'map'
image_temp.height=1080
image_temp.width=1920
image_temp.encoding='bgr8'
# image_temp.data=np.array(frame).tostring()
image_temp.data=np.array(self.frame).tobytes()
image_temp.header=header
image_temp.step=(self.width*3)
self.pub1.publish(image_temp)
def main():
rospy.init_node('node_qm_usb_camera', anonymous=True)
SAPObject = SubscribeAndPublish()
rospy.spin()
if __name__ == "__main__":
main()
python launch 文件
<launch>
<node name = "qm_usb_cam" pkg = "qm_usb_cam" type = "one_camera.py" output = "screen">
<param name = "pub_topic_name" type="string" value = "/usb_cam/image_raw"/>
<param name = "width" type="int" value = "1920"/>
<param name = "height" type="int" value = "1080"/>
<param name = "fps" type="int" value = "30"/>
<param name = "camera_id" type="int" value = "0"/>
</node>
</launch>
【ROS高级】
1 项目文件结构
1.1 功能包套娃
功能包可以处于任意文件夹，名字需要ws内唯一即可，findpackage只需要根据名字即可，无需关注路径
1.2 全局msgs
①新建一个msgs的文件夹
②里面建立一个功能包perception_msgs
该功能包名字就是自定义消息名字
内容只有msg/obstacle.msg msg/obstaclearray.msg Cmakelists xml
然后建立一个功能包obu_msgs
套娃。。。
③别的pkg调用方法
cmakelists
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
perception_msgs
)
xml
<build_depend>perception_msgs</build_depend>
<build_export_depend>perception_msgs</build_export_depend>
<exec_depend>perception_msgs</exec_depend>
1.3 每个节点都是类
1.3.1 C++
class obu_to_xy
{
private:
//创建节点句柄
ros::NodeHandle nh;
//声明订阅器
ros::Subscriber sub_obu;
ros::Subscriber sub_lidar;
//声明发布器
ros::Publisher pub_markers_obu;
ros::Publisher pub_markers_lidar;
//定时器
ros::Timer o_timer;
public:
//构造函数
obu_to_xy()
{
//订阅
sub_obu = nh.subscribe(sub_obu_str, 1, &obu_to_xy::callback_rec_obu, this);
sub_lidar = nh.subscribe(sub_lidar_str, 1, &obu_to_xy::callback_rec_lidar, this);
//发布
pub_markers_obu = nh.advertise<visualization_msgs::MarkerArray>("/pub_markers_obu",1);
pub_obu = nh.advertise<obu_msgs::OBU_fusion>(pub_obu_str,1);
pub_lidar = nh.advertise<perception_msgs::ObstacleArray>(pub_lidar_str,1);
//定时
// init_arrays();
o_timer=nh.createTimer(ros::Duration(1), &obu_to_xy::o_timer_callback, this);
};
//回调函数
void callback_rec_obu(obu_msgs::OBU_fusion t);
void callback_rec_lidar(perception_msgs::ObstacleArray t);
void o_timer_callback(const ros::TimerEvent& event);
};
1.3.2 python
可视化 marker与makerarray
pub_markers_obu = nh.advertise<visualization_msgs::MarkerArray>("/pub_markers_obu",1);
产生一个空的marker
visualization_msgs::MarkerArray obu_to_xy::make_one_empty_markerarray()
{
visualization_msgs::MarkerArray t;
return t;
}
产生一个空的marker array
visualization_msgs::Marker obu_to_xy::make_one_empty_marker()
{
visualization_msgs::Marker t;
t.header.frame_id = "fake";
t.header.stamp = ros::Time::now();
// Set the namespace and id for this marker. This serves to create a unique ID
// Any marker sent with the same namespace and id will overwrite the old one
t.ns = "obu_marker";
t.id = 0;
// Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
t.type = visualization_msgs::Marker::CUBE;
// Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
t.action = visualization_msgs::Marker::ADD;
// Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header
t.pose.position.x = 0;
t.pose.position.y = 0;
t.pose.position.z = 0;
t.pose.orientation.x = 0.0;
t.pose.orientation.y = 0.0;
t.pose.orientation.z = 0.0;
t.pose.orientation.w = 0.0;
// Set the scale of the marker -- 1x1x1 here means 1m on a side
t.scale.x = 0.0;
t.scale.y = 0.0;
t.scale.z = 0.0;
// Set the color -- be sure to set alpha to something non-zero!
t.color.r = 0.0f;
t.color.g = 1.0f;
t.color.b = 0.0f;
t.color.a = 1.0;
t.lifetime = ros::Duration(1);
return t;
}
void tilt_to_horizon::pub_markers_from_obs(perception_msgs::ObstacleArray obs_array)
{
visualization_msgs::MarkerArray tmp_marker_array;
tmp_marker_array=make_one_empty_markerarray();
for(int i=0;i<obs_array.obstacles.size();i++)
{
visualization_msgs::Marker tmp_marker;
tmp_marker=make_one_empty_marker();
//写入obs信息
tmp_marker=make_one_empty_marker();
tmp_marker.id=i;
tmp_marker.pose.position.x=obs_array.obstacles[i].pose.position.x;
tmp_marker.pose.position.y=obs_array.obstacles[i].pose.position.y;
tmp_marker.scale.x=obs_array.obstacles[i].scale.x;
tmp_marker.scale.y=obs_array.obstacles[i].scale.y;
tmp_marker.scale.z=obs_array.obstacles[i].scale.z;
tmp_marker.color.r = 1.0f;
tmp_marker.color.g = 0.0f;
tmp_marker.color.b = 0.0f;
tmp_marker.pose.orientation.z = obs_array.obstacles[i].pose.orientation.z;
tmp_marker.pose.orientation.w = obs_array.obstacles[i].pose.orientation.w;
//
tmp_marker_array.markers.push_back(tmp_marker);
}
pub_markerarray.publish(tmp_marker_array);
}
ARROW=0//箭头
uint8 CUBE=1//立方体
uint8 SPHERE=2//球
uint8 CYLINDER=3//圆柱体
uint8 LINE_STRIP=4//线条（点的连线）
uint8 LINE_LIST=5//线条序列
uint8 CUBE_LIST=6//立方体序列
uint8 SPHERE_LIST=7//球序列
uint8 POINTS=8//点集
uint8 TEXT_VIEW_FACING=9//显示3D的文字
uint8 MESH_RESOURCE=10//网格？
uint8 TRIANGLE_LIST=11//三角形序列
ROS定时器
在类中
//声明
ros::Timer o_timer;
//开始工作
o_timer=nh.createTimer(ros::Duration(1), &obu_to_xy::o_timer_callback, this);
//实际函数
void obu_to_xy::o_timer_callback(const ros::TimerEvent& event)
{
// pub_markers_obu.publish(markers_all_this_frame);
// std::cout<<"pub one markers all"<<endl;
if(o_add_flag==1)
{
double tmp=ros::Time::now().toSec();
int delta_time=tmp-o_time;
if(delta_time>o_life_time)
{
o_add_flag=0;
}
}
}
launch
写法
py＋cpp
<!-- -->
<launch>
<node name = "I2V" pkg = "obu_parsing" type = "I2V.py" output = "screen">
<param name = "task_me" type="int" value = "9"/>
</node>
<node name = "obu_to_xy" pkg = "obu_to_xy" type = "obu_to_xy" output = "screen">
<param name = "sub_obu_str" type="string" value = "/I2V_info"/>
<param name = "sub_lidar_str" type="string" value = "/obstacle_array"/>
<param name = "pub_obu_str" type="string" value = "/msgs_obu"/>
<param name = "pub_lidar_str" type="string" value = "/obstacle_array_obu"/>
<param name = "o_life_time" type="int" value = "1"/>
<param name = "o_scale_x" type="double" value = "1"/>
<param name = "o_scale_y" type="double" value = "1"/>
<param name = "o_scale_z" type="double" value = "1"/>
<param name = "o_angle" type="double" value = "0"/>
<param name = "distance_emergency" type="int" value = "50"/>
</node>
</launch>
参数传递
cpp
void obu_to_xy::init_para()
{
ros::param::get("~sub_obu_str", sub_obu_str); //~必须添加
ros::param::get("~sub_lidar_str", sub_lidar_str);
ros::param::get("~pub_obu_str", pub_obu_str);
ros::param::get("~pub_lidar_str", pub_lidar_str);
ros::param::get("~o_life_time", o_life_time);
ros::param::get("~o_scale_x", o_scale_x);
ros::param::get("~o_scale_y", o_scale_y);
ros::param::get("~o_scale_z", o_scale_y);
ros::param::get("~o_angle", o_angle);
ros::param::get("~distance_emergency", distance_emergency);
}
python
self.para1=rospy.get_param('~para1')
1168
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
