一.话题与服务编程
话题与服务编程:通过代码新生一只海龟,放置在(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文章来源:https://www.toymoban.com/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})
运行:文章来源地址https://www.toymoban.com/news/detail-472150.html
roscore
rosrun demo_tf tf_listener
rosrun demo_tf tf_broadcaster
到了这里,关于ROS:古月居第一次作业(话题与服务编程、动作编程、TF编程)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!