def model_state_based_rotate_by_angle(self, angle): # Stop the robot before rotating move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Track how far we have turned turn_angle = 0 goal_angle = rospy.get_param("~goal_angle", radians(angle)) # degrees converted to radians rotation = self.rotation # Track the last angle measured last_angle = rotation # Set the movement command to a rotation move_cmd.angular.z = self.angular_speed if goal_angle > 0 else -self.angular_speed # Begin the rotation while abs(turn_angle + self.angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) self.rate.sleep() # Get the current rotation rotation = self.rotation # Compute the amount of rotation since the last lopp delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation #Stop the robot when we are done move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) rospy.loginfo('Robot Turned %s. degrees.' %(math.degrees(normalize_angle(turn_angle))))
def old_rotate(self, angle): last_angle = self.rot turn_angle = 0 while abs(turn_angle + self.angular_tolerance) < abs( angle) and not rospy.is_shutdown(): self.rotate_inc() self.update_odom() delta_angle = normalize_angle(self.rot - last_angle) turn_angle += delta_angle last_angle = self.rot self.cmd_vel.publish(Twist())
def scan(self,Sframe_id,i,distance): #angular in rad from -pi~+pi #while not rospy.is_shutdown(): for n in range (0, int(i*10), int(pi/4*10)): print 'n',n angular=normalize_angle(n/10.0) print 'angular:',angular self.Sposition=[distance,0.0,0.0] self.Sorientation=[0.0,0.0,angular,1.0] print 'self.Sorientation:%s \n,self.Sposition:%s\n'%(self.Sorientation,self.Sposition) goalpose=self.params(Sframe_id,self.Sorientation,self.Sposition) print 'goalpose\n',goalpose argue=self.command(goalpose) self.action(argue)
def scan(self,Sframe_id): i=0.0 while not rospy.is_shutdown(): self.Sposition=[0.0,0.0,0.0] angular=normalize_angle(i) self.Sorientation=[0.0,0.0,angular,1.0] print 'self.Sorientation: ',self.Sorientation goalpose=self.params(Sframe_id,self.Sorientation,self.Sposition) print 'goalpose\n',goalpose argue=self.command(goalpose) self.action(argue) print '############\n i:',i print 'angular:',angular i+=pi/18 if 0>angular>-0.05: break
def scan(self, Sframe_id): i = 0.0 while not rospy.is_shutdown(): self.Sposition = [0.0, 0.0, 0.0] angular = normalize_angle(i) self.Sorientation = [0.0, 0.0, angular, 1.0] print 'self.Sorientation: ', self.Sorientation goalpose = self.params(Sframe_id, self.Sorientation, self.Sposition) print 'goalpose\n', goalpose argue = self.command(goalpose) self.action(argue) print '############\n i:', i print 'angular:', angular i += pi / 18 if 0 > angular > -0.05: break
def scan(self, Sframe_id, i, distance): #angular in rad from -pi~+pi #while not rospy.is_shutdown(): for n in range(0, int(i * 10), int(pi / 4 * 10)): print 'n', n angular = normalize_angle(n / 10.0) print 'angular:', angular self.Sposition = [distance, 0.0, 0.0] self.Sorientation = [0.0, 0.0, angular, 1.0] print 'self.Sorientation:%s \n,self.Sposition:%s\n' % ( self.Sorientation, self.Sposition) goalpose = self.params(Sframe_id, self.Sorientation, self.Sposition) print 'goalpose\n', goalpose argue = self.command(goalpose) self.action(argue)
def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to exectute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 30 r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) self.speed = rospy.get_param('~speed', 0.7) # radians per second self.tolerance = rospy.get_param('tolerance', radians(5)) # degrees converted to radians self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up dynamic_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): # Execute the rotation if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() last_angle = self.odom_angle turn_angle = 0 # Alternate directions between tests reverse = -reverse angular_speed = reverse * self.speed while abs(turn_angle) < abs(self.test_angle): if rospy.is_shutdown(): return move_cmd = Twist() move_cmd.angular.z = angular_speed self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() # Compute how far we have gone since the last measurement delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) # Add to our total angle so far turn_angle += delta_angle last_angle = self.odom_angle # Stop the robot self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('nav_square', anonymous=False) # Set rospy to exectute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the parameters for the target square goal_distance = rospy.get_param("~goal_distance", 1.0) # meters goal_angle = rospy.get_param("~goal_angle", radians(90)) # degrees converted to radians linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Cycle through the four sides of the square for i in range(4): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before rotating move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 # Begin the rotation while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last lopp delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Stop the robot when we are done self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) # Set rospy to exectute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.2 meters per second linear_speed = 0.2 # Set the travel distance in meters goal_distance = 1.0 # Set the rotation speed in radians per second angular_speed = 1.0 # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(2.5) # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # Initialize the tf listener self.tf_listener = tf.TransformListener() # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo( "Cannot find transform between /odom and /base_link or /base_footprint" ) rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Loop once for each leg of the trip for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt( pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 while abs(turn_angle + angular_tolerance) < abs( goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name # 节点名字 rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = rospy.get_param('~rate', 20) r = rospy.Rate(self.rate) # The test angle is 360 degrees # 设置角度校准的相关参数 self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) self.speed = rospy.get_param('~speed', 1.0) # radians per second self.tolerance = radians(rospy.get_param( 'tolerance', 1)) # degrees converted to radians self.odom_angular_scale_correction = rospy.get_param( '~odom_angular_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Publisher to control the robot's speed # 发布速度指令到cmd_vel话题 self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) # The base frame is usually base_link or base_footprint # 设置机器人坐标系的名字 self.base_frame = rospy.get_param('~base_frame', '/base_footprint') # The odom frame is usually just /odom # 设置里程计坐标系的名字 self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener # 初始化tf变换数据的收听功能 self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): # 如果测试可以开始 if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() last_angle = self.odom_angle turn_angle = 0 self.test_angle *= reverse error = self.test_angle - turn_angle # Alternate directions between tests reverse = -reverse # 只要测试处于开始状态并且角度差值大于容忍值 while abs(error) > self.tolerance and self.start_test: if rospy.is_shutdown(): return # Rotate the robot to reduce the error # 旋转机器人减少角度差值 move_cmd = Twist() move_cmd.angular.z = copysign(self.speed, error) self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf # 从tf变换中读取并设置当前的角度/姿态 self.odom_angle = self.get_odom_angle() # Compute how far we have gone since the last measurement # 计算从上一时刻到当前转过的角度 delta_angle = self.odom_angular_scale_correction * normalize_angle( self.odom_angle - last_angle) # Add to our total angle so far turn_angle += delta_angle # Compute the new error error = self.test_angle - turn_angle # Store the current angle for the next comparison last_angle = self.odom_angle # Stop the robot # 退出while循环后, 就停止机器人 self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist())
def __init__(self): # initiliaze rospy.init_node('out_and_back', anonymous=False) # tell user how to stop Robot rospy.loginfo("To stop Robot CTRL + C") # What function to call when you ctrl + c rospy.on_shutdown(self.shutdown) # Create a publisher which can "talk" to Robot and tell it to move # Tip: You may need to change /cmd_vel self.cmd_vel = rospy.Publisher('cmd_vel', Twist, queue_size=10) # self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) # How fast will we update the robot's movement? rate = 20 r = rospy.Rate(rate) # 20hz # 設定 out_and_back 參數 # Set the travel distance to 1.0 meters goal_distance = 1.0 # Set the forward linear speed to 0.15 meters per second linear_speed = 0.15 print("設定 前進距離為%5.2f meters,速度為%5.2f m/s" % (goal_distance, linear_speed)) # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # Set the rotation speed in radians per second angular_speed = 0.5 print("設定 旋轉角度為%5.2f radians,旋轉速度為%5.2f rad/s" % (goal_angle, angular_speed)) # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(1.0) # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Set the odom frame self.odom_frame = '/odom' # self.odom_frame = rospy.get_param('~odom_frame', '/odom') self.base_frame = '/base_link' # self.base_frame = '/base_footprint' # self.base_frame = rospy.get_param('~base_frame', '/base_footprint') # Initialize the position variable as a Point type position = Point() for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed (position, rotation) = self.get_odom() #print ("I get position =", position) #print ("I get rotation =", rotation) x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 print(" distance = %5.2f meters " % distance) # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt( pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) print(" distance = %5.2f meters " % distance) # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 print(" turn_angle = %5.2f degrees " % degrees(turn_angle)) while abs(turn_angle + angular_tolerance) < abs( goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation print(" turn_angle = %5.2f degrees " % degrees(turn_angle)) # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to exectute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 30 r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) self.speed = rospy.get_param('~speed', 0.7) # radians per second self.tolerance = rospy.get_param( 'tolerance', radians(5)) # degrees converted to radians self.odom_angular_scale_correction = rospy.get_param( '~odom_angular_scale_correction', 1.0) self.start_test = rospy.get_param('~start_test', True) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) # Fire up the dynamic_reconfigure server dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) # Connect to the dynamic_reconfigure server dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): # Execute the rotation if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() last_angle = self.odom_angle turn_angle = 0 # Alternate directions between tests reverse = -reverse angular_speed = reverse * self.speed while abs(turn_angle) < abs(self.test_angle): if rospy.is_shutdown(): return move_cmd = Twist() move_cmd.angular.z = angular_speed self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() # Compute how far we have gone since the last measurement delta_angle = self.odom_angular_scale_correction * normalize_angle( self.odom_angle - last_angle) # Add to our total angle so far turn_angle += delta_angle last_angle = self.odom_angle # Stop the robot self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist())
def __init__(self): rospy.init_node('move_to_direction', anonymous=False) rospy.on_shutdown(self.shutdown) self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) self.rate = rospy.get_param('~rate', 50) r = rospy.Rate(self.rate) #max speed linear_speed = 0.26 #linear_speed = 0 angular_speed = 2 angular_tolerance = radians(1.0) self.tf_listener = tf.TransformListener() rospy.sleep(0.5) self.odom_frame = '/odom' try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo( "Cannot find transform between /odom and /base_link or /base_footprint" ) rospy.signal_shutdown("tf Exception") az, el, time = locater.get() # str = raw_input("goal direction:") str = az print("goal direction is:", str) ## str = [-pi,pi] goal_angle = float(str) if goal_angle < 0: # goal_angle = pi*2 - goal_angle goal_angle = -goal_angle angular_speed = -angular_speed # these 4 sentences are commented by me position = Point() # angular_duration = goal_angle / angular_speed move_cmd = Twist() move_cmd.linear.x = linear_speed move_cmd.angular.z = angular_speed (position, rotation) = self.get_odom() last_angle = rotation turn_angle = 0 # this while-loop is commented by me while abs(turn_angle + angular_tolerance) < abs( goal_angle) and not rospy.is_shutdown(): self.cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = self.get_odom() delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation # ticks = int(goal_angle * self.rate) # for t in range(ticks): # self.cmd_vel.publish(move_cmd) # r.sleep() # angular_speed = 1.82 # these 4 sentences are commented by me move_cmd = Twist() move_cmd.linear.x = 0.26 self.cmd_vel.publish(move_cmd) rospy.sleep(0.5)
def set_cmd_vel(self, position): rospy.loginfo(" goal: [%5.2f, %5.2f]", position.x, position.y) goal_x = round(position.x, 4) goal_y = round(position.y, 4) # Initialize the robot_position variable as a Point type robot_position = Point() # Initialize the command move_cmd = Twist() rospy.loginfo(" now -> %s", ok) # Get the starting position values (robot_position, rotation) = self.get_odom() if (ok == True): # Set the movement command to forward motion move_cmd.linear.x = self.linear_speed distanceToGoal = sqrt( pow((goal_x - robot_position.x), 2) + pow((goal_y - robot_position.y), 2)) while (distanceToGoal > self.distance_tolerance): #and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Get the current position (robot_position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distanceToGoal = sqrt( pow((goal_x - robot_position.x), 2) + pow((goal_y - robot_position.x), 2)) rospy.loginfo( " distanceToGoal:[%5.2f], now -> robot:[%5.2f,%5.2f]", distanceToGoal, robot_position.x, robot_position.y) # Stop the robot before the rotation #Stopping our robot after the movement is over move_cmd.linear.x = 0 move_cmd.angular.z = 0 self.cmd_vel.publish(move_cmd) #rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = self.angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 goal_angle = atan2(goal_y - robot_position.y, goal_x - robot_position.x) while abs(turn_angle + self.angular_tolerance) < abs( goal_angle): #and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Get the current rotation (robot_position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('out_and_back', anonymous=False) # Set rospy to exectute a shutdown function when exiting rospy.on_shutdown(self.shutdown) # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist) # How fast will we update the robot's movement? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the forward linear speed to 0.2 meters per second linear_speed = 0.2 # Set the travel distance in meters goal_distance = 1.0 # Set the rotation speed in radians per second angular_speed = 1.0 # Set the angular tolerance in degrees converted to radians angular_tolerance = radians(2.5) # Set the rotation angle to Pi radians (180 degrees) goal_angle = pi # Initialize the tf listener self.tf_listener = tf.TransformListener() # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Loop once for each leg of the trip for i in range(2): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt(pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before the rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last loop delta_angle = normalize_angle(rotation - last_angle) # Add to the running total turn_angle += delta_angle last_angle = rotation # Stop the robot before the next leg move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1) # Stop the robot for good self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('nav_square', anonymous=False) # Set rospy to exectute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? rate = 20 # Set the equivalent ROS rate variable r = rospy.Rate(rate) # Set the parameters for the target square goal_distance = rospy.get_param("~goal_distance", 1.0) # meters goal_angle = rospy.get_param( "~goal_angle", radians(90)) # degrees converted to radians linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second angular_tolerance = rospy.get_param("~angular_tolerance", radians(2)) # degrees to radians # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('mybot/cmd_vel', Twist) # ajout eric la base de mybot est footprint # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Set the odom frame self.odom_frame = '/odom' # Find out if the robot uses /base_link or /base_footprint try: #self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) #self.base_frame = '/base_footprint' self.tf_listener.waitForTransform(self.odom_frame, '/footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo( "Cannot find transform between /odom and /base_link or /base_footprint" ) rospy.signal_shutdown("tf Exception") # Initialize the position variable as a Point type position = Point() # Cycle through the four sides of the square for i in range(4): # Initialize the movement command move_cmd = Twist() # Set the movement command to forward motion move_cmd.linear.x = linear_speed # Get the starting position values (position, rotation) = self.get_odom() x_start = position.x y_start = position.y # Keep track of the distance traveled distance = 0 # Enter the loop to move along a side while distance < goal_distance and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current position (position, rotation) = self.get_odom() # Compute the Euclidean distance from the start distance = sqrt( pow((position.x - x_start), 2) + pow((position.y - y_start), 2)) # Stop the robot before rotating move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Set the movement command to a rotation move_cmd.angular.z = angular_speed # Track the last angle measured last_angle = rotation # Track how far we have turned turn_angle = 0 # Begin the rotation while abs(turn_angle + angular_tolerance) < abs( goal_angle) and not rospy.is_shutdown(): # Publish the Twist message and sleep 1 cycle self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation (position, rotation) = self.get_odom() # Compute the amount of rotation since the last lopp delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation move_cmd = Twist() self.cmd_vel.publish(move_cmd) rospy.sleep(1.0) # Stop the robot when we are done self.cmd_vel.publish(Twist())
def __init__(self): # Give the node a name rospy.init_node('calibrate_angular', anonymous=False) # Set rospy to execute a shutdown function when terminating the script rospy.on_shutdown(self.shutdown) # How fast will we check the odometry values? self.rate = 10 r = rospy.Rate(self.rate) # The test angle is 360 degrees self.test_angle = 2 * pi self.speed = 0.8 # radians per second self.tolerance = 1 # degrees converted to radians self.odom_angular_scale_correction = 1 self.start_test = True # Publisher to control the robot's speed self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) # The base frame is usually base_link or base_footprint self.base_frame = rospy.get_param('~base_frame', '/base_link') # The odom frame is usually just /odom self.odom_frame = rospy.get_param('~odom_frame', '/odom') # Initialize the tf listener self.tf_listener = tf.TransformListener() # Give tf some time to fill its buffer rospy.sleep(2) # Make sure we see the odom and base frames self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) rospy.loginfo("Bring up rqt_reconfigure to control the test.") reverse = 1 while not rospy.is_shutdown(): if self.start_test: # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() rospy.loginfo("self.odom_angle: " + str(self.odom_angle)) last_angle = self.odom_angle turn_angle = 0 self.test_angle *= reverse error = self.test_angle - turn_angle rospy.loginfo("errir: " + str(error)) # Alternate directions between tests reverse = -reverse while abs(error) > self.tolerance and self.start_test: if rospy.is_shutdown(): return rospy.loginfo("*************************** ") # Rotate the robot to reduce the error move_cmd = Twist() move_cmd.angular.z = copysign(self.speed, error) rospy.loginfo("move_cmd.angular.z: " + str(move_cmd.angular.z)) self.cmd_vel.publish(move_cmd) r.sleep() # Get the current rotation angle from tf self.odom_angle = self.get_odom_angle() rospy.loginfo("current rotation angle: " + str(self.odom_angle)) # Compute how far we have gone since the last measurement delta_angle = self.odom_angular_scale_correction * normalize_angle( self.odom_angle - last_angle) rospy.loginfo("delta_angle: " + str(delta_angle)) # Add to our total angle so far turn_angle += abs(delta_angle) rospy.loginfo("turn_angle: " + str(turn_angle)) # Compute the new error error = self.test_angle - turn_angle rospy.loginfo("error: " + str(error)) # Store the current angle for the next comparison last_angle = self.odom_angle # Stop the robot self.cmd_vel.publish(Twist()) # Update the status flag self.start_test = False params = {'start_test': False} dyn_client.update_configuration(params) rospy.sleep(0.5) # Stop the robot self.cmd_vel.publish(Twist())
def __init__(self): rospy.init_node('Test', anonymous=True) #rospy.on_shutdown(self.shutdown) self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=10) rate = 20 r = rospy.Rate(rate) angular_speed = radians(90) angular_tolerance = radians(1.0) #goal_angle = radians(270) self.tf_listener = tf.TransformListener() rospy.sleep(2) self.odom_frame = '/odom' try: self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_footprint' except (tf.Exception, tf.ConnectivityException, tf.LookupException): try: self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) self.base_frame = '/base_link' except (tf.Exception, tf.ConnectivityException, tf.LookupException): rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") rospy.signal_shutdown("tf Exception") position = Point() move_cmd = Twist() move_cmd.linear.x = 0 #move_cmd.angular.z = angular_speed ser = serial.Serial( port = "/dev/ttyUSB0",baudrate = 115200,bytesize = serial.EIGHTBITS, parity = serial.PARITY_NONE,stopbits = serial.STOPBITS_ONE); #while 1: while not rospy.is_shutdown(): SERIAL = ser.readline() if SERIAL.find("angle") != -1: ANGLE = SERIAL goal_angle = float(filter(str.isdigit,ANGLE.encode('gbk'))) print goal_angle if goal_angle <= 180: move_cmd.angular.z = -angular_speed else: move_cmd.angular.z = angular_speed goal_angle = 360 - goal_angle (position, rotation) = self.get_odom() last_angle = rotation turn_angle = 0 while abs(turn_angle + angular_tolerance) < abs(radians(goal_angle)) and not rospy.is_shutdown(): #while abs(turn_angle + angular_tolerance) < abs(radians(goal_angle)): self.cmd_vel.publish(move_cmd) r.sleep() (position, rotation) = self.get_odom() delta_angle = normalize_angle(rotation - last_angle) turn_angle += delta_angle last_angle = rotation self.cmd_vel.publish(Twist())