Ewenwan / Ros
机器人操作系统ROS 语音识别 语义理解 视觉控制 gazebo仿真 雷达建图导航
Stars: ✭ 414
Labels
Projects that are alternatives of or similar to Ros
Libopencm3 Examples
Simple example projects showing how to use libopencm3.
Stars: ✭ 372 (-10.14%)
Mutual labels: makefile
7xx Rfc
At Railscamp X it became clear there is a gap in the current HTTP specification.
Stars: ✭ 4,190 (+912.08%)
Mutual labels: makefile
Go Project Blueprint
Blueprint/Boilerplate For Golang Projects
Stars: ✭ 376 (-9.18%)
Mutual labels: makefile
Letter Boilerplate
Finest letter typesetting from the command line
Stars: ✭ 374 (-9.66%)
Mutual labels: makefile
Hermit
Hermit is a monospace font designed to be clear, pragmatic and very readable.
Stars: ✭ 367 (-11.35%)
Mutual labels: makefile
Luggage
Project to automate OS X package creation without using the packagemaker GUI
Stars: ✭ 368 (-11.11%)
Mutual labels: makefile
Unchained
My personal study of blockchain related technology.
Stars: ✭ 379 (-8.45%)
Mutual labels: makefile
Python Package Template
🚀 Your next Python package needs a bleeding-edge project structure.
Stars: ✭ 356 (-14.01%)
Mutual labels: makefile
Openwrt Shadowsocksr Libev Full
ShadowsocksR-libev-full for OpenWrt
Stars: ✭ 406 (-1.93%)
Mutual labels: makefile
Intel Nuc Dsdt Patch
Patches and Clover configuration required for Intel NUC5/6/7/8 series mini PCs
Stars: ✭ 355 (-14.25%)
Mutual labels: makefile
Istio Handbook
Istio Handbook - Istio Service Mesh Advanced Practical(Istio 服务网格进阶实战) - https://jimmysong.io/istio-handbook
Stars: ✭ 374 (-9.66%)
Mutual labels: makefile
Hostapd Wpe
Modified hostapd to facilitate AP impersonation attacks
Stars: ✭ 409 (-1.21%)
Mutual labels: makefile
Datasciencetoolbox
Complete environments for busy polyglot data scientists
Stars: ✭ 384 (-7.25%)
Mutual labels: makefile
Learn ROS
ROS,工业自动化,opencv,3d点云,机器学习,机械臂, 智能机器人,机器人分拣
ROS 1 和 ROS 2 的前世、今生、安装使用说明与资料汇总
Mastering-ROS-for-Robotics-Programming-Second-Edition 代码
第十四届全国大学生智能汽车竞赛室外光电竞速创意赛,ART-Racecar ros 激光雷达+IMU建图导航
感谢支持
一、消息
1. 发布 字符串 消息
#include "ros/ros.h"
#include "std_msgs/String.h"// 字符串消息 其他 int.h
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "example1a");// 节点初始化
ros::NodeHandle n;
ros::Publisher pub = n.advertise<std_msgs::String>("message", 100);// 发布消息到 message 话题,100个数据空间
ros::Rate loop_rate(10);// 发送频率
while (ros::ok())
{
std_msgs::String msg;
std::stringstream ss;
ss << "Hello World!"; // 生成消息
msg.data = ss.str();
pub.publish(msg);// 发布
ros::spinOnce();// 给ros控制权
loop_rate.sleep();// 时间没到,休息
}
return 0;
}
2. 订阅消息
#include "ros/ros.h"
#include "std_msgs/String.h"
// 订阅消息的回调函数
void messageCallback(const std_msgs::String::ConstPtr& msg)
{
ROS_INFO("Thanks: [%s]", msg->data.c_str());
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "example1b");
ros::NodeHandle n;
// 订阅话题,消息,接收到消息就会 调用 回调函数 messageCallback
ros::Subscriber sub = n.subscribe("message", 100, messageCallback);
ros::spin();
return 0;
}
3. 发布自定义消息 msg
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg.h" // 项目 msg文件下
// msg/chapter2_msg.msg 包含3个整数的消息
// int32 A
// int32 B
// int32 C
#include <sstream>
int main(int argc, char **argv)
{
ros::init(argc, argv, "example2a");
ros::NodeHandle n;
// 发布自定义消息====
ros::Publisher pub = n.advertise<chapter2_tutorials::chapter2_msg>("chapter2_tutorials/message", 100);
ros::Rate loop_rate(10);
while (ros::ok())
{
chapter2_tutorials::chapter2_msg msg;
msg.A = 1;
msg.B = 2;
msg.C = 3;
pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
4. 订阅自定义消息 msg
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_msg.h"
void messageCallback(const chapter2_tutorials::chapter2_msg::ConstPtr& msg)
{
ROS_INFO("I have received: [%d] [%d] [%d]", msg->A, msg->B, msg->C);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "example3_b");
ros::NodeHandle n;
// 订阅自定义消息===
ros::Subscriber sub = n.subscribe("chapter2_tutorials/message", 100, messageCallback);
ros::spin();
return 0;
}
5. 发布自定义服务 srv
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_srv.h" // 项目 srv文件下
// chapter2_srv.srv
// int32 A 请求
// int32 B
// ---
// int32 sum 响应---该服务完成求和服务
// 服务回调函数==== 服务提供方具有 服务回调函数
bool add(chapter2_tutorials::chapter2_srv::Request &req, // 请求
chapter2_tutorials::chapter2_srv::Response &res) // 回应
{
res.sum = req.A + req.B; // 求和服务
ROS_INFO("Request: A=%d, B=%d", (int)req.A, (int)req.B);
ROS_INFO("Response: [%d]", (int)res.sum);
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "adder_server");
ros::NodeHandle n;
// 发布服务(打广告) 广而告之 街头叫卖 等待被撩.jpg
ros::ServiceServer service = n.advertiseService("chapter2_tutorials/adder", add);
ROS_INFO("adder_server has started");
ros::spin();
return 0;
}
6. 订阅服务 获取服务 强撩.jpg
#include "ros/ros.h"
#include "chapter2_tutorials/chapter2_srv.h"
#include <cstdlib>
int main(int argc, char **argv)
{
ros::init(argc, argv, "adder_client");
if (argc != 3)
{
ROS_INFO("Usage: adder_client A B ");
return 1;
}
ros::NodeHandle n;
// 服务客户端,需求端,调用服务
ros::ServiceClient client = n.serviceClient<chapter2_tutorials::chapter2_srv>("chapter2_tutorials/adder");
//创建服务类型
chapter2_tutorials::chapter2_srv srv;
// 设置请求内容
srv.request.A = atoll(argv[1]);
srv.request.B = atoll(argv[2]);
// 调用服务===
if (client.call(srv))
{
// 打印服务带有的响应数据====
ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call service adder_server");
return 1;
}
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(chapter2_tutorials) # 项目名称
## 依赖包===========
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation # 生成自定义消息的头文件
dynamic_reconfigure
)
## 自定义消息文件====
add_message_files(
FILES
chapter2_msg.msg
)
## 自定义服务文件====
add_service_files(
FILES
chapter2_srv.srv
)
## 生成消息头文件
generate_messages(
DEPENDENCIES
std_msgs
)
## 依赖
catkin_package(
CATKIN_DEPENDS message_runtime
)
## 编译依赖库文件
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# 创建可执行文件
add_executable(example1a src/example_1a.cpp)
add_executable(example1b src/example_1b.cpp)
add_executable(example2a src/example_2a.cpp)
add_executable(example2b src/example_2b.cpp)
add_executable(example3a src/example_3a.cpp)
add_executable(example3b src/example_3b.cpp)
## 添加依赖
add_dependencies(example1a chapter2_tutorials_generate_messages_cpp)
add_dependencies(example1b chapter2_tutorials_generate_messages_cpp)
add_dependencies(example2a chapter2_tutorials_generate_messages_cpp)
add_dependencies(example2b chapter2_tutorials_generate_messages_cpp)
add_dependencies(example3a chapter2_tutorials_generate_messages_cpp)
add_dependencies(example3b chapter2_tutorials_generate_messages_cpp)
# 动态链接库
target_link_libraries(example1a ${catkin_LIBRARIES})
target_link_libraries(example1b ${catkin_LIBRARIES})
target_link_libraries(example2a ${catkin_LIBRARIES})
target_link_libraries(example2b ${catkin_LIBRARIES})
target_link_libraries(example3a ${catkin_LIBRARIES})
target_link_libraries(example3b ${catkin_LIBRARIES})
二、行动action类型 参数服务器 坐标变换 tf可视化 安装插件 gazebo仿真
1. 发布行动 action
// 类似于服务,但是是应对 服务任务较长的情况,避免客户端长时间等待,
// 以及服务结果是一个序列,例如一件工作先后很多步骤完成
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h> // action 服务器
#include <actionlib_tutorials/FibonacciAction.h> // 自定义的 action类型 产生斐波那契数列
// action/Fibonacci.action
// #goal definition 任务目标
// int32 order
// ---
// #result definition 最终 结果
// int32[] sequence
// ---
// #feedback 反馈 序列 记录中间 递增 序列
// int32[] sequence
// 定义的一个类========================
class FibonacciAction
{
// 私有=============
protected:
ros::NodeHandle nh_; // 节点实例
// 节点实例必须先被创建 NodeHandle instance
actionlib::SimpleActionServer<actionlib_tutorials::FibonacciAction> as_; // 行动服务器,输入自定义的模板类似
std::string action_name_;// 行动名称
// 行动消息,用来发布的 反馈feedback / 结果result
actionlib_tutorials::FibonacciFeedback feedback_;
actionlib_tutorials::FibonacciResult result_;
// 公开==================
public:
// 类构造函数=============
FibonacciAction(std::string name) :
// 行动服务器 需要绑定 行动回调函数===FibonacciAction::executeCB====
as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1), false),
action_name_(name)
{
as_.start();// 启动
}
// 类析构函数========
~FibonacciAction(void)
{
}
// 行动回调函数=========
void executeCB(const actionlib_tutorials::FibonacciGoalConstPtr &goal)
{
ros::Rate r(1);// 频率
bool success = true;// 标志
/* the seeds for the fibonacci sequence */
feedback_.sequence.clear();// 结果以及反馈
feedback_.sequence.push_back(0); // 斐波那契数列
feedback_.sequence.push_back(1);
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
/* start executing the action */
for(int i=1; i<=goal->order; i++)// order 为序列数量
{
/* check that preempt has not been requested by the client */
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
/* set the action state to preempted */
as_.setPreempted();
success = false;
break;
}
// 产生后一个数
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
/* publish the feedback */
as_.publishFeedback(feedback_);// 发布
/* this sleep is not necessary, however, the sequence is computed at 1 Hz for demonstration purposes */
r.sleep();
}
if(success)
{
// 最终结果
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
/* set the action state to succeeded */
as_.setSucceeded(result_);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "fibonacci server");
FibonacciAction fibonacci("fibonacci");
ros::spin();
return 0;
}
2. 行动客户端 类似 服务消费者
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>// action 客户端
#include <actionlib/client/terminal_state.h> // action 状态
#include <actionlib_tutorials/FibonacciAction.h> // 自定义行动类型
int main (int argc, char **argv)
{
ros::init(argc, argv, "fibonacci client");
/* create the action client
"true" causes the client to spin its own thread */
// action 客户端 =====
actionlib::SimpleActionClient<actionlib_tutorials::FibonacciAction> ac("fibonacci", true);
ROS_INFO("Waiting for action server to start.");
/* will be waiting for infinite time */
ac.waitForServer(); // 等待 行动服务器启动
ROS_INFO("Action server started, sending goal.");
// 发布任务目标 产生20个数量的 斐波那契数列序列
actionlib_tutorials::FibonacciGoal goal;
goal.order = 20;
ac.sendGoal(goal);// 发给 行动服务器=====
// 等待 行动 执行结果
bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
if (finished_before_timeout)
{
actionlib::SimpleClientGoalState state = ac.getState();// 状态
ROS_INFO("Action finished: %s",state.toString().c_str());
}
else
ROS_INFO("Action doesnot finish before the time out.");
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(actionlib_tutorials)
# add_compile_options(-std=c++11)
# 找到包依赖
find_package(catkin REQUIRED COMPONENTS
actionlib
actionlib_msgs
message_generation
roscpp
rospy
std_msgs
)
## 行动自定义文件
add_action_files(
DIRECTORY action
FILES Fibonacci.action
)
## 生成行动类型 头文件
generate_messages(
DEPENDENCIES actionlib_msgs std_msgs
)
## 包依赖
catkin_package(
INCLUDE_DIRS include
LIBRARIES actionlib_tutorials
CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs
DEPENDS system_lib
)
## 包含
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
## 编译 连接
add_executable(fibonacci_server src/fibonacci_server.cpp)
add_executable(fibonacci_client src/fibonacci_client.cpp)
target_link_libraries(fibonacci_server ${catkin_LIBRARIES})
target_link_libraries(fibonacci_client ${catkin_LIBRARIES})
add_dependencies(fibonacci_server ${actionlib_tutorials_EXPORTED_TARGETS})
add_dependencies(fibonacci_client ${actionlib_tutorials_EXPORTED_TARGETS})
3. 参数服务器 parameter_server
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>// 动态参数 调整
#include <parameter_server_tutorials/parameter_server_Config.h> // 自定义的 配置参数列表
// cfg/parameter_server_tutorials.cfg===========
/*
# coding:utf-8
#!/usr/bin/env python
PACKAGE = "parameter_server_tutorials" # 包名
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()# 参数生成器
# 参数列表 ====================
gen.add("BOOL_PARAM", bool_t, 0, "A Boolean parameter", True) # BOOL量类型参数
gen.add("INT_PARAM", int_t, 0, "An Integer Parameter", 1, 0, 100) # 整形量参数
gen.add("DOUBLE_PARAM", double_t, 0, "A Double Parameter", 0.01, 0, 1)# 浮点型变量参数
gen.add("STR_PARAM", str_t, 0, "A String parameter", "Dynamic Reconfigure") # 字符串类型变量参数
# 自定义 枚举常量 类型 ==========
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, "Hight :2")],
"Selection List")
# 添加自定义 变量类型
gen.add("SIZE", int_t, 0, "Selection List", 1, 0, 3, edit_method=size_enum)
# 生成 动态参数配置 头文件 以 parameter_server_ 为前缀
exit(gen.generate(PACKAGE, "parameter_server_tutorials", "parameter_server_"))
*/
// 参数改变后 的回调函数,parameter_server_Config 为参数头
void callback(parameter_server_tutorials::parameter_server_Config &config, uint32_t level)
{
ROS_INFO("Reconfigure Request: %s %d %f %s %d",
config.BOOL_PARAM?"True":"False",
config.INT_PARAM,
config.DOUBLE_PARAM,
config.STR_PARAM.c_str(),
config.SIZE);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "parameter_server_tutorials");
dynamic_reconfigure::Server<parameter_server_tutorials::parameter_server_Config> server;// 参数服务器
dynamic_reconfigure::Server<parameter_server_tutorials::parameter_server_Config>::CallbackType f;// 参数改变 回调类型
// 绑定回调函数
f = boost::bind(&callback, _1, _2);
// 参数服务器设置 回调器
server.setCallback(f);
ROS_INFO("Spinning");
ros::spin();// 启动
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(parameter_server_tutorials)
# add_compile_options(-std=c++11)
# 找到包
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
message_generation
dynamic_reconfigure
)
# 动态参数配置文件
generate_dynamic_reconfigure_options(
cfg/parameter_server_tutorials.cfg
)
# 依赖
catkin_package(
CATKIN_DEPENDS message_runtime
)
# 包含
include_directories(
include
${catkin_INCLUDE_DIRS}
)
# 生成可执行文件
add_executable(parameter_server_tutorials src/parameter_server_tutorials.cpp)
add_dependencies(parameter_server_tutorials parameter_server_tutorials_gencfg)
target_link_libraries(parameter_server_tutorials ${catkin_LIBRARIES})
4. 坐标变换发布 tf_broadcaster
#include <ros/ros.h>
#include <tf/transform_broadcaster.h> // 坐标变换发布/广播
#include <turtlesim/Pose.h>// 小乌龟位置类型
std::string turtle_name;
// 小乌龟 位姿 话题 回调函数 =======
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);// 按照 rpy 姿态向量形式设置 平面上只有 绕Z轴的旋转 偏航角
transform.setRotation(q);// 姿态
// 广播位姿变换消息=====
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "tf_broadcaster");
if (argc != 2){ROS_ERROR("need turtle name as argument"); return -1;};
turtle_name = argv[1];
ros::NodeHandle node;
// 订阅小乌龟 位姿 话题数据 绑定回调函数 poseCallback
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
ros::spin();
return 0;
}
5. 坐标变换监听 tf_listener
#include <ros/ros.h>
#include <tf/transform_listener.h>// 坐标变换监听
#include <geometry_msgs/Twist.h> // 消息类型
#include <turtlesim/Spawn.h>// 生成一个小乌龟
int main(int argc, char** argv)
{
ros::init(argc, argv, "tf_listener");
ros::NodeHandle node;
ros::service::waitForService("spawn");// 等待 生成小乌龟的服务到来
ros::ServiceClient add_turtle =
node.serviceClient<turtlesim::Spawn>("spawn"); // 服务客户端
turtlesim::Spawn srv;
add_turtle.call(srv); // 调用服务
// 发布小乌龟运动指令=====
ros::Publisher turtle_vel =
node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel", 10);
// 左边变换监听
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
tf::StampedTransform transform; // 得到的坐标变换消息
try
{
// 两个小乌龟坐标变换消息 之差 左边变换??
// 有两个 坐标变换发布器 一个发布 /turtle1 一个发布 /turtle2
listener.lookupTransform("/turtle2", "/turtle1",
ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据位姿差,发布 命令 让 小乌龟2 追赶上 小乌龟1
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.publish(vel_msg);
rate.sleep();
}
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(tf_tutorials)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
tf
turtlesim
)
catkin_package()
include_directories(
# include
${catkin_INCLUDE_DIRS}
)
add_executable(turtle_tf_broadcaster src/turtle_tf_broadcaster.cpp)
target_link_libraries(turtle_tf_broadcaster ${catkin_LIBRARIES})
add_executable(turtle_tf_listener src/turtle_tf_listener.cpp)
target_link_libraries(turtle_tf_listener ${catkin_LIBRARIES})
start_demo.launch
<launch>
<!-- Turtlesim Node 小乌龟1-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!-- 小乌龟1 键盘控制 -->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<!-- Axes -->
<param name="scale_linear" value="2" type="double"/>
<param name="scale_angular" value="2" type="double"/>
<!-- 发布 小乌龟1 位姿 ->
<node pkg="tf_tutorials" type="turtle_tf_broadcaster"
args="/turtle1" name="turtle1_tf_broadcaster" />
<!-- 发布 小乌龟2 位姿 ->
<node pkg="tf_tutorials" type="turtle_tf_broadcaster"
args="/turtle2" name="turtle2_tf_broadcaster" />
<!-- 监听两者位姿变换 让小乌龟2 追上 小乌龟1 ->
<node pkg="tf_tutorials" type="turtle_tf_listener"
name="listener" />
</launch>
6. 可视化 插件
三、日志 + 话题/服务/参数/action/发布图像/发布点云/发布marker
1. 定义 ROS_DEBUG
#include <ros/ros.h>
#include <ros/console.h> // 控制台
#define OVERRIDE_NODE_VERBOSITY_LEVEL 0
int main( int argc, char **argv )
{
ros::init( argc, argv, "program1" );
#if OVERRIDE_NODE_VERBOSITY_LEVEL
/* Setting the logging level manually to DEBUG */
// 日志等级 Debug
ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Debug);
#endif
ros::NodeHandle nh;
const double val = 3.14;
// ros 打印日志
ROS_DEBUG( "We are looking DEBUG message" );
ROS_DEBUG( "We are looking DEBUG message with an argument: %f", val );
ROS_DEBUG_STREAM("We are looking DEBUG stream message with an argument: " << val);
ros::spinOnce();
return EXIT_SUCCESS;
}
2. 各种消息接口 名字消息 条件消息 过滤消息 单次消息 频率消息
#include <ros/ros.h>
#include <ros/console.h>
int main( int argc, char **argv )
{
ros::init( argc, argv, "program2" );
ros::NodeHandle n;
const double val = 3.14;
/* Basic messages: 基本消息 */
ROS_INFO( "ROS INFO message." ); //
ROS_INFO( "ROS INFO message with argument: %f", val ); // 相当于c中的printf;
ROS_INFO_STREAM( "ROS INFO stream message with argument: " << val); // 相当于c++中的cout;
/* Named messages: 为调试信息命名 */
// 表示为这段信息命名,为了更容易知道这段信息来自那段代码.
ROS_INFO_STREAM_NAMED("named_msg","ROS named INFO stream message; val = " << val);
/* Conditional messages: 条件消息*/
ROS_INFO_STREAM_COND(val < 0., "ROS conditional INFO stream message; val (" << val << ") < 0");
ROS_INFO_STREAM_COND(val >= 0.,"ROS conditional INFO stream message; val (" << val << ") >= 0");
/* Conditional Named messages: 条件 名字消息*/
ROS_INFO_STREAM_COND_NAMED(val < 0., "cond_named_msg","ROS conditional INFO stream message; val (" << val << ") < 0");
ROS_INFO_STREAM_COND_NAMED(val >= 0., "cond_named_msg","ROS conditional INFO stream message; val (" << val << ") >= 0");
/* Filtered messages: 滤波消息*/
struct ROSLowerFilter : public ros::console::FilterBase
{
ROSLowerFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value < 0.;// 小于0
}
double value;
};
struct ROSGreaterEqualFilter : public ros::console::FilterBase
{
ROSGreaterEqualFilter( const double& val ) : value( val ) {}
inline virtual bool isEnabled()
{
return value >= 0.; // 大于0
}
double value;
};
ROSLowerFilter filter_lower(val);// 小于0的消息
ROSGreaterEqualFilter filter_greater_equal(val);// 大于0的消息
// 宏定义接口传入 过滤消息实例================
ROS_INFO_STREAM_FILTER(
&filter_lower,
"ROS filter INFO stream message; val (" << val << ") < 0"
);
ROS_INFO_STREAM_FILTER(
&filter_greater_equal,
"ROS filter INFO stream message; val (" << val << ") >= 0"
);
/* Once messages: 单次显示*/
for( int i = 0; i < 10; ++i ) {
// 在循环中让信息只输出一次
ROS_INFO_STREAM_ONCE(
"ROS once INFO stream message; i = " << i
);
}
/* Throttle messages: 设置显示频率 */
for( int i = 0; i < 10; ++i ) {
// THROTTLE表示节流的意思, 代码运行两次输出一次INFO throttle message.
ROS_INFO_STREAM_THROTTLE(
2,
"ROS throttle INFO stream message; i = " << i
);
ros::Duration(1).sleep();
}
ros::spinOnce();
return EXIT_SUCCESS;
}
3. debug info warn error fatal
#include <ros/ros.h>
#include <ros/console.h>
int main( int argc, char **argv )
{
ros::init( argc, argv, "program3" );
ros::NodeHandle nh;
ros::Rate rate(1);
while(ros::ok())
{
ROS_DEBUG_STREAM( "ROS DEBUG message."); // debug 等级消息
ROS_INFO_STREAM ( "ROS INFO message."); // info 普通正常消息
ROS_WARN_STREAM ( "ROS WARN message." ); // warn 警告消息
ROS_ERROR_STREAM( "ROS ERROR message." ); // error 错误消息
ROS_FATAL_STREAM( "ROS FATAL message." ); // fatal 验证错误消息
ROS_INFO_STREAM_NAMED( "named_msg", "ROS INFO named message." );// 名字消息
ROS_INFO_STREAM_THROTTLE(2, "ROS INFO Throttle message." ); // 频率消息
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
4. 自定义服务消息 客户端 + 日志打印 北京瘫.jpg
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Vector3.h>
#include <chapter4_tutorials/SetSpeed.h> // 自定义服务消息类型
// srv/SetSpeed.srv-----------
// float32 desired_speed // 请求,期望速度
// ---
// float32 previous_speed // 反馈,上一次的速度
// float32 current_speed // 当前速度
// bool stalled // 设置完成标志
int main( int argc, char **argv )
{
ros::init( argc, argv, "program4" );
ros::NodeHandle nh;
// 发布温度数据
ros::Publisher pub_temp = nh.advertise< std_msgs::Int32 >( "temperature", 1000 );// 普通整形数据话题,温度数据
// 发布加速度消息 1*3 向量
ros::Publisher pub_accel = nh.advertise< geometry_msgs::Vector3 >( "acceleration", 1000 );
// 服务客户端,请求服务,获取服务,消费者
ros::ServiceClient srv_speed = nh.serviceClient< chapter4_tutorials::SetSpeed>( "speed" );
std_msgs::Int32 msg_temp;// 温度数据
geometry_msgs::Vector3 msg_accel;// 三轴加速度消息
chapter4_tutorials::SetSpeed msg_speed;// 服务消息
int i = 0;
ros::Rate rate( 1 );// 频率为1
while( ros::ok() )
{
msg_temp.data = i;// 温度数据======
msg_accel.x = 0.1 * i;// 三轴加速度消息=====
msg_accel.y = 0.2 * i;
msg_accel.z = 0.3 * i;
// 服务数据,设置 期望值,消费者提出的服务标准====
msg_speed.request.desired_speed = 0.01 * i;// 期望速度===
pub_temp.publish( msg_temp );// 发布温度数据
pub_accel.publish( msg_accel );// 发布加速度消息
// 服务消费者,调用服务,享受服务===
if( srv_speed.call( msg_speed ) )// 服务数据中携带,服务反馈值
{
// 日志消息打印,服务数据反馈值========================
ROS_INFO_STREAM(
"SetSpeed response:\n" <<
"Previous speed = " << msg_speed.response.previous_speed << "\n" <<
"Current speed = " << msg_speed.response.current_speed << "\n" <<
"Motor stalled = " << (msg_speed.response.stalled ? "true" : "false" )
);
}
else
{
/* Note that this might happen at the beginning, because
the service server could have not started yet! */
// 暂时无服务,获取服务提供错误
ROS_ERROR_STREAM( "Call to speed service failed!" );
}
++i;
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
5. 自定义服务消息 服务端 + 日志打印 上街叫卖.jpg
#include <ros/ros.h>
#include <ros/console.h>
#include <std_msgs/Int32.h>
#include <geometry_msgs/Vector3.h>
#include <chapter4_tutorials/SetSpeed.h>
// 全局变量,记录前后两次的速度=====
float previous_speed = 0.;
float current_speed = 0.;
// 订阅 温度数据话题,回调函数
void callback_temperature( const std_msgs::Int32::ConstPtr& msg )
{
// 日志打印收到的消息
ROS_INFO_STREAM( "Temperature = " << msg->data );
}
// 订阅加速度数据话题,回调函数
void callback_acceleration( const geometry_msgs::Vector3::ConstPtr& msg )
{
// 日志打印收到的消息
ROS_INFO_STREAM("Acceleration = (" << msg->x << ", " << msg->y << ", " << msg->z << ")");
}
// 话题数据======是生产者主导==============被动消费=====容易爆仓========生产导向=================
// 服务话题回调函数=====消费者主导==========主动消费=====主动权在手=====顾客是上帝=====需求导向====
bool callback_speed(chapter4_tutorials::SetSpeed::Request &req, // 服务请求,消费者主动发来的
chapter4_tutorials::SetSpeed::Response &res) // 服务反馈,提供者,完成服务后的反馈信息
{
// 打印 服务客户端发来的 服务请求,服务要求,期望速度
ROS_INFO_STREAM("Speed service request: desired speed = " << req.desired_speed);
current_speed = 0.9 * req.desired_speed;// 当前速度,仿真
res.previous_speed = previous_speed;
res.current_speed = current_speed;
res.stalled = current_speed < 0.1;
previous_speed = current_speed;// 迭代======
return true;
}
int main( int argc, char **argv )
{
ros::init( argc, argv, "program5" );
ros::NodeHandle nh;
// 订阅话题,直接购买商品,有多少我要多少=====土豪脸.jpg
// 温度数据 话题
ros::Subscriber sub_temp = nh.subscribe( "temperature", 1000, callback_temperature);
// 加速度数据话题
ros::Subscriber sub_accel = nh.subscribe( "acceleration", 1000, callback_acceleration);
// 发布服务,广播消息,打广告,请把需求砸过来!!!!!!! 可爱脸.jpg
ros::ServiceServer srv_speed = nh.advertiseService( "speed", callback_speed );
ros::spin();
return EXIT_SUCCESS;
}
6. 动态参数配置 + 日志
#include <ros/ros.h>
#include <dynamic_reconfigure/server.h>
#include <chapter4_tutorials/DynamicParamConfig.h>// 自定义 参数
// cfg/DynamicParam.cfg-----------------------
/*
# coding: utf-8
#!/usr/bin/env python
PACKAGE='chapter4_tutorials' # 包名
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import *
from dynamic_reconfigure.msg import SensorLevels
gen = ParameterGenerator() # 参数生成
gen.add('BOOL', bool_t, SensorLevels.RECONFIGURE_RUNNING,
'Bool param', True)
gen.add('INT', int_t, SensorLevels.RECONFIGURE_STOP,
'Int param', 0, -10, 10)
gen.add('DOUBLE', double_t, SensorLevels.RECONFIGURE_CLOSE,
'Double param', 0.0, -pi, pi)
# 常量
foo = gen.const('ros', str_t, 'Ros', 'ROS')
bar = gen.const('cook', str_t, 'Cook', 'COOK')
baz = gen.const('book', str_t, 'Book', 'BOOK')
# 枚举变量
strings = gen.enum([foo, bar, baz], 'Strings')
# 添加自定义的枚举变量
gen.add('STRING', str_t, SensorLevels.RECONFIGURE_RUNNING,
'String param', 'Ros', edit_method = strings)
# 生成消息 头文件
exit(gen.generate(PACKAGE, PACKAGE, 'DynamicParam'))
*/
// ------------------------------------
// 动态参数服务器
class DynamicParamServer
{
public:
DynamicParamServer()
{
// 动态参数配置服务器设置,参数改变后响应的 回调函数
_cfg_server.setCallback(boost::bind(&DynamicParamServer::callback, this, _1, _2));
}
void callback(chapter4_tutorials::DynamicParamConfig& config, uint32_t level)
{
// 打印动态配置后的参数
ROS_INFO_STREAM(
"New configuration received with level = " << level << ":\n" <<
"BOOL = " << config.BOOL << "\n" <<
"INT = " << config.INT<< "\n" <<
"DOUBLE = " << config.DOUBLE << "\n" <<
"STRING = " << config.STRING
);
}
private:
// 接收 参数类型后实例化的 动态参数配置服务器对象
dynamic_reconfigure::Server<chapter4_tutorials::DynamicParamConfig> _cfg_server;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "program6");
DynamicParamServer dps;// 定义参数服务器类,修改参数后,回调函数会指定执行
while(ros::ok())
{
ros::spin();
}
return EXIT_SUCCESS;
}
7. diagnostic_updater 诊断
diagnostic_updater/diagnostic_updater.h 诊断???
8. 发布图像消息 + 日志
#include <ros/ros.h>
#include <image_transport/image_transport.h> // 图像发送
#include <cv_bridge/cv_bridge.h>// opencv 图像 转换成 ros图像
#include <sensor_msgs/image_encodings.h> // 图像编码
#include <opencv2/highgui/highgui.hpp>// opencvgui
int main( int argc, char **argv )
{
ros::init( argc, argv, "program8" );
ros::NodeHandle nh;
/*Open camera with CAMERA_INDEX (webcam is typically #0).*/
const int CAMERA_INDEX = 0; // 摄像头id
cv::VideoCapture capture( CAMERA_INDEX );// opencv打开相机
if(not capture.isOpened() )
{// 打开相机发生错误
ROS_ERROR_STREAM("Failed to open camera with index " << CAMERA_INDEX << "!");
ros::shutdown();
}
// 图像信息发送器
image_transport::ImageTransport it(nh);
// 发布图像消息
image_transport::Publisher pub_image = it.advertise( "camera", 1 );
// opencv 图像 带 时间戳
cv_bridge::CvImagePtr frame = boost::make_shared< cv_bridge::CvImage >();
frame->encoding = sensor_msgs::image_encodings::BGR8;
while( ros::ok() ) {
capture >> frame->image;// 图像域
if( frame->image.empty() )
{
ROS_ERROR_STREAM( "Failed to capture frame!" );
ros::shutdown();
}
frame->header.stamp = ros::Time::now();// 时间戳
pub_image.publish( frame->toImageMsg() );// 转换成 ros图像消息后发布出去====
cv::waitKey( 3 );
ros::spinOnce();
}
capture.release();// 释放相机=======
return EXIT_SUCCESS;
}
9. 发布点云消息 + 日志
#include <ros/ros.h>
#include <visualization_msgs/Marker.h> // rviz可视化图像/marker
#include <sensor_msgs/PointCloud2.h> // 点云消息
#include <pcl_conversions/pcl_conversions.h> // pcl类型转换成 rospcl类型
#include <pcl/point_cloud.h>// 点云
#include <pcl/point_types.h>// 点类型
int main( int argc, char **argv )
{
ros::init( argc, argv, "program9" );
ros::NodeHandle n;
// 发布marker消息
ros::Publisher pub_marker = n.advertise< visualization_msgs::Marker >( "marker", 1000 );
// 发布点云消息
ros::Publisher pub_pc = n.advertise< sensor_msgs::PointCloud2 >( "pc", 1000 );
// 可视化marker消息----------------------------------------------------
visualization_msgs::Marker msg_marker;
msg_marker.header.frame_id = "/frame_world"; // 消息头,坐标系id
msg_marker.ns = "shapes"; // 所属命名空间
msg_marker.id = 0; // id
msg_marker.type = visualization_msgs::Marker::CUBE; // 形状类型,正方体
msg_marker.action = visualization_msgs::Marker::ADD; // 叠加
msg_marker.pose.position.x = 0.;// 位置
msg_marker.pose.position.y = 1.;
msg_marker.pose.position.z = 2.;
msg_marker.pose.orientation.x = 0.;// 姿态 四元素类型
msg_marker.pose.orientation.y = 0.;
msg_marker.pose.orientation.z = 0.;
msg_marker.pose.orientation.w = 1.;
msg_marker.scale.x = 1.;// 尺寸
msg_marker.scale.y = 1.;
msg_marker.scale.z = 1.;
msg_marker.color.r = 1.; // 颜色
msg_marker.color.g = 0.;
msg_marker.color.b = 0.;
msg_marker.color.a = 1.; // 透明度,不透明
msg_marker.lifetime = ros::Duration();// 声生命周期
ROS_INFO_STREAM( "Marker Created." );
// 点云消息--------------------------------------------
sensor_msgs::PointCloud2 msg_pc;// rospcl 类型
pcl::PointCloud< pcl::PointXYZ > pc;// pcl XYZ类型点云
pc.width = 300;
pc.height = 200; // 有序点云
pc.is_dense = false;// 有nan点
pc.points.resize( pc.width * pc.height );
// 随机生成假的点云数据
for( size_t i = 0; i < pc.height; ++i ) {
for( size_t j = 0; j < pc.width; ++j ) {
const size_t k = pc.width * i + j;
pc.points[k].x = 0.1 * i;
pc.points[k].y = 0.2 * j;
pc.points[k].z = 1.5;
}
}
ROS_INFO_STREAM( "Point Cloud Created." );
ros::Rate rate( 1 );
while( ros::ok() )
{
msg_marker.header.stamp = ros::Time::now(); // marker时间戳
msg_marker.pose.position.x += 0.01; // 位置在移动
msg_marker.pose.position.y += 0.02;
msg_marker.pose.position.z += 0.03;
for( size_t i = 0; i < pc.height; ++i ) {
for( size_t j = 0; j < pc.width; ++j ) {
const size_t k = pc.width * i + j;
pc.points[k].z -= 0.1; // z方向位置在移动
}
}
pcl::toROSMsg( pc, msg_pc );// pcl点云类型 转换成 rospcl类型
msg_pc.header.stamp = msg_marker.header.stamp;// 时间戳
msg_pc.header.frame_id = "/frame_robot";// 坐标系
pub_marker.publish( msg_marker );// 发布marker
pub_pc.publish( msg_pc ); // 发布 点云
ros::spinOnce();
rate.sleep();
}
return EXIT_SUCCESS;
}
10. 交互式marker +日志
#include <ros/ros.h>
#include <tf/tf.h>
#include <interactive_markers/interactive_marker_server.h> // 交互式marker 可以响应鼠标
// 有交互后的回调函数----------------------------------------------------------------------------
void feedback_callback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback)
{
double roll, pitch, yaw;
tf::Quaternion q;
tf::quaternionMsgToTF(feedback->pose.orientation, q);// 获取四元素姿态
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);// 对应的姿态向量
// 打印marker的位置 和 姿态
ROS_INFO_STREAM(
feedback->marker_name << "position (x, y, z) = (" <<
feedback->pose.position.x << ", " <<
feedback->pose.position.y << ", " <<
feedback->pose.position.z << "), orientation (roll, pitch, yaw) = (" <<
roll << ", " << pitch << ", " << yaw << ")"
);
}
int main( int argc, char** argv )
{
ros::init(argc, argv, "program10");
// 交互式marker服务器
interactive_markers::InteractiveMarkerServer server("marker");
visualization_msgs::InteractiveMarker marker;// 交互式marker 类型
marker.header.frame_id = "base_link";// 头,坐标系
marker.name = "marker";// 名字
marker.description = "2-DOF Control";// 自我介绍
/* Box marker */
visualization_msgs::Marker box_marker;
box_marker.type = visualization_msgs::Marker::CUBE; // 正方体
box_marker.scale.x = 0.5;// 尺寸
box_marker.scale.y = 0.5;
box_marker.scale.z = 0.5;
box_marker.color.r = 0.5;// 颜色
box_marker.color.g = 0.5;
box_marker.color.b = 0.5;
box_marker.color.a = 1.0;
/* Non-interactive control which contains the box */
visualization_msgs::InteractiveMarkerControl box_control;// 交互式marker控制
box_control.always_visible = true;// 一直显示
box_control.markers.push_back(box_marker);// 设置控制对象
/* Controls to move the box */
visualization_msgs::InteractiveMarkerControl move_x_control, rotate_z_control;
move_x_control.name = "move_x";
move_x_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;// 沿轴方向移动
rotate_z_control.name = "rotate_z";
rotate_z_control.orientation.w = 1;
rotate_z_control.orientation.y = 1;
rotate_z_control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;// 沿轴方向旋转
// 交互式marker设置可 交互方式
marker.controls.push_back(box_control);
marker.controls.push_back(move_x_control);
marker.controls.push_back(rotate_z_control);
// 交互式marker服务器吗,设置携带交互方式的 交互式marker
server.insert(marker, &feedback_callback);
server.applyChanges();
ros::spin();
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8.3)
project(chapter4_tutorials)
set(ROS_BUILD_TYPE Debug) # 编译模式
# 找到依赖包
find_package(catkin REQUIRED
COMPONENTS
roscpp
message_generation
std_msgs
geometry_msgs
sensor_msgs
visualization_msgs
dynamic_reconfigure
diagnostic_updater
cv_bridge
image_transport
pcl_conversions
interactive_markers)
# 找依赖库
find_package(OpenCV)
find_package(PCL REQUIRED)
# 自定义服务类型
add_service_files(FILES SetSpeed.srv)
# 生成服务类型 的 头文件
generate_messages(DEPENDENCIES std_msgs)
# 生成动态参数配置参数 的头文件
generate_dynamic_reconfigure_options(cfg/DynamicParam.cfg)
# 设置包
catkin_package(
CATKIN_DEPENDS
roscpp
message_runtime
std_msgs
geometry_msgs
sensor_msgs
visualization_msgs
dynamic_reconfigure
diagnostic_updater
cv_bridge
image_transport
pcl_conversions
interactive_markers)
# 添加依赖库
include_directories(
${catkin_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS})
# 编译
add_executable(program1 src/program1.cpp)
target_link_libraries(program1 ${catkin_LIBRARIES})
add_executable(program1_dump src/program1_dump.cpp)
target_link_libraries(program1_dump ${catkin_LIBRARIES})
add_executable(program1_mem src/program1_mem.cpp)
target_link_libraries(program1_mem ${catkin_LIBRARIES})
add_executable(program2 src/program2.cpp)
target_link_libraries(program2 ${catkin_LIBRARIES})
add_executable(program3 src/program3.cpp)
target_link_libraries(program3 ${catkin_LIBRARIES})
add_executable(program4 src/program4.cpp)
add_dependencies(program4 ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(program4 ${catkin_LIBRARIES})
add_executable(program5 src/program5.cpp)
add_dependencies(program5 ${PROJECT_NAME}_generate_messages_cpp)
target_link_libraries(program5 ${catkin_LIBRARIES})
add_executable(program6 src/program6.cpp)
add_dependencies(program6 ${PROJECT_NAME}_gencfg)
target_link_libraries(program6 ${catkin_LIBRARIES})
add_executable(program7 src/program7.cpp)
target_link_libraries(program7 ${catkin_LIBRARIES})
add_executable(program8 src/program8.cpp)
target_link_libraries(program8 ${catkin_LIBRARIES} ${OpenCV_LIBRARIES})
add_executable(program9 src/program9.cpp)
target_link_libraries(program9 ${catkin_LIBRARIES} ${PCL_LIBRARIES})
add_executable(program10 src/program10.cpp)
target_link_libraries(program10 ${catkin_LIBRARIES})
四、发布 雷达数据 坐标变换 里程计数据
1. 发布雷达数据
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h> // 雷达扫描数据
int main(int argc, char** argv)
{
ros::init(argc, argv, "laser_scan_publisher");
ros::NodeHandle n;
// 话题 发布 雷达扫描数据
ros::Publisher scan_pub = n.advertise<sensor_msgs::LaserScan>("scan", 50);
unsigned int num_readings = 100; // 一周数据点??
double laser_frequency = 40; // 频率
double ranges[num_readings]; // 范围
double intensities[num_readings]; // 密度
int count = 0;
ros::Rate r(1.0);
while(n.ok()){
// 生成假的雷达数据=============
for(unsigned int i = 0; i < num_readings; ++i)
{
ranges[i] = count; // 距离数据
intensities[i] = 100 + count; // 密度数据,反射强度??
}
// 准备雷达数据=========================
ros::Time scan_time = ros::Time::now();
sensor_msgs::LaserScan scan;// 定义雷达数据
scan.header.stamp = scan_time;// 时间戳
scan.header.frame_id = "base_link";// 坐标系
scan.angle_min = -1.57; // 扫描最小角度 -90度
scan.angle_max = 1.57; // 扫描最大角度 +90度
scan.angle_increment = 3.14 / num_readings; // 180度 100个数据,角度分辨率
scan.time_increment = (1 / laser_frequency) / (num_readings);// 每一个扫描需要的时间,时间增量
scan.range_min = 0.0; // 数据范围
scan.range_max = 100.0;
scan.ranges.resize(num_readings); // 距离范围数据
scan.intensities.resize(num_readings);// 强度数据??
for(unsigned int i = 0; i < num_readings; ++i)
{
// 填充距离数据 和 强度数据
scan.ranges[i] = ranges[i];
scan.intensities[i] = intensities[i];
}
// 发布雷达数据
scan_pub.publish(scan);
++count;
r.sleep();
}
}
2. 发布里程计数据
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h> // 关节状态??
#include <tf/transform_broadcaster.h> // 左边变换 广播
#include <nav_msgs/Odometry.h> // 导航下的里程计消息
int main(int argc, char** argv)
{
ros::init(argc, argv, "state_publisher");
ros::NodeHandle n;
// 发布里程计消息
ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 10);
// 初始2d位姿
double x = 0.0;
double y = 0.0;
double th = 0;
// 速度 velocity
double vx = 0.4; // 前进线速度
double vy = 0.0;
double vth = 0.4;// 旋转角速度
ros::Time current_time;
ros::Time last_time;
current_time = ros::Time::now();// 当前时间
last_time = ros::Time::now(); // 上次时间
tf::TransformBroadcaster broadcaster; // 位姿 广播
ros::Rate loop_rate(20);// 频率
const double degree = M_PI/180; // 度转 弧度
// message declarations
geometry_msgs::TransformStamped odom_trans; // 坐标变换消息
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
while (ros::ok()) {
current_time = ros::Time::now(); // 当前时间
double dt = (current_time - last_time).toSec();// 两次时间差
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
// \vy y /vx
// \ | /
// \ |/
// -------x-------
//
x += delta_x;
y += delta_y;
th += delta_th;
geometry_msgs::Quaternion odom_quat;// 四元素位姿
odom_quat = tf::createQuaternionMsgFromRollPitchYaw(0,0,th);// rpy转换到 四元素
// 更新左边变换消息,tf广播发布==================
odom_trans.header.stamp = current_time; // 当前时间
odom_trans.transform.translation.x = x; // 位置
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);// 位姿
// 更新 里程计消息
nav_msgs::Odometry odom;// 里程计消息
odom.header.stamp = current_time;// 当前时间
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
// 位置 position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat; // 位姿
// 速度 velocity
odom.twist.twist.linear.x = vx;// 线速度
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = 0.0; // 小速度
odom.twist.twist.angular.y = 0.0;
odom.twist.twist.angular.z = vth;
last_time = current_time;// 迭代消息
// publishing the odometry and the new tf
broadcaster.sendTransform(odom_trans);// 发布坐标变换消息 =====
odom_pub.publish(odom);// 发布里程计消息====
loop_rate.sleep();
}
return 0;
}
3. 发布 目标位置 action
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h> // 移动底盘 action消息
#include <actionlib/client/simple_action_client.h>// action 客户端,发布目标
#include <tf/transform_broadcaster.h>// 坐标变换广播
#include <sstream>
// action 客户端==========
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
int main(int argc, char** argv)
{
ros::init(argc, argv, "navigation_goals");
// action 客户端
MoveBaseClient ac("move_base", true);
// 等待action服务 启动
while(!ac.waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the move_base action server");
}
// action 目标信息 目标位置
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.header.frame_id = "map";// 坐标系
goal.target_pose.header.stamp = ros::Time::now();// 时间戳
goal.target_pose.pose.position.x = 1.0;// 目标位置
goal.target_pose.pose.position.y = 1.0;
goal.target_pose.pose.orientation.w = 1.0;// 姿态
ROS_INFO("Sending goal");
ac.sendGoal(goal);// 发送 action 目标
ac.waitForResult(); // 等待 action服务端 完成action
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("You have arrived to the goal position");
else{
ROS_INFO("The base failed for some reason");
}
return 0;
}
五、综合 应用
1. 三维重建 opencv pcl g2o
2. RGBD数据处理
3. 机器人控制 pid 笛卡尔臂控制 差分底盘控制
4. 智能抓取
5. universal_robot UR机械臂 UR3 UR5 UR10 MOVI配置 gazebo 运动学
6. 发布自定义消息 msg
multi sensor fusion EKF多传感器融合框架
7. 地理信息系统
8. 无人机仿真
Note that the project description data, including the texts, logos, images, and/or trademarks,
for each open source project belongs to its rightful owner.
If you wish to add or remove any projects, please contact us at [email protected].