ROS turtlebot mapping

ROS tuttlebot mapping

假设已经安装好turtlebot、arbotox等模拟平台，需要安装以下工具包

sudo apt-get install ros-indigo-move-basesudo apt-get install ros-indigo-map-server

cmd lines

• 启动：依次在不同的terminal中打开运行以下命令

  roscore roslaunch rbx1_bringup fake_turtlebot.launch #turtlebot平台运行roslaunch rbx1_nav fake_move_base_blank_map.launch  # map bulidingrosrun rviz rviz -d `rospack find rbx1_nav`/sim.rviz # 运行rviz实时显示

• 运行

  rostopic pub /move_ba_simple/goal geometry_msgs/PoseStamped '{ header: { frame_id: "base_link" }, pose: { position: { x: 1.0, y: 0, z: 0 }, orientation: { x: 0, y: 0, z: 0, w: 1 } } }'

  在运行的时候点击2D navigation，再通过鼠标左键选择点，即可实现turtlebot自动路径规划（当它停止后，再选择终点好像就无效了）

有障碍物的地图构建与导航

安装

需要安装以下安装包

sudo apt-get install ros-indigo-gmappingsudo apt-get install ros-indigo-amcl

安装完之后，重启Terminal，依次运行以下命令

roslaunch rbx1_nav fake_move_base_map_with_obstacles.launchroslaunch rbx1_bringup fake_turtlebot.launch

在workspace中的src/rbx1/rbx1_nav/node中新建py文件move_base_obstacle.py如下

#!/usr/bin/env pythonimport roslib; roslib.load_manifest('rbx1_nav')import rospyimport actionlibfrom actionlib_msgs.msg import *from geometry_msgs.msg import Pose, Point, Quaternion, Twistfrom move_base_msgs.msg import MoveBaseAction, MoveBaseGoalfrom tf.transformations import quaternion_from_eulerfrom visualization_msgs.msg import Markerfrom math import radians, piclass MoveBaseSquare():    def __init__(self):        rospy.init_node('nav_test', anonymous=False)        rospy.on_shutdown(self.shutdown)        # How big is the square we want the robot to navigate?        # 设定正方形的尺寸，默认是一米        square_size = rospy.get_param("~square_size", 1.0) # meters        # Create a list to hold the target quaternions (orientations)        # 创建一个列表，保存目标的角度数据        quaternions = list()        # First define the corner orientations as Euler angles        # 定义四个顶角处机器人的方向角度（Euler angles:http://zh.wikipedia.org/wiki/%E6%AC%A7%E6%8B%89%E8%A7%92)        euler_angles = (pi/2, pi, 3*pi/2, 0)        # Then convert the angles to quaternions        # 将上面的Euler angles转换成Quaternion的格式        for angle in euler_angles:            q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')            q = Quaternion(*q_angle)            quaternions.append(q)        # Create a list to hold the waypoint poses        # 创建一个列表存储导航点的位置        waypoints = list()        # Append each of the four waypoints to the list.  Each waypoint        # is a pose consisting of a position and orientation in the map frame.        # 创建四个导航点的位置（角度和坐标位置）        waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0]))        waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1]))        waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2]))        waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))        # Initialize the visualization markers for RViz        # 初始化可视化标记        self.init_markers()        # Set a visualization marker at each waypoint         # 给每个定点的导航点一个可视化标记(就是rviz中看到的粉色圆盘标记)        for waypoint in waypoints:                       p = Point()            p = waypoint.position            self.markers.points.append(p)        # Publisher to manually control the robot (e.g. to stop it)        # 发布TWist消息控制机器人        self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist)        # Subscribe to the move_base action server        # 订阅move_base服务器的消息        self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)        rospy.loginfo("Waiting for move_base action server...")        # Wait 60 seconds for the action server to become available        # 等待move_base服务器建立        self.move_base.wait_for_server(rospy.Duration(60))        rospy.loginfo("Connected to move base server")        rospy.loginfo("Starting navigation test")        # Initialize a counter to track waypoints        # 初始化一个计数器，记录到达的顶点号        i = 0        # Cycle through the four waypoints        # 主循环,环绕通过四个定点        while i < 4 and not rospy.is_shutdown():            # Update the marker display            # 发布标记指示四个目标的位置，每个周期发布一起，确保标记可见            self.marker_pub.publish(self.markers)            # Intialize the waypoint goal            # 初始化goal为MoveBaseGoal类型            goal = MoveBaseGoal()            # Use the map frame to define goal poses            # 使用map的frame定义goal的frame id            goal.target_pose.header.frame_id = 'map'            # Set the time stamp to "now"            # 设置时间戳            goal.target_pose.header.stamp = rospy.Time.now()            # Set the goal pose to the i-th waypoint            # 设置目标位置是当前第几个导航点            goal.target_pose.pose = waypoints[i]            # Start the robot moving toward the goal            # 机器人移动            self.move(goal)            i += 1    def move(self, goal):            # Send the goal pose to the MoveBaseAction server            # 把目标位置发送给MoveBaseAction的服务器            self.move_base.send_goal(goal)            # Allow 1 minute to get there            # 设定1分钟的时间限制            finished_within_time = self.move_base.wait_for_result(rospy.Duration(60))             # If we don't get there in time, abort the goal            # 如果一分钟之内没有到达，放弃目标            if not finished_within_time:                self.move_base.cancel_goal()                rospy.loginfo("Timed out achieving goal")            else:                # We made it!                state = self.move_base.get_state()                if state == GoalStatus.SUCCEEDED:                    rospy.loginfo("Goal succeeded!")    def init_markers(self):        # Set up our waypoint markers        # 设置标记的尺寸        marker_scale = 0.2        marker_lifetime = 0 # 0 is forever        marker_ns = 'waypoints'        marker_id = 0        marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0}        # Define a marker publisher.        # 定义一个标记的发布者        self.marker_pub = rospy.Publisher('waypoint_markers', Marker)        # Initialize the marker points list.        # 初始化标记点的列表        self.markers = Marker()        self.markers.ns = marker_ns        self.markers.id = marker_id        self.markers.type = Marker.SPHERE_LIST        self.markers.action = Marker.ADD        self.markers.lifetime = rospy.Duration(marker_lifetime)        self.markers.scale.x = marker_scale        self.markers.scale.y = marker_scale        self.markers.color.r = marker_color['r']        self.markers.color.g = marker_color['g']        self.markers.color.b = marker_color['b']        self.markers.color.a = marker_color['a']        self.markers.header.frame_id = 'map'        self.markers.header.stamp = rospy.Time.now()        self.markers.points = list()    def shutdown(self):        rospy.loginfo("Stopping the robot...")        # Cancel any active goals        self.move_base.cancel_goal()        rospy.sleep(2)        # Stop the robot        self.cmd_vel_pub.publish(Twist())        rospy.sleep(1)if __name__ == '__main__':    try:        MoveBaseSquare()    except rospy.ROSInterruptException:        rospy.loginfo("Navigation test finished.")

将其改为可以运行的文件，运行以下命令

rosrun rviz rviz -d `rospack find rbx1_nav`/nav_obstacles.rvizrosrun rbx1_nav move_base_square.py

得到如下的路径规划结果

amcl导航与定位

依次运行以下命令

roslaunch rbx1_bringup fake_turtlebot.launch roslaunch rbx1_nav fake_amcl.launch map:=test_map.yamlrosrun rviz rviz -d `rospack find rbx1_nav`/amcl.rviz

在打开的地图上选择“2D navi goal”，鼠标左键选择目标位置，即可看出路进规划的效果，如下：

实时生成随机目标点并进行路径规划

roslaunch rbx1_nav fake_nav_test.launch rosrun rviz rviz -d `rospack find rbx1_nav`/amcl.rviz

在打开的rviz界面中点击“2d Pose Estimate”，可以看见turtlebot实时趋向于随机给出的终点，如下图