ROS编程（ETH）2018更新版习题说明（三）
【摘要】 习题3
课程要点：
- ROS发布器
-rqt用户界面
- TF转换系统（可选）
- 机器人模型（URDF）（可选）
- 仿真描述（SDF）（可选）
--练习--
本课练习的目标是实现Husky机器人闭环控制。 首先，从激光扫描中获取支柱（singlepillar）的位置，然后控制机器人，使其行驶到支柱附近。
1. 修改上次练习中的启动文件，以便： a. ...
习题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
-
<?xml version="1.0"?>
-
<!--
-
-->
-
<launch>
-
-
<arg name="world_name" default="$(find husky_highlevel_controller)/worlds/singlePillar.world"/>
-
-
<arg name="laser_enabled" default="true"/>
-
<arg name="kinect_enabled" default="false"/>
-
-
<include file="$(find gazebo_ros)/launch/empty_world.launch">
-
<arg name="world_name" value="$(arg world_name)"/> <!-- world_name is wrt GAZEBO_RESOURCE_PATH environment variable -->
-
<arg name="paused" value="false"/>
-
<arg name="use_sim_time" value="true"/>
-
<arg name="gui" value="true"/>
-
<arg name="headless" value="false"/>
-
<arg name="debug" value="false"/>
-
</include>
-
-
<include file="$(find husky_gazebo)/launch/spawn_husky.launch">
-
<arg name="laser_enabled" value="$(arg laser_enabled)"/>
-
<arg name="kinect_enabled" value="$(arg kinect_enabled)"/>
-
</include>
-
-
<!-- <node pkg="teleop_twist_keyboard" name="teleop_husky" type="teleop_twist_keyboard.py"/> -->
-
-
<node pkg="husky_highlevel_controller" type="husky_highlevel_controller" name="husky_highlevel_controller"
-
output="screen" launch-prefix="gnome-terminal --command">
-
<rosparam command="load" file="$(find husky_highlevel_controller)/config/config.yaml" />
-
</node>
-
-
<node pkg="rviz" type="rviz" name="rviz" />
-
-
</launch>
world
-
<?xml version="1.0" ?>
-
<sdf version="1.4">
-
<world name="default">
-
<!-- A global light source -->
-
<include>
-
<uri>model://sun</uri>
-
</include>
-
<!-- A ground plane -->
-
<include>
-
<uri>model://ground_plane</uri>
-
</include>
-
<!-- Cylinder -->
-
<model name='unit_cylinder'>
-
<pose frame=''>20 5 0.5 0 -0 0</pose>
-
<link name='link'>
-
<inertial>
-
<mass>1</mass>
-
<inertia>
-
<ixx>0.145833</ixx>
-
<ixy>0</ixy>
-
<ixz>0</ixz>
-
<iyy>0.145833</iyy>
-
<iyz>0</iyz>
-
<izz>0.125</izz>
-
</inertia>
-
</inertial>
-
<collision name='collision'>
-
<geometry>
-
<cylinder>
-
<radius>0.2</radius>
-
<length>2</length>
-
</cylinder>
-
</geometry>
-
<max_contacts>10</max_contacts>
-
</collision>
-
<visual name='visual'>
-
<geometry>
-
<cylinder>
-
<radius>0.2</radius>
-
<length>2</length>
-
</cylinder>
-
</geometry>
-
<material>
-
<script>
-
<name>Gazebo/Grey</name>
-
<uri>file://media/materials/scripts/gazebo.material</uri>
-
</script>
-
</material>
-
</visual>
-
<self_collide>0</self_collide>
-
<kinematic>0</kinematic>
-
</link>
-
</model>
-
-
-
</world>
-
</sdf>
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): "<<smallest_distance<<"\n";
-
//ROS_INFO_STREAM("ROS_INFO_STREAM Minimum laser distance(m): "<<smallest_distance);
-
//ROS_INFO("Pillar laser distance(m):%lf", smallest_distance);
-
ROS_INFO("Pillar offset angle(rad):%lf", alpha_pillar);
-
ROS_INFO("pillar x distance(m):%lf", x_pillar);
-
ROS_INFO("pillar y distance(m):%lf", y_pillar);
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 COMPONENTS
-
roscpp
-
sensor_msgs
-
geometry_msgs
-
)
-
-
## DEPENDS: system dependencies of this project that dependent projects also need
-
catkin_package(
-
INCLUDE_DIRS
-
include
-
# LIBRARIES
-
CATKIN_DEPENDS
-
roscpp
-
sensor_msgs
-
geometry_msgs
-
# DEPENDS
-
)
package
-
<?xml version="1.0"?>
-
<package format="2">
-
<name>husky_highlevel_controller</name>
-
<version>0.1.0</version>
-
<description>The husky_highlevel_controller package</description>
-
<maintainer email="dominic.jud@mavt.ethz.ch">Dominic Jud</maintainer>
-
<license>BSD</license>
-
<author email="dominic.jud@mavt.ethz.ch">Dominic Jud</author>
-
-
<buildtool_depend>catkin</buildtool_depend>
-
-
<depend>roscpp</depend>
-
<depend>sensor_msgs</depend>
-
<depend>geometry_msgs</depend>
-
</package>
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 gain
-
float 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 Marker
-
marker.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--
文章来源: zhangrelay.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。
原文链接：zhangrelay.blog.csdn.net/article/details/79956801
【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱：
cloudbbs@huaweicloud.com
- 点赞
- 收藏
- 关注作者
评论（0）