ROS_ Core analysis of Python programming case code

Keywords: Python ROS

ROS_ Core analysis of Python programming case code

Through the intermediate tutorial supporting the Handsfree mini robot platform, I learned about ROS_ The knowledge of realizing sensor data reading, motion control and autonomous navigation by python programming is summarized as follows:

1. Sensor data reading

1.1 obtain and print the basic information of the underlying hardware of the robot

# Core code
#!/usr/bin/env
#coding=UTF-8 

import rospy
from handsfree_msgs.msg import robot_state

def callback(data): #Callback function
    rospy.loginfo("the embedded system_time: %fus",data.system_time) #Lower computer system time 
    rospy.loginfo("the embedded cpu temperature is: %f",data.cpu_temperature) #cpu temperature
    rospy.loginfo("the battery voltage is: %f",data.battery_voltage) #Battery voltage
    rospy.loginfo("the battery power remain is: percent %f",data.power_remain*100) #Battery remaining
    rospy.loginfo("-------------------— \n\r") 

def sub_state():
    rospy.init_node('sub_state', anonymous=True)
    rospy.Subscriber("/handsfree/robot_state", robot_state, callback) 
    rospy.spin()

if __name__ == '__main__':
    sub_state()

Essence: rospy.Subscriber("/handsfree/robot_state", robot_state, callback)

The second parameter of the constructor of the topic Subscriber subscriber object is the topic type to subscribe to

1.2 obtain IMU data and convert it into Euler angle for printing

 #!/usr/bin/env
 #coding=UTF-8

 import rospy
 import tf
 from tf.transformations import *
 from sensor_msgs.msg import Imu

 def callback(data):
     #This function is in tf and can convert quaternions to Euler angles
     (r,p,y) = tf.transformations.euler_from_quaternion((data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))
     #Since it is radian system, it looks more convenient to change it to angle system
     rospy.loginfo("Pitch = %f,Roll = %f, Yaw = %f",r*180/3.1415926,p*180/3.1415926,y*180/3.1415926)

 def get_imu():
     rospy.init_node('get_imu', anonymous=True)
     rospy.Subscriber("/handsfree/imu", Imu, callback)
     rospy.spin()

 if __name__ == '__main__':
     get_imu()

Essence: (r,p,y)= tf.transformations.euler_from_quaternion((data.orientation.x,data.orientation.y,data.orientation.z,data.orientation.w))

Among them, the mutual transformation between Euler angle and quaternion in ros can be realized as follows:

# Euler angular quaternion
pos = Pose()
q = tf.transformations.quaternion_from_euler(0, 0, point.z)
pos.orientation.x = q[0]
pos.orientation.y = q[1]
pos.orientation.z = q[2]
pos.orientation.w = q[3]

# Quaternion to Euler angle
(r, p, y) = tf.transformations.euler_from_quaternion([msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w])
sell.fill_euler_msg(msg, r, p, y)

1.3 acquire and print lidar data

Lidar data refers to that after the radar detects the position of the target by transmitting laser beam, the received signal reflected from the target (target echo) is compared with the transmitted signal, and the relevant information of the target can be obtained after appropriate processing. Generally, the lidar driver will be packaged and released through topic.

 #!/usr/bin/env
 #coding=UTF-8 

 import rospy
 from sensor_msgs.msg import LaserScan

 def callback(data):
     rospy.loginfo("the angle_min is %f",data.angle_min) #Print some information
     rospy.loginfo("the angle_max is %f",data.angle_max)
     rospy.loginfo("the scan data is ")
     for i in range(0, len(data.ranges)):
         print data.ranges[i]
     print "\n"

 def sub_state():
     rospy.init_node('sub_scan', anonymous=True)
     rospy.Subscriber("/scan", LaserScan, callback)
     rospy.spin()

 if __name__ == '__main__':
     sub_state()

1.4 obtain camera data and use opencv_python library display

Camera data is an image, which refers to the color rgb image and depth depth image obtained by the camera. Generally, they are encapsulated in the camera driver and can be called directly

 #!/usr/bin/env
 #coding=UTF-8

 import cv2
 import rospy
 from sensor_msgs.msg import Image
 from cv_bridge import CvBridge, CvBridgeError

 def callback(data):
     try:
         cv_image = CvBridge().imgmsg_to_cv2(data, "bgr8") #Using cv_bridge converts it to mat type
     except CvBridgeError as e:
         print(e)
     (rows,cols,channels) = cv_image.shape
     cv2.imshow("Image window", cv_image) #Display image
     cv2.waitKey(30) #Play the next frame after 30ms

 def get_photo():
     rospy.init_node('get_photo', anonymous=True)
     rospy.Subscriber("/camera/rgb/image_raw", Image, callback)
     rospy.spin() 
	   
 if __name__ == '__main__':
     get_photo()

Essence: cv_image = CvBridge().imgmsg_to_cv2(data, "bgr8")

Among them, CvBridge is used to connect ros information and opencv information

2. Control robot movement

2.1 control robot linear motion

The ros system of the industrial control computer of handsfree mini Robot has two codes: linear_move.py and linear_move_by_srv.py

  • linear_move.py transfers parameters by changing the parameter configuration of the code
  • linear_move_by_srv.py passes parameters by calling the Service interface

Where, linear_ The source code of move.py is as follows:

#!/usr/bin/env python
import tf
import math
import rospy
import geometry_msgs.msg

class LinearMove(object):
    def __init__(self):
        self.frame_base = rospy.get_param('~base_frame', '/base_link')
        self.frame_odom = rospy.get_param('~odom_frame', '/odom')
        self.velocity_topic = rospy.get_param('~vel_topic', '/mobile_base/mobile_base_controller/cmd_vel')
        self.tolerance_distance = rospy.get_param('~tolerance', 0.08) # m
        self.speed_linear = rospy.get_param('~speed_linear', 0.1) # m/s
        self.rate_pub = rospy.get_param('~velocity_pub_rate', 10) # 10Hz (10 times a second)
        self.rate = rospy.Rate(self.rate_pub)
        self.vel_pub = rospy.Publisher(self.velocity_topic,
                                       geometry_msgs.msg.Twist,
                                       queue_size=1)
        self.tf_listener = tf.TransformListener()
        rospy.on_shutdown(self.brake)
        try:
            self.tf_listener.waitForTransform(self.frame_odom,
                                              self.frame_base,
                                              rospy.Time(0),
                                              rospy.Duration(5.0))
        except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
            rospy.logerr('tf catch exception when robot waiting for transform......')
            exit(-1)

    def move_to_target(self, dis_to_move=0):
        """
        to make robot move forward/back dis_to_move meters
        :param: dis_to_move: the distance make robot move, >0 means move forward, <0 means move back
        :type: float
        :return:
        """
        self.robot_start_pos = self.__get_robot_pos()
        rospy.logdebug("****************************************************************************")
        rospy.logdebug("robot current position is x = %f, y = %f, try to move forward/back %f Meter"
                       %(self.robot_start_pos.x, self.robot_start_pos.y, dis_to_move))
        rospy.logdebug("****************************************************************************")
        while self.__is_robot_arrived(dis_to_move) is not True:
            self.__move_robot(direction=(1 if dis_to_move > 0 else -1))
            self.rate.sleep()
        self.brake()  # we have arrived the target position, so stop robot
        rospy.loginfo('arrived the target point')

    def __is_robot_arrived(self, dis_to_move):
        """
        to check has the robot arrived target position
        :param: dis_to_move: the distance thar robot needs to move forward/back
        :type: float
        :return: False --- robot has not arrived the target position
                 True --- robot has arrived the target position
        """
        robot_cur_pos = self.__get_robot_pos()
        dis_moved = math.sqrt(math.pow((robot_cur_pos.x - self.robot_start_pos.x), 2) +
                                    math.pow((robot_cur_pos.y - self.robot_start_pos.y), 2))
        dis_need_move = math.fabs(dis_to_move) - dis_moved
        return False if math.fabs(dis_need_move) > self.tolerance_distance else True

    def __move_robot(self, direction=1):
        """
        send velocity to robot according to the direction
        :param: direction: when direction = 1: make robot move forward
                when direction = -1: make robot move back
        :type: int
        :return:
        """
        move_cmd = geometry_msgs.msg.Twist()
        move_cmd.linear.x = math.copysign(self.speed_linear, direction)
        self.vel_pub.publish(move_cmd)

    def __get_robot_pos(self):
        """
        to get current position(x,y,z) of robot
        :return: A geometry_msgs.msg.Point type store robot's position (x,y,z)
        """
        try:
            (trans, rot) = self.tf_listener.lookupTransform(self.frame_odom,
                                                            self.frame_base,
                                                            rospy.Time(0))
        except (tf.Exception, tf.ConnectivityException, tf.LookupException):
            rospy.logerr('tf catch exception when robot looking up transform')
            exit(-1)
        return geometry_msgs.msg.Point(*trans)

    def brake(self):
        """
        send command to stop the robot
        :return:
        """
        self.vel_pub.publish(geometry_msgs.msg.Twist())


if __name__ == '__main__':
    rospy.init_node('LinearMove')
    t_dis = rospy.get_param('~t_dis', 0.2) # m
    if t_dis == 0.0:
        rospy.logerr('no target distance set!')
        exit(-1)
    rospy.loginfo('try to move %f meters'%t_dis)
    LinearMove().move_to_target(dis_to_move=t_dis)
# Essence 1 geometry_msgs.msg.Twist
self.vel_pub = rospy.Publisher(self.velocity_topic,geometry_msgs.msg.Twist,
                               queue_size=1)

geometry_msgs.msg.Twist is a message class. Its original definition is as follows:

Vector3  linear
Vector3  angular

Compactness is defined as follows:

geometry_msgs/Vector3 linear
geometry_msgs/Vector3 angular
# 2 tf.transformlistener class
self.tf_listener = tf.TransformListener()

self.tf_listener.waitForTransform(self.frame_odom,
                                  self.frame_base,
                                  rospy.Time(0),
                                  rospy.Duration(5.0))

By monitoring tf, the tedious calculation of rotation matrix can be avoided and relevant information can be obtained directly. In listening, the two most commonly used functions are: tf.TransformListener.lookupTransform() and tf.TransformListener.transformPoint().

  1. tf.TransformListener.lookupTransform():

(1) Function: obtain the pose relationship from the first coordinate system to the second coordinate system, including rotation and translation; rospy.Time(0) refers to the data stored at the latest time. It cannot be changed to rospy.Time.now(). rospy.Time.now() refers to the current time. Because the monitored data needs to be cached, it cannot be detected in real time, and there will be a delay of several milliseconds

(2) Parameter and return value: the first parameter is the first coordinate system, the second parameter is the second coordinate system, and the third parameter is the time of transformation; The return values (trans, rot) are translation translation and rotation respectively, where rot is in the form of quaternion

(3) Instance function:

# Get the function of the current pose of the robot, where self.tf_ The listener.lookuptransform (self.frame_odom, self.frame_base, rospy. Time (0)) function obtains Odom and base_link transformed pose, that is, the pose of the robot moving relative to the origin, so as to obtain the current state
def __get_robot_pos(self):
    try:
        (trans, rot) = self.tf_listener.lookupTransform(self.frame_odom,
                                                        self.frame_base,
                                                        rospy.Time(0))
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        rospy.logerr('tf catch exception when robot looking up transform')
        exit(-1)
        return geometry_msgs.msg.Point(*trans)
  1. tf.TransformListener.transformPoint():

(1) Function: you can convert the coordinates of points in one coordinate system to another coordinate system

(2) Parameters:

# m_ normal_ The data type of pose is geometry_msgs::PointStamped, m needs to be defined_ normal_ pose.header.frame_ ID is the coordinate system to which the point belongs
# "PTAM_world" is the result of coordinate conversion, and the data type is also geometry_msgs::PointStamped
listener_.transformPoint("PTAM_world",m_normal_pose,pose_PTAM_world);

In addition, linear_ move_ by_ The essence of srv.py source code is as follows:

service_move_dis = rospy.Service('move_distance', handsfree_srv.SpecialMove, callback_linear_move)

2.2 control robot steering motion

The python program controls the robot to rotate left and right along the z-axis, that is, yaw. The code is similar to the linear motion code. The ros system of the industrial control computer of handsfree mini Robot has three codes, namely radial_ turn.py,radian_turn_by_srv.py and radial_ turn_ direct_ by_ srv.py

  • radian_turn.py passes parameters by changing the parameter configuration of the code
  • radian_turn_by_srv.py and radial_ turn_ direct_ by_ Srv.py passes parameters by calling the Service interface
  • radian_turn.py and radial_ turn_ by_ Srv.py will only rotate between - 180 ~ 180 degrees (for example, if you run these two codes and pass parameter 6.28, the robot will not move, but if you run radial_turn_direct_by_srv.py, the robot will rotate once)
  • Counterclockwise rotation, the angle is positive; Clockwise rotation, the angle is negative

Among them, radial_ The main functions of turn.py are as follows:

def __get_robot_pos(self):
    try:
        trans, rot = self.tf_listener.lookupTransform(self.frame_odom,
                                                      self.frame_base,
                                                      rospy.Time(0))
    except (tf.Exception, tf.ConnectivityException, tf.LookupException):
        rospy.logerr('tf catch exception when robot looking up transform')
        exit(-1)
        return tf.transformations.euler_from_quaternion(rot)[2]

def __turn_robot(self, turn_direction=1):
    """
    send velocity command to robot according to the direction
    :param: direction: when direction = 1: make robot turn in clockwise
        when direction = -1: make robot turn in counterclockwise
    :type: int
    :return:
    """
    move_cmd = geometry_msgs.msg.Twist()
    move_cmd.angular.z = math.copysign(self.speed_radian, turn_direction)
    self.vel_pub.publish(move_cmd)    
    
def turn_to_target(self, radian_to_turn=0.0):
    # yaw [-pi,pi]->[0,2*pi)
    robot_start_yaw = (self.__get_robot_pos() + math.pi*2) % (math.pi*2)

    # return target_yaw = radian_to_turn + robot_start_yaw 
    target_yaw = math.copysign(math.fabs(radian_to_turn)%(math.pi*2), radian_to_turn)+robot_start_yaw

    # transform the range of target_yaw
    target_yaw = (target_yaw + math.pi*2) % (math.pi*2)

    # to find the shortest direction to turn
    radian_to_move = target_yaw - robot_start_yaw
    if radian_to_move < -math.pi or math.pi < radian_to_move:
        direction = 1 if radian_to_move < -math.pi else -1
    else:
        direction = 1 if radian_to_move > 0 else -1
    self.brake()  # to stop robot first
    rospy.logdebug("****************************************************************************")
    rospy.logdebug("the robot's Yaw = %f, try to turn to Yaw = %f, the direction = %d"
                  %(robot_start_yaw, target_yaw, direction))
    rospy.logdebug("****************************************************************************")
    while self.__is_robot_arrived(target_yaw) is not True:
        self.__turn_robot(turn_direction=direction)
        self.rate.sleep()
    self.brake()
    rospy.loginfo('arrived the target radian!')

Knowledge learning: understanding of Euler angle

Rotation is always a difficult and confusing point in playing games. It means that there are many ways to rotate, such as simple Euler angles, more complex quaternions and more complex matrices.

Euler angle is the simplest way to express rotation. In the form of a three-dimensional vector, its values represent the rotation angle of the object around the three axes (x, y, z) of the coordinate system. Obviously, different rotation order definitions will produce different rotation results, so general engines will specify their own rotation order.

The rotation values of pitch, yaw and roll about three axes come from the aviation industry and are translated as pitch, yaw and roll respectively,

Note: pitch, yaw, roll and x, y, z do not have a clear and fixed corresponding order, but are only artificially defined, so there may be differences!!!

In addition, the pitch angle value is between [- 90,90] [universal lock], and the yaw angle value is between [- 180180] [before and after crossing 180 °, - 180 °, the nearest rotation direction is opposite (refer to the figure below)]

In the ros system of the industrial control computer of handsfree mini Robot, target_ Move corresponding to yaw_ cmd.angular.z

# Essence 1 tf.transformations.euler_from_quaternion()
return tf.transformations.euler_from_quaternion(rot)[2] #According to the Euler angle list subscript, the code here returns the yaw axis angle value
# Essence 2 
# def turn_to_target(self, radian_to_turn=0.0):
#    robot_start_yaw = (self.__get_robot_pos() + math.pi*2) % (math.pi*2)

# Resolution:
# yaw∈[-pi,pi], (yaw+2*pi)∈[pi,3*pi], (yaw+2*pi)%(2*pi)∈[0,2*pi)
# Essence 3
# def turn_to_target(self, radian_to_turn=0.0):
#    target_yaw = math.copysign(math.fabs(radian_to_turn)%(math.pi*2), radian_to_turn)+robot_start_yaw

# Parse 1: math.fabs (radial_to_turn)% (math. Pi * 2)
# radian_to_turn indicates the angle value to be rotated (in radians). math.fabs() takes the absolute value to ensure that the calculated angle value is positive

# Parse 2: math.copysign()
# Math.copysign (math.fabs (radial_to_turn)% (math. Pi * 2), radial_to_turn) returns a floating-point number, which is composed of the value of the first parameter math.fabs (radial_to_turn)% (math. Pi * 2) [i.e. the arc angle value (positive angle) to be rotated] and the second parameter radial_ to_ Turn [i.e. the radian angle value to be rotated (positive or negative is uncertain)] is composed of the sign
# Essence 4
# radian_to_move = target_yaw-robot_start_yaw
# if radian_to_move < -math.pi or math.pi < radian_to_move:
#     direction = 1 if radian_to_move < -math.pi else -1
# else:
#     direction = 1 if radian_to_move > 0 else -1

# See the figure below for analysis:

radian_ turn_ direct_ by_ The essence of srv.py is as follows:

# def callback_turn_radian(req):
#     radian_to_turn=req.target_radian_turn  
#     while radian_to_turn > 3.14:
#         radian_to_turn = radian_to_turn - 3.14;
#         turn_radian.turn_to_target(3.14)
#     while radian_to_turn < -3.14:
#         radian_to_turn = radian_to_turn + 3.14;
#         turn_radian.turn_to_target(-3.14)
#     turn_radian.turn_to_target(radian_to_turn)
#     return handsfree_srv.SpecialTurnResponse(True)

# Resolution:
# Since the yaw angle value is between [- 180180] [before and after crossing 180 °, - 180 °, the nearest rotation direction is opposite], the source code is radial_ Turn.py and radial_ turn_ by_ Srv.py can only rotate between - 180 ~ 180 degrees, and cannot rotate more than half a turn at one time; And the source code is radial_ turn_ direct_ by_ Srv.py can rotate more than half a turn. The new function is: if you need to rotate more than half a turn, rotate it twice, that is, rotate half a turn first, and then rotate the remaining angle (Note: This is only a way of understanding, and the robot rotation is consistent in actual operation)

2.3 write a keyboard remote control program

So far, I have personally contacted several robot cars, such as racecar, spark, mini, etc. among many source codes, the keyboard remote control program is never absent. Looking at these source codes, we can find that the keyboard remote control programs are almost the same. Therefore, I would like to comment on the keyboard remote control program of handsfree mini Robot, which can be used as a reference when writing keyboard remote control program in the future.

#!/usr/bin/env python
import roslib
import rospy
from geometry_msgs.msg import Twist
import sys, select, termios, tty

msg = """
Reading from the keyboard and Publishing to Twist!
---------------------------
Moving around:
   u    i    o
   j    k    l
   m    ,    .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
   U    I    O
   J    K    L
   M    <    >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
"""

moveBindings = {
		'i':(1,0,0,0),
		'o':(1,0,0,-1),
		'j':(0,0,0,1),
		'l':(0,0,0,-1),
		'u':(1,0,0,1),
		',':(-1,0,0,0),
		'.':(-1,0,0,1),
		'm':(-1,0,0,-1),
		'O':(1,-1,0,0),
		'I':(1,0,0,0),
		'J':(0,1,0,0),
		'L':(0,-1,0,0),
		'U':(1,1,0,0),
		'<':(-1,0,0,0),
		'>':(-1,-1,0,0),
		'M':(-1,1,0,0),
		't':(0,0,1,0),
		'b':(0,0,-1,0),
	       }

speedBindings={
		'q':(1.1,1.1),
		'z':(.9,.9),
		'w':(1.1,1),
		'x':(.9,1),
		'e':(1,1.1),
		'c':(1,.9),
	      }

def getKey():
	# tty.setraw(fd, when=termios.TCSAFLUSH) changes the mode of the file descriptor fd to raw.
	# If when is omitted, it defaults to termios.TCSAFLUSH and is passed to termios.tcsetattr()
	tty.setraw(sys.stdin.fileno())
    # Int select (int maxfdpl, fd_set * readset, fd_set * writeset, fd_set * exceptset, const struct timeout). The first parameter is the maximum file descriptor length, the second is the socket that needs to listen for readability, the third is the socket that needs to listen for writable, the fourth is the socket that needs to listen for exceptions, and the fifth is the time limit setting. Other source codes use the return value of the function: if the listening socket meets the readable and writable conditions, the returned can_read or can_write will have values, and then you can use these returned values for subsequent operations
	select.select([sys.stdin], [], [], 0)
	key = sys.stdin.read(1)
	# int tcsetattr(int fd, int optional_actions, const struct termios *termios_p)
	# The tcsetattr function is used to set terminal parameters. The function returns 0 on success and - 1 on failure
	# Parameter fd is the open terminal file descriptor, and parameter optional_actions is used to control when the modification takes effect, while the structure termios_p saves the parameters to be modified
	# optional_actions can take the following values: tcsnow (change the attribute immediately before data transmission is completed), tcsadrain (change the attribute after all data transmission is completed),
	# TCSAFLUSH: wait until all data transmission is completed and the I / O buffer is cleared before changing the attribute
	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
	return key

speed = 0.30
turn = 0.6

def vels(speed,turn):
	return "currently:\tspeed %s\tturn %s " % (speed,turn)

if __name__ == "__main__":
	# sys.stdin represents standardized input, and termios.tcgetattr(fd) returns a list containing tty attributes of file descriptor fd
	settings = termios.tcgetattr(sys.stdin)
	
	pub = rospy.Publisher('/mobile_base/mobile_base_controller/cmd_vel', Twist, queue_size = 1)
	rospy.init_node('teleop_twist_keyboard')

	x = 0
	y = 0
	z = 0
	th = 0
	status = 0

	try:
		print msg
		print vels(speed,turn)
		while(1):
			key = getKey()
			if key in moveBindings.keys():
				x = moveBindings[key][0]
				y = moveBindings[key][1]
				z = moveBindings[key][2]
				th = moveBindings[key][3]
			elif key in speedBindings.keys():
				speed = speed * speedBindings[key][0]
				turn = turn * speedBindings[key][1]
				print vels(speed,turn)
				if (status == 14):
					print msg
				status = (status + 1) % 15
				x = 0
				y = 0
				z = 0
				th = 0
			else:
				x = 0
				y = 0
				z = 0
				th = 0
				if (key == '\x03'): # ctrl+c
					break

			twist = Twist()
			twist.linear.x = x*speed; twist.linear.y = y*speed; twist.linear.z = z*speed;
			twist.angular.x = 0; twist.angular.y = 0; twist.angular.z = th*turn #Only yaw angle
			pub.publish(twist)

	except:
		print e

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

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

# Essence 1 use a two-dimensional dictionary to define the motion information corresponding to the key (including motion direction and speed)
moveBindings = {
		'i':(1,0,0,0),
		'o':(1,0,0,-1),
		'j':(0,0,0,1),
		'l':(0,0,0,-1),
		'u':(1,0,0,1),
		',':(-1,0,0,0),
		'.':(-1,0,0,1),
		'm':(-1,0,0,-1),
		'O':(1,-1,0,0),
		'I':(1,0,0,0),
		'J':(0,1,0,0),
		'L':(0,-1,0,0),
		'U':(1,1,0,0),
		'<':(-1,0,0,0),
		'>':(-1,-1,0,0),
		'M':(-1,1,0,0),
		't':(0,0,1,0),
		'b':(0,0,-1,0),
	       }

speedBindings={
		'q':(1.1,1.1),
		'z':(.9,.9),
		'w':(1.1,1),
		'x':(.9,1),
		'e':(1,1.1),
		'c':(1,.9),
	      }
# Essence 2 python to get keyboard key information
def getKey():
	# tty.setraw(fd, when=termios.TCSAFLUSH) changes the mode of the file descriptor fd to raw.
	# If when is omitted, it defaults to termios.TCSAFLUSH and is passed to termios.tcsetattr()
	tty.setraw(sys.stdin.fileno())
	# The first parameter is to listen for a readable socket, the second is to listen for a writable socket, the third is to listen for an exception socket, and the fourth is to set the time limit
	# If the listening socket meets the read-write condition, the returned can_read or can_write will have values, and then you can use these returned values for subsequent operations
	select.select([sys.stdin], [], [], 0)
	key = sys.stdin.read(1)
	# int tcsetattr(int fd, int optional_actions, const struct termios *termios_p)
	# The tcsetattr function is used to set terminal parameters. The function returns 0 on success and - 1 on failure
	# Parameter fd is the open terminal file descriptor, and parameter optional_actions is used to control when the modification takes effect, while the structure termios_p saves the parameters to be modified
	# optional_actions can take the following values: tcsnow (change the attribute immediately before data transmission is completed), tcsadrain (change the attribute after all data transmission is completed),
	# TCSAFLUSH: wait until all data transmission is completed and the I / O buffer is cleared before changing the attribute
	termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
	return key

3. Navigation sending target point

Through the navigation interface of ros, move_base navigation system sends goal to let the robot navigate to the target point

Posted by nando on Mon, 22 Nov 2021 08:55:11 -0800