Gazebo-Ros搭建小车和场景并运行slam算法进行建图4--为机器人添加运动控制器控制其移动

Gazebo-Ros搭建小车和场景并运行slam算法进行建图4–为机器人添加运动控制器控制其移动

1.要想机器人小车在gazebo中运动还需要为其添加运动插件

在文章3中的my_robot2.urdf 最下边(前边)添加如下部分:

​ 这里使用的二轮差速控制,选择对应的插件libgazebo_ros_diff_drive.so


  
    Debug
    false
    1
    false
    100.0
    left_wheel_joint
    right_wheel_joint
    0.4
    0.12
    1.8
    cmd_vel
    odom
    base_footprint
    odom
    1
  


  
    
      /my_robot
    
  
其中
0.4 这个是轮距
0.12 这个是轮子直径,根据机器人模型的实际参数设置

添加上述部分,文章3中的my_robot2.urdf 更新为 --完整代码如下:




    
        
	    
            
                
                    
        

        
	    
            
                
            
	    
                
            
        

	
            
            
            
        
    

    
   	Gazebo/Orange
    

    
        
        
        
        
    

    
	
	    
            
                
            
	

        
            
            
                
            
        

	
            
            
            
        		
    

    
        
        
        
        
    

    
	
	    
            
                
            
	

        
            
            
                
            
        

	
            
            
            
        
    

    
        
        
        
        
    

    
	
	    
            
                
            
	

        
            
            
                
            
        

	
            
            
            
        
    

    
        
        
        
        
    

    
	
	    
            
                
            
	

        
            
            
                
            
        

	
            
            
            
        
    

    
	
	    
                      
                
            
	

        
            
                      
                
            
            
                
            
        

	
            
            
            
        
    

    
   	Gazebo/Black
	
      30.0
      
        1.3962634
        
          800
          800
          R8G8B8
        
        
          0.02
          300
        
        
          gaussian
          
          0.0
          0.007
        
      
      
        true
        0.0
        rrbot/camera1
        image_raw
        camera_info
        camera_link
        0.07
        0.0
        0.0
        0.0
        0.0
        0.0
      
	
    

    
        
        
        
    

    
	
	   
	   
		
	   
	

	
	   
	   
		
	   
	   
	

	
            
            
            
        
    

    
   	Gazebo/Black
	
      	   0 0 0 0 0 0
      	   false
      	   40
      	   
        	
          	   
            		720
            		1
            		-1.570796
            		1.570796
          	   
        	
        	
          	   0.10
          	   30.0
          	   0.01
        	
        	
          	   gaussian
          	
          	   0.0
          	   0.01
        	
      	   
       	   
        	/scan
        	laser_link
      	   
    	
    

    
        
        
        
    

   
	
	    
                      
                
            
	

        
            
                      
                
            
        

	
            
            
            
        
    

  
    true
    
      true
      100
      true
      __default_topic__
      
        imu
        imu_link
        10.0
        0.0
        0 0 0
        0 0 0
        imu_link
        false
      
      0 0 0 0 0 0
    
  

    
        
        
        
    


  
    Debug
    false
    1
    false
    100.0
    left_wheel_joint
    right_wheel_joint
    0.4
    0.12
    1.8
    cmd_vel
    odom
    base_footprint
    odom
    1
  


  
    
      /my_robot
    
  


2.使用turtlebot3中的控制机器人的文件

​ 2.1在src目录下创建teleop_robot功能包,并添加依赖rospy geometry_msgs

​ $ cd ~/Documents/test_ws/src
​ $ catkin_create_pkg teleop_robot rospy geometry_msgs

​ 2.2 在teleop_robot/src文件夹下创建文件teleop_robot_key.py

​ $ cd src/teleop_robot/src/
​ $ gedit teleop_robot_key

​ 输入以下内容:

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
import sys, select, os
if os.name == 'nt':
  import msvcrt, time
else:
  import tty, termios

BURGER_MAX_LIN_VEL = 0.32
BURGER_MAX_ANG_VEL = 2.84

WAFFLE_MAX_LIN_VEL = 0.26
WAFFLE_MAX_ANG_VEL = 1.82

LIN_VEL_STEP_SIZE = 0.01
ANG_VEL_STEP_SIZE = 0.1

msg = """
Control Your TurtleBot3!
---------------------------
Moving around:
        w
   a    s    d
        x

w/x : increase/decrease linear velocity (Burger : ~ 0.32, Waffle and Waffle Pi : ~ 0.26)
a/d : increase/decrease angular velocity (Burger : ~ 2.84, Waffle and Waffle Pi : ~ 1.82)

space key, s : force stop

CTRL-C to quit
"""

e = """
Communications Failed
"""

def getKey():
    if os.name == 'nt':
        timeout = 0.1
        startTime = time.time()
        while(1):
            if msvcrt.kbhit():
                if sys.version_info[0] >= 3:
                    return msvcrt.getch().decode()
                else:
                    return msvcrt.getch()
            elif time.time() - startTime > timeout:
                return ''

    tty.setraw(sys.stdin.fileno())
    rlist, _, _ = select.select([sys.stdin], [], [], 0.1)
    if rlist:
        key = sys.stdin.read(1)
    else:
        key = ''

    termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
    return key

def vels(target_linear_vel, target_angular_vel):
    return "currently:\tlinear vel %s\t angular vel %s " % (target_linear_vel,target_angular_vel)

def makeSimpleProfile(output, input, slop):
    if input > output:
        output = min( input, output + slop )
    elif input < output:
        output = max( input, output - slop )
    else:
        output = input

    return output

def constrain(input, low, high):
    if input < low:
      input = low
    elif input > high:
      input = high
    else:
      input = input

    return input

def checkLinearLimitVelocity(vel):
    if turtlebot3_model == "burger":
      vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)
    elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
      vel = constrain(vel, -WAFFLE_MAX_LIN_VEL, WAFFLE_MAX_LIN_VEL)
    else:
      vel = constrain(vel, -BURGER_MAX_LIN_VEL, BURGER_MAX_LIN_VEL)

    return vel

def checkAngularLimitVelocity(vel):
    if turtlebot3_model == "burger":
      vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)
    elif turtlebot3_model == "waffle" or turtlebot3_model == "waffle_pi":
      vel = constrain(vel, -WAFFLE_MAX_ANG_VEL, WAFFLE_MAX_ANG_VEL)
    else:
      vel = constrain(vel, -BURGER_MAX_ANG_VEL, BURGER_MAX_ANG_VEL)

    return vel

if __name__=="__main__":
    if os.name != 'nt':
        settings = termios.tcgetattr(sys.stdin)

    rospy.init_node('turtlebot3_teleop')
    pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)

    turtlebot3_model = rospy.get_param("model", "burger")

    status = 0
    target_linear_vel   = 0.0
    target_angular_vel  = 0.0
    control_linear_vel  = 0.0
    control_angular_vel = 0.0

    try:
        print(msg)
        while not rospy.is_shutdown():
            key = getKey()
            if key == 'w' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel + LIN_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'x' :
                target_linear_vel = checkLinearLimitVelocity(target_linear_vel - LIN_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'a' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel + ANG_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == 'd' :
                target_angular_vel = checkAngularLimitVelocity(target_angular_vel - ANG_VEL_STEP_SIZE)
                status = status + 1
                print(vels(target_linear_vel,target_angular_vel))
            elif key == ' ' or key == 's' :
                target_linear_vel   = 0.0
                control_linear_vel  = 0.0
                target_angular_vel  = 0.0
                control_angular_vel = 0.0
                print(vels(target_linear_vel, target_angular_vel))
            else:
                if (key == '\x03'):
                    break

            if status == 20 :
                print(msg)
                status = 0

            twist = Twist()

            control_linear_vel = makeSimpleProfile(control_linear_vel, target_linear_vel, (LIN_VEL_STEP_SIZE/2.0))
            twist.linear.x = control_linear_vel; twist.linear.y = 0.0; twist.linear.z = 0.0

            control_angular_vel = makeSimpleProfile(control_angular_vel, target_angular_vel, (ANG_VEL_STEP_SIZE/2.0))
            twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = control_angular_vel

            pub.publish(twist)

    except:
        print(e)

    finally:
        twist = Twist()
        twist.linear.x = 0.0; twist.linear.y = 0.0; twist.linear.z = 0.0
        twist.angular.x = 0.0; twist.angular.y = 0.0; twist.angular.z = 0.0
        pub.publish(twist)

    if os.name != 'nt':
        termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)

3.进入工作空间根目录进行编译

$ cd ~/Documents/test_ws
$ catkin_make
$ source devel/setup.bash
$ rospack find teleop_robot  (编译好的话可以找到这个功能包的所在路径)

4.开启三个终端测试控制效果

进入终端进入到~/Documents/test_ws后需要执行一下
$ source devel/setup.bash 

然后分别在三个终端运行以下三个命令:
1.打开gazebo
$ roslaunch myrobot_description test.launch 

2.打开rviz
$ roslaunch myrobot_description display_my_robot.launch 

3.运行控制节点,根据提示可以改变线速度和角速度让机器人动起来
$ rosrun teleop_robot teleop_robot_key

Gazebo-Ros搭建小车和场景并运行slam算法进行建图4--为机器人添加运动控制器控制其移动_第1张图片

Gazebo-Ros搭建小车和场景并运行slam算法进行建图4--为机器人添加运动控制器控制其移动_第2张图片

5.接下来就是运行slam算法了

主要就是slam算法订阅的传感器主题要和机器人发布的传感器一致,这里是/scan,还有坐标系,这里雷达的坐标系是 laser_link .

你可能感兴趣的:(Gazebo仿真,自动驾驶,人工智能)