zhangrelay的专栏

多机器人系统,机器人三维仿真

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

<?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原创文章,有错请轻拍,转载请注明,谢谢... https://blog.csdn.net/ZhangRelay/article/details/79956801
文章标签: ROS ETH
上一篇ROS机器人操作系统新发布软件包摘录--(2018.03)
下一篇ROS编程(ETH)2018更新版习题说明(四)
想对作者说点什么? 我来说一句

ROS机器人暑期学校资料汇总包

2017年08月01日 150.85MB 下载

ETH ROS Course

2017年11月18日 23.52MB 下载

没有更多推荐了,返回首页

关闭
关闭