习题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="[email protected]">Dominic Jud</maintainer> <license>BSD</license> <author email="[email protected]">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--