ROS编程（ETH）2018更新版习题说明（三）
习题3
课程要点：
- ROS发布器
-rqt用户界面
- TF转换系统（可选）
- 机器人模型（URDF）（可选）
- 仿真描述（SDF）（可选）
--练习--
本课练习的目标是实现Husky机器人闭环控制。 首先，从激光扫描中获取支柱（singlepillar）的位置，然后控制机器人，使其行驶到支柱附近。
1. 修改上次练习中的启动文件，以便：
a. 键盘遥控节点删除（keyboard twist node）。
b. $（find husky_highlevel_controller）/worlds/singlePillar.world作为世界环境加载，将singlePillar.world文件从RSL主页上下载Zip文件并复制到该文件夹。
A: 提示与结果
launch
world
model://sun model://ground_plane 20 5 0.5 0 -0 0 1 0.145833 0 0 0.145833 0 0.125 0.2 2 10 0.2 2 0 0 2. 从激光扫描中提取支柱相对于机器人的位置信息。
A: 这里只有一个支柱 ( 类似实验2 )
HuskyHighlevelController.hpp
//------Pillar info----
pillar position
float x_pillar;
float y_pillar;
// the orientation of the pillar with respect to the x_axis
float alpha_pillar;
HuskyHighlevelController.cpp
int arr_size = floor((scan_msg.angle_max-scan_msg.angle_min)/scan_msg.angle_increment);for (int i=0 ; i< arr_size ;i++){if (scan_msg.ranges[i] < smallest_distance){smallest_distance = scan_msg.ranges[i];alpha_pillar = (scan_msg.angle_min + i*scan_msg.angle_increment);}}//Pillar Husky offset pose x_pillar = smallest_distance*cos(alpha_pillar);y_pillar = smallest_distance*sin(alpha_pillar);//cout<<"cout Minimum laser distance(m): "<
3. 在创建一个发布者到主题 /cmd_vel 上，以便能够向Husky发送速度指令（twist），需要将geometry_msgs作为依赖项添加到CMakeLists.txt和package.xml（与sensor_msgs的结构相同）。 （参考讲座2，第18幻灯片）
A:
CMakeLists
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTSroscppsensor_msgsgeometry_msgs
)## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(INCLUDE_DIRSinclude
# LIBRARIESCATKIN_DEPENDS roscppsensor_msgsgeometry_msgs
# DEPENDS
)package
husky_highlevel_controller 0.1.0 The husky_highlevel_controller package Dominic Jud BSD Dominic Jud catkin roscpp sensor_msgs geometry_msgs
4. 编写一个简单的P控制器，使Husky行驶到支柱附件。 注意，使用ROS参数修改控制器增益（参考讲座2，第21幻灯片）， 将代码写入到激光扫描主题的回调函数中。
A:
hpp
//----Subscribers----// // subscriber to /scan topic
ros::Subscriber scan_sub_;
std::string subscriberTopic_;
//------Pillar info----pillar position
float x_pillar;
float y_pillar;
// the orientation of the pillar with respect to the x_axis
float alpha_pillar;
//-----Publishers-----publisher to /cmd_vel
ros::Publisher cmd_pub_;
//------msgs-------twist
msggeometry_msgs::Twist vel_msg_;
cpp
//P-Controller to drive husky towards the pillar//propotinal gainfloat p_gain_vel = 0.1;float p_gain_ang = 0.4;if(x_pillar>0.2){if (x_pillar <= 0.4 ){vel_msg_.linear.x = 0; vel_msg_.angular.z = 0;}else {vel_msg_.linear.x = x_pillar * p_gain_vel ;vel_msg_.angular.z = -(y_pillar * p_gain_ang) ;}}else{vel_msg_.linear.x = 0;vel_msg_.angular.z = 0;}cmd_pub_.publish(vel_msg_);
5. 将一个RobotModel插件添加到RViz，可视化Husky机器人。 （参考讲座3，第17幻灯片）
A:
6. 将一个TF显示插件添加到RViz。 （参考讲座3，第7幻灯片）
A:
7. 发布RViz的可视化标记，显示支柱的估计位置。 两种方式如下：
（简易）将激光帧中的点作为RViz标记发布。 RViz会自动将标记转换为odom坐标。
参考：http://wiki.ros.org/rviz/DisplayTypes/Marker
（困难）实现一个TF监听器，将提取的点从激光帧变换到odom帧。
参考：http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28C%2B%2 B％29
在odom坐标中将该点作为RViz标记发布。参考：http://wiki.ros.org/rviz/DisplayTypes/Marker
A:
cpp
//RViz Markermarker.header.frame_id = "base_laser"; //base no marker.header.stamp = ros::Time();marker.ns = "pillar";marker.id = 0;marker.type = visualization_msgs::Marker::SPHERE;marker.action = visualization_msgs::Marker::ADD;marker.pose.position.x = x_pillar;marker.pose.position.y = y_pillar; marker.scale.x = 0.2;marker.scale.y = 0.2;marker.scale.z = 2.0;marker.color.a = 1.0; // Don't forget to set the alpha!marker.color.r = 0.1;marker.color.g = 0.1;marker.color.b = 0.1;vis_pub_.publish(marker);对比如下两图差异：
原图（选自原文档中）：
--评分标准--
启动launch文件。 Husky应该向支柱驶去。
1. Husky正常行驶[20％]
2. Husky到达支柱附近[30％]
查看RViz配置（TF、机器人模型和激光扫描均正常显示）。[20％]
可视化标记正确显示在RViz中。[30％]
--End--
本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！