move_obstacle.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy, math, random
4 from geometry_msgs.msg import Twist, Pose
5 from nav_msgs.msg import Odometry
6 
7 # Parameters
8 vel_max = 1.0
9 vel_min = 0.1
10 
11 # Define parameters, if parameter server provides no definitions
12 # Lower bound of the obstacle movement in y-direction
13 if rospy.has_param("pos_lb"):
14  pos_lb = rospy.get_param("pos_lb")
15 else:
16  pos_lb = 0.5
17 
18 # Upper bound of the obstacle movement in y-direction
19 if rospy.has_param("pos_ub"):
20  pos_ub = rospy.get_param("pos_ub")
21 else:
22  pos_ub = 5.5
23 
24 # Define global Twist message which is modified in the callback_base_pose_ground_truth and published in the main loop
25 Twist_msg = Twist()
26 
27 
28 # This function manages turnarounds and new (possibly random) velocities of the object
29 def callback_base_pose_ground_truth(base_pose_ground_truth):
30  # Define random velocity if no specific velocity is defined in the parameter server
31  if rospy.has_param("vel_y"):
32  vel_y = rospy.get_param("vel_y")
33  else:
34  vel_y = random.uniform(vel_min, vel_max)
35 
36  # Turn in y-direction
37  if base_pose_ground_truth.pose.pose.position.y >= pos_ub:
38  Twist_msg.linear.y = -vel_y
39  if base_pose_ground_truth.pose.pose.position.y <= pos_lb:
40  Twist_msg.linear.y = vel_y
41 
42 
43 # This function initializes the mover node and publishes continously a Twist message
45  rospy.init_node("Mover")
46  r = rospy.Rate(10)
47  pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
48  sub = rospy.Subscriber('base_pose_ground_truth', Odometry, callback_base_pose_ground_truth)
49  start_t = rospy.get_time()
50 
51  # initialize movement with random direction
52  if rospy.has_param("vel_y"):
53  vel_y = rospy.get_param("vel_y")
54  else:
55  vel_y = random.uniform(vel_min, vel_max)
56  Twist_msg.linear.y = float(random.choice(['-1', '1'])) * vel_y
57 
58  # publish movement command continuously
59  while not rospy.is_shutdown():
60  pub.publish(Twist_msg)
61  r.sleep()
62 
63 
64 if __name__ == '__main__':
65  try:
66  move_object()
67  except rospy.ROSInterruptException:
68  pass
def callback_base_pose_ground_truth(base_pose_ground_truth)


teb_local_planner_tutorials
Author(s): Christoph Rösmann
autogenerated on Thu Jun 6 2019 19:31:31