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())
