一.話題與服務編程
話題與服務編程：通過代碼新生一只海龜，放置在（5,5）點，命名為“turtle2”；通過代碼訂閱turtle2的實時位置并打印在終端；控制turtle2實現(xiàn)旋轉(zhuǎn)運動；
demo_turtle.launch
<launch>
<!--海龜仿真器-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!--鍵盤控制-->
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
</launch>demo_turtle.cpp
#include <ros/ros.h>
#include <turtlesim/Spawn.h>//創(chuàng)建turtle2
#include <turtlesim/Pose.h>//訂閱turtle的位姿信息
#include <geometry_msgs/Twist.h>//發(fā)布速度信息
ros::Publisher pub;
//回調(diào)函數(shù)打印turtle2的position
void poseCallback(const turtlesim::PoseConstPtr &msg){
ROS_INFO("turtle2: X=[%.2f], Y=[%.5f]",msg->x,msg->y);
}
int main(int argc,char** argv){
ros::init(argc,argv,"create_turtle2");
ros::NodeHandle node;
//通過服務調(diào)用，產(chǎn)生第二只烏龜turtle2
ros::service::waitForService("spawn");
ros::ServiceClient add_turtle=node.serviceClient<turtlesim::Spawn>("spawn");
turtlesim::Spawn srv;
srv.request.x=5;//初始化機器人位置
srv.request.y=5;
srv.request.name="turtle2";
add_turtle.call(srv);
ROS_INFO("turtle2 already!");
//訂閱烏龜位姿信息
ros::Subscriber sub=node.subscribe("/turtle2/pose",10,&poseCallback);
ROS_INFO("publish vel_cmd!");
//發(fā)布速度信息
pub=node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);
ros::Rate r(10.0);
while(ros::ok()){
//發(fā)布速度信息
geometry_msgs::Twist twist;
twist.angular.z=0.5;
twist.linear.x=0.5;
pub.publish(twist);
ros::spinOnce();
r.sleep();
}
return 0;
}CMakeList.txt
add_executable(demo_turtle src/demo_turtle.cpp)
target_link_libraries(demo_turtle ${catkin_LIBRARIES})運行：
roslaunch demo_tf demo_turtle.launch
rosrun demo_tf demo_turtle
結(jié)果：
二.動作編程
動作編程：客戶端發(fā)送一個運動目標，模擬機器人運動到目標位置的過程，包含服務端和客戶端的代碼實現(xiàn)，要求帶有實時位置反饋。
demo_Client_move.cpp
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include "demo_tf/moveAction.h"
typedef actionlib::SimpleActionClient<demo_tf::moveAction> Client;
//當action完成后會調(diào)用該回調(diào)函數(shù)一次
void doneCb(const actionlib::SimpleClientGoalState& state,const demo_tf::moveResultConstPtr& result){
ROS_INFO("Yay! start moving!");
ros::shutdown();
}
//當action激活后會調(diào)用該回調(diào)函數(shù)一次
void activeCb(){
ROS_INFO("Goal just went active");
}
//收到feedback后調(diào)用該回調(diào)函數(shù)
void feedbackCb(const demo_tf::moveFeedbackConstPtr& feedback){
ROS_INFO("percent_complete: X=[%f], Y=[%f], theta=[%f]",feedback->present_turtle_x,feedback->present_turtle_y,feedback->present_turtle_theta);
}
int main(int argc,char** argv){
ros::init(argc, argv, "Client");
//定義一個客戶端
Client client("move_client",true);
//等待服務器端
ROS_INFO("waitint for action server to start.");
client.waitForServer();
ROS_INFO("action server started, sending goal");
//創(chuàng)建一個action的goal
demo_tf::moveGoal goal;
goal.turtle_target_x=6.5;
goal.turtle_target_y=0;
goal.turtle_target_theta=0;
//發(fā)送action的goal給服務器，并且設(shè)置回調(diào)函數(shù)
client.sendGoal(goal,&doneCb,&activeCb,&feedbackCb);
ros::spin();
return 0;
}demo_Service_move.cpp
#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include "demo_tf/moveAction.h"
#include <turtlesim/Pose.h>
#include <geometry_msgs/Twist.h>
typedef actionlib::SimpleActionServer<demo_tf::moveAction> Server;
//創(chuàng)建發(fā)布者
ros::Publisher pub;
//記錄機器人位置
struct pos{
float x;
float y;
float theta;
}origin_pos,target_pos;
//訂閱小烏龜pose
void poseCallBack(const turtlesim::PoseConstPtr& msg){
//打印每一次turtle的位姿
ROS_INFO("Turtle origin position: [%f], [%f], [%f]",msg->x,msg->y,msg->theta);
origin_pos.x=msg->x;
origin_pos.y=msg->y;
origin_pos.theta=msg->theta;
}
//收到action的goal后調(diào)用該回調(diào)函數(shù)
void execute(const demo_tf::moveGoalConstPtr& goal,Server* as){
ros::Rate r(10);
demo_tf::moveFeedback feedback;
ROS_INFO("Turtle goal position: X=[%f], Y=[%f], theta=[%f]",goal->turtle_target_x,goal->turtle_target_y,goal->turtle_target_theta);
target_pos.x=goal->turtle_target_x;
target_pos.y=goal->turtle_target_y;
target_pos.theta=goal->turtle_target_theta;
geometry_msgs::Twist vel_msgs;
while(ros::ok()){
//發(fā)布速度指令
vel_msgs.linear.x = 0.1;
vel_msgs.angular.z = 0.;
pub.publish(vel_msgs);
//發(fā)布反饋信息（當前機器人位置）
feedback.present_turtle_x=origin_pos.x;
feedback.present_turtle_y=origin_pos.y;
feedback.present_turtle_theta=origin_pos.theta;
as->publishFeedback(feedback);
//判斷是否到達目標點
if(target_pos.x<=origin_pos.x&&target_pos.y<=origin_pos.y&&target_pos.theta<=origin_pos.theta){
//發(fā)布速度指令
vel_msgs.linear.x = 0.;
vel_msgs.angular.z = 0.;
pub.publish(vel_msgs);
break;
}
else
r.sleep();
}
as->setSucceeded();
}
int main(int argc,char** argv){
ros::init(argc,argv,"Server");
ros::NodeHandle n,node_server;
//訂閱小烏龜?shù)奈恢眯畔? ros::Subscriber sub=node_server.subscribe("turtle1/pose",10,&poseCallBack);
//申請發(fā)布速度
pub=node_server.advertise<geometry_msgs::Twist>("turtle1/cmd_vel",10);
//定義一個服務器 "move_client"為客戶端名稱（要注意對應）
Server server(n,"move_client",boost::bind(&execute,_1,&server),false);
ROS_INFO("waiting_Server");
//服務器開始運行
server.start();
ROS_INFO("start_Server");
ros::spin();
return 0;
}debug.launch?
<launch>
<!--海龜仿真器-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<!--Server-->
<node pkg="demo_tf" type="demo_Service_move" name="Server" output="screen"/>
</launch>CMakeList.txt
add_executable(demo_Client_move src/demo_Client_move.cpp)
target_link_libraries(demo_Client_move ${catkin_LIBRARIES})
add_dependencies(demo_Client_move ${${PROJECT_NAME}_EXPORTED_TARGETS})
add_executable(demo_Service_move src/demo_Service_move.cpp)
target_link_libraries(demo_Service_move ${catkin_LIBRARIES})
add_dependencies(demo_Service_move ${${PROJECT_NAME}_EXPORTED_TARGETS})運行：
roslaunch demo_tf demo_turtle.launch
rosrun demo_tf demo_Client_move三.TF編程
廣播并監(jiān)聽機器人你的坐標變換，已知激光雷達和機器人底盤的坐標關(guān)系，求解激光雷達數(shù)據(jù)在底盤坐標系下的坐標值。
tf_broadcaster.cpp
/*
發(fā)布tf變換
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
int main(int argc,char** argv){
ros::init(argc,argv,"tf_nroadcaster");
ros::NodeHandle node;
static tf::TransformBroadcaster br;
while(ros::ok()){
//初始化tf數(shù)據(jù)，設(shè)定變換矩陣，也就是激光雷達在底盤坐標系下轉(zhuǎn)換
//laser_word*word_base=laser_base=transform
tf::Transform transform;
transform.setOrigin(tf::Vector3(0.1,0.1,0.2));
transform.setRotation(tf::Quaternion(0,0,0,2));
//廣播base_link和base_laser坐標系之間的tf數(shù)據(jù)
br.sendTransform(tf::StampedTransform(transform,ros::Time::now(),"base_link","base_laser"));
}
return 0;
}tf_listener.cpp
/*
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/PointStamped.h>
int main(int argc, char *argv[])
{
ros::init(argc, argv, "tf_listener");
ros::NodeHandle n;
//創(chuàng)建監(jiān)聽者
tf::TransformListener listener;
ros::Rate rate(10.0);
while (ros::ok())
{
geometry_msgs::PointStamped laser_point;
laser_point.header.frame_id="base_laser";
laser_point.header.stamp=ros::Time();//此處不能設(shè)置為ros::Time::now(),兩者是不同的，可以看下面參考網(wǎng)址
laser_point.point.x=0.3;
laser_point.point.y=0.0;
laser_point.point.z=0.0;
try{
listener.waitForTransform("base_link","base_laser",ros::Time(0),ros::Duration(3.0));
geometry_msgs::PointStamped base_point;
listener.transformPoint("base_link",laser_point,base_point);
ROS_INFO("base_laser: (%.2f, %.2f. %.2f) -----> base_link: (%.2f, %.2f, %.2f) at time %.2f",
laser_point.point.x, laser_point.point.y, laser_point.point.z,
base_point.point.x, base_point.point.y, base_point.point.z, base_point.header.stamp.toSec());
}catch(tf::TransformException& ex){
ROS_ERROR("Received an exception trying to transform a point from \"base_laser\" to \"base_link\": %s", ex.what());
}
rate.sleep();
}
return 0;
}CMakeLists.txt文章來源：http://www.zghlxwxcb.cn/news/detail-472150.html
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
tf
)
add_executable(tf_broadcaster src/tf_broadcaster.cpp)
target_link_libraries(tf_broadcaster ${catkin_LIBRARIES})
add_executable(tf_listener src/tf_listener.cpp)
target_link_libraries(tf_listener ${catkin_LIBRARIES})運行：文章來源地址http://www.zghlxwxcb.cn/news/detail-472150.html
roscore
rosrun demo_tf tf_listener
rosrun demo_tf tf_broadcaster到了這里，關(guān)于ROS：古月居第一次作業(yè)（話題與服務編程、動作編程、TF編程）的文章就介紹完了。如果您還想了解更多內(nèi)容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！
