一.话题与服务编程

话题与服务编程：通过代码新生一只海龟，放置在（5,5）点，命名为“turtle2”；通过代码订阅turtle2的实时位置并打印在终端；控制turtle2实现旋转运动；

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>//创建turtle2
#include <turtlesim/Pose.h>//订阅turtle的位姿信息
#include <geometry_msgs/Twist.h>//发布速度信息ros::Publisher pub;//回调函数打印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;//通过服务调用，产生第二只乌龟turtle2ros::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!");//发布速度信息pub=node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel",10);ros::Rate r(10.0);while(ros::ok()){//发布速度信息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

结果：

二.动作编程

动作编程：客户端发送一个运动目标，模拟机器人运动到目标位置的过程，包含服务端和客户端的代码实现，要求带有实时位置反馈。

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完成后会调用该回调函数一次
void doneCb(const actionlib::SimpleClientGoalState& state,const demo_tf::moveResultConstPtr& result){ROS_INFO("Yay! start moving!");ros::shutdown();
}
//当action激活后会调用该回调函数一次
void activeCb(){ROS_INFO("Goal just went active");
}
//收到feedback后调用该回调函数
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");//创建一个action的goaldemo_tf::moveGoal goal;goal.turtle_target_x=6.5;goal.turtle_target_y=0;goal.turtle_target_theta=0;//发送action的goal给服务器，并且设置回调函数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;
//创建发布者
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后调用该回调函数
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()){//发布速度指令vel_msgs.linear.x = 0.1; vel_msgs.angular.z = 0.; pub.publish(vel_msgs);//发布反馈信息（当前机器人位置）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){//发布速度指令vel_msgs.linear.x = 0.; vel_msgs.angular.z = 0.; pub.publish(vel_msgs);break;} elser.sleep();}as->setSucceeded();
}
int main(int argc,char** argv){ros::init(argc,argv,"Server");ros::NodeHandle n,node_server;//订阅小乌龟的位置信息ros::Subscriber sub=node_server.subscribe("turtle1/pose",10,&poseCallBack);//申请发布速度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编程

广播并监听机器人你的坐标变换，已知激光雷达和机器人底盘的坐标关系，求解激光雷达数据在底盘坐标系下的坐标值。

tf_broadcaster.cpp

/*发布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数据，设定变换矩阵，也就是激光雷达在底盘坐标系下转换//laser_word*word_base=laser_base=transformtf::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数据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;//创建监听者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();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

find_package(catkin REQUIRED COMPONENTSroscpprospystd_msgstf
)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})

运行：

roscorerosrun demo_tf tf_listenerrosrun demo_tf tf_broadcaster