一.话题与服务编程: 通过代码新生一只小海龟, 放置在(5.5,5.5)点, 命名为 “JerryXXXX”; 通过代码订阅JerryXXXX的实时位置并在终端打印; 编程控制JerryXXXX实现旋转运动(学号为奇数的小海龟顺时针旋转,学号为偶数的小海龟逆时针旋转,旋转直径:学号末三位除以3的余数,若整除直径取2.5);
1、创建一个名为ROS_2019217987的工作空间,src为功能包
mkdir -p ~/ROS_2019217987/src
切换路径到src的目录下
cd ~/ROS_2019217987/src
初始化工作空间
catkin_init_workspace
结果如下
2、返回工作空间
cd ~/ROS_2019217987
并对工作空间进行编译
catkin_make
结果如下
3、在bash中注册工作区
source devel/setup.bash
验证上一步是否成功(自信即巅峰)
echo $ROS_PACKAGE_PATH
4、在功能包里面创建一个数据包lesson2_homework,先切换到工作区
cd src
使用catkin_create_pkg命令去创建一个叫lesson2_homework的包,依赖包roscpp rospy std_msgs
catkin_create_pkg lesson2_homework roscpp rospy std_msgs
结果如下
5、进入数据包lesson2_homework的src目录下,在src目录中创建一个turtle_control.cpp文件
cd ~/ROS_2019217987/src/lesson2_homework/src
创建turtle_control.cpp
vi turtle_control.cpp
在末尾输入:wq
保存退出,在控制台输入gedit turtle_control.cpp
修改里面的内容。
(或者自己在电脑直接创建一个写好的文件,然后拖进~/ROS_2019217987/src/lesson2_homework/src
就大功告成了。)
在turtle_control.cpp填入以下内容
#include <ros/ros.h>
#include <turtlesim/Spawn.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Pose.h>
ros::Publisher turtle_vel;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
ROS_INFO("Turtle2 position:(%f %f)",msg->x,msg->y);
}
int main(int argc,char** argv)
{
//初始化节点
ros::init(argc,argv,"turtle_control");
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("The turtle2 has been spawn!");
//订阅乌龟的pose信息
ros::Subscriber sub=node.subscribe("turtle2/pose",10,&poseCallback);
//定义turtle的速度控制发布器
turtle_vel=node.advertise<geometry_msgs::Twist>("turtle2/cmd_vel",10);
ros::Rate rate(10.0);
while(node.ok())
{
//发布速度控制命令
geometry_msgs::Twist vel_msg;
vel_msg.angular.z=1;
vel_msg.linear.x=1;
turtle_vel.publish(vel_msg);
ros::spinOnce();
rate.sleep();
}
return 0;
}
在终端打开并修改CMakeLists.txt的内容,也可以直接在手动在文件夹中打开并修改。
cd ~/ROS_2019217987/src/lesson2_homework
gedit CMakeLists.txt
在CMakeLists.txt里面添加相关的配置,在build部分加入下面的两行代码
(关于CMakeLists.txt:规定了这个package要依赖哪些package,要编译生成哪些目标,如何编译等等 流程。)
add_executable(turtle_control src/turtle_control.cpp)
target_link_libraries(turtle_control ${catkin_LIBRARIES})
第一行的意思是把程序文件src/turtle_control.cpp
编译为可执行文件turtle_control
,第二行是turtle_control
与ROS一些库进行链接。
lesson2_homework工程包下新建一个launch文件夹,然后在文件夹里新建一个turtle_control.launch文件(可以通过运行.launch文件来运行turtle_control.cpp里面的代码,当一个程序需要多个.cpp文件同时运行时就可以把这些.cpp文件写入.launch文件,达到同时运行多个.cpp文件的目的)
<launch>
<node pkg="turtlesim" type="turtlesim_node" name="turtlesim_node"/>
<node pkg="lesson2_homework" type="turtle_control" name="turtle_control"
output="screen"/>
</launch>
6、进行编译
cd ~/ROS_2019217987
catkin_make
打开一个新的终端启动ros核心程序
roscore
再打开一个终端
cd ~/ROS_2019217987
source ./devel/setup.bash
rosrun lesson2_homework turtle_control
再打开一个终端
rosrun turtlesim turtlesim_node
基本已经成型了,不过任务还没有完成
修改turtle_control.cpp的内容
vel_msg.angular.z=1; //机器人的自转速度,+左转,-右转,单位是rad/s
vel_msg.linear.x=1; //线速度 m/s
设置顺时针
vel_msg.angular.z=-1; //机器人的自转速度,+左转,-右转,单位是rad/s
修改线速度来修改半径,小工告成
二、修改turtlesim的背景颜色(学号最末两位除以3的余数,为1改为红色,为2改为绿色,为0改为蓝色)
设置turtlesim的背景颜色的代码
/**
* 该例程设置/读取海龟例程中的参数
*/
/**
* 该例程设置/读取海龟例程中的参数
*/
#include <string>
#include <ros/ros.h>
#include <std_srvs/Empty.h>
#define Stu_num 64
//学号后两位宏定义
int main(int argc, char **argv)
{
int red, green, blue;
// ROS节点初始化
ros::init(argc, argv, "parameter_config");
// 创建节点句柄
ros::NodeHandle node;
//注意:这里的turtlesim的名字应与小海龟仿真节点创建的名字相同!否则会改变不了颜色!
// 设置背景颜色参数
//此处学号后两位余3为0为蓝色,为1为红色,为2为绿色
ros::param::set("/turtlesim/background_r", ((Stu_num%3)==1)?255:0);
ros::param::set("/turtlesim/background_g", ((Stu_num%3)==2)?255:0);
ros::param::set("/turtlesim/background_b", ((Stu_num%3)==0)?255:0);
// 读取背景颜色参数
ros::param::get("/turtlesim/background_r", red);
ros::param::get("/turtlesim/background_g", green);
ros::param::get("/turtlesim/background_b", blue);
//打印颜色参数
ROS_INFO("Re-get Backgroud Color[%d, %d, %d]", red, green, blue);
// 调用服务,刷新背景颜色
ros::service::waitForService("/clear");
ros::ServiceClient clear_background = node.serviceClient<std_srvs::Empty>("/clear");
std_srvs::Empty srv;
//调用服务
clear_background.call(srv);
sleep(1);
return 0;
}
三、TF编程: 广播并监听JerryXXXX的坐标变换 ,让新生的第二只小海龟,命名为TomXXXX去跟随JerryXXXX。
安装新包,试了一下,发现已经安装了
sudo apt-get install ros-melodic-turtle-tf
mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_create_pkg learning_tf roscpp rospy tf turtlesim
catkin_init_workspace
在~/catkin_ws/src/learning_tf/src
这个目录下添加turtle_tf_broadcaster.cpp和turtle_tf_listener.cpp文件代码分别如下
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程产生tf数据,并计算、发布turtle2的速度指令
*/
#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>
std::string turtle_name;
void poseCallback(const turtlesim::PoseConstPtr& msg)
{
// 创建tf的广播器
static tf::TransformBroadcaster br;
// 初始化tf数据
tf::Transform transform;
transform.setOrigin( tf::Vector3(msg->x, msg->y, 0.0) );
tf::Quaternion q;
q.setRPY(0, 0, msg->theta);
transform.setRotation(q);
// 广播world与海龟坐标系之间的tf数据
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_broadcaster");
// 输入参数作为海龟的名字
if (argc != 2)
{
ROS_ERROR("need turtle name as argument");
return -1;
}
turtle_name = argv[1];
// 订阅海龟的位姿话题
ros::NodeHandle node;
ros::Subscriber sub = node.subscribe(turtle_name+"/pose", 10, &poseCallback);
// 循环等待回调函数
ros::spin();
return 0;
};
/***********************************************************************
Copyright 2020 GuYueHome (www.guyuehome.com).
***********************************************************************/
/**
* 该例程监听tf数据,并计算、发布turtle2的速度指令
*/
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
int main(int argc, char** argv)
{
// 初始化ROS节点
ros::init(argc, argv, "my_tf_listener");
// 创建节点句柄
ros::NodeHandle node;
// 请求产生turtle2
ros::service::waitForService("/spawn");
ros::ServiceClient add_turtle = node.serviceClient<turtlesim::Spawn>("/spawn");
turtlesim::Spawn srv;
add_turtle.call(srv);
// 创建发布turtle2速度控制指令的发布者
ros::Publisher turtle_vel = node.advertise<geometry_msgs::Twist>("/turtle2/cmd_vel", 10);
// 创建tf的监听器
tf::TransformListener listener;
ros::Rate rate(10.0);
while (node.ok())
{
// 获取turtle1与turtle2坐标系之间的tf数据
tf::StampedTransform transform;
try
{
listener.waitForTransform("/turtle2", "/turtle1", ros::Time(0), ros::Duration(3.0));
listener.lookupTransform("/turtle2", "/turtle1", ros::Time(0), transform);
}
catch (tf::TransformException &ex)
{
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
continue;
}
// 根据turtle1与turtle2坐标系之间的位置关系,发布turtle2的速度控制指令
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;
};
要修改一下learning_tf文件夹里面的CMakeLists.txt文件,加入以下内容
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})
接下来需要添加launch文件在终端输入以下代码,不添加launch文件会导致输入较多的命令才能运行这个程序。
(ps:launch文件是系统文件,是链接其他包里面的功能的,不需要自己包里面有什么可编程的东西,即不需要依赖包。)
cd ~/catkin_ws/src
catkin_create_pkg learning_launch
一般在learning_launch里面创建一个launch文件夹,launch文件夹里面放的是.launch文件,创建.launch文件
<launch>
<!-- Turtlesim Node-->
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle2" name="turtle2_tf_broadcaster" />
<node pkg="learning_tf" type="turtle_tf_listener" name="listener" />
</launch>
下面.launch代码简单说明
#启动海龟仿真器,海龟键盘控制节点
<node pkg="turtlesim" type="turtlesim_node" name="sim"/>
<node pkg="turtlesim" type="turtle_teleop_key" name="teleop" output="screen"/>
#这个节点是位于learning_tf里面的turtle_tf_broadcaster
#type可执行文件名,name执行时的节点名
<node pkg="learning_tf" type="turtle_tf_broadcaster" args="/turtle1" name="turtle1_tf_broadcaster" />
接下来就要编译一下
cd ~/catkin_ws
catkin_make
注册一下
source ./devel/setup.bash
运行一下,这里是直接运行.launch文件
roslaunch learning_launch start_tf_demo_c++.launch
点击终端可以控制一个小海龟运动。