ROS：古月居第一次作业（话题与服务编程、动作编程、TF编程）

一.话题与服务编程

话题与服务编程：通过代码新生一只海龟，放置在（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;
     //通过服务调用，产生第二只乌龟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!");
     //发布速度信息
     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的goal
     demo_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;
          } 
          else
               r.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=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数据
          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();//此处不能设置为ros::Time::now(),两者是不同的，可以看下面参考网址
        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 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})

运行：

roscore

rosrun demo_tf tf_listener

rosrun demo_tf tf_broadcaster