Learn ROS
一、消息
1. 釋出 字元串 消息
#include "ros/ros.h"
#include "std_msgs/String.h"// 字元串消息 其他 int.h
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "example1a");// 節點初始化
ros::NodeHandle n;
ros::Publisher pub = n.advertise<: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
int main(int argc, char **argv)
{
ros::init(argc, argv, "example2a");
ros::NodeHandle n;
// 釋出自定義消息====
ros::Publisher pub = n.advertise<: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
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_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
#include // action 伺服器
#include // 自定義的 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<: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;// 标志
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]);
for(int i=1; i<=goal->order; i++)// order 為序列數量
{
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
as_.setPreempted();
success = false;
break;
}
// 産生後一個數
feedback_.sequence.push_back(feedback_.sequence[i] + feedback_.sequence[i-1]);
as_.publishFeedback(feedback_);// 釋出
r.sleep();
}
if(success)
{
// 最終結果
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
}
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "fibonacci server");
FibonacciAction fibonacci("fibonacci");
ros::spin();
return 0;
}
2. 行動用戶端 類似 服務消費者
#include
#include // action 用戶端
#include // action 狀态
#include // 自定義行動類型
int main (int argc, char **argv)
{
ros::init(argc, argv, "fibonacci client");
// action 用戶端 =====
actionlib::SimpleActionClient<:fibonacciaction> ac("fibonacci", true);
ROS_INFO("Waiting for action server to start.");
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
#include // 動态參數 調整
#include // 自定義的 配置參數清單
// cfg/parameter_server_tutorials.cfg===========
// 參數改變後 的回調函數，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_config> server;// 參數伺服器
dynamic_reconfigure::Server<: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
#include // 坐标變換釋出/廣播
#include // 小烏龜位置類型
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
#include // 坐标變換監聽
#include // 消息類型
#include // 生成一個小烏龜
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<:spawn>("spawn"); // 服務用戶端
turtlesim::Spawn srv;
add_turtle.call(srv); // 調用服務
// 釋出小烏龜運動指令=====
ros::Publisher turtle_vel =
node.advertise<: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
![roszhong指定rviz的點啟動_Rviz 實作 pannel 插件[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)