def move_circle(self, center, radius):
# move on all parts of the polygon
yaw_step = math.asin(1.0 / self.node_frequency * self.vel / radius)
yaw = 0.0
while not rospy.is_shutdown():
if yaw >= 2 * math.pi:
rospy.loginfo("starting new round")
yaw = 0.0
object_new_pose = Pose()
object_new_pose.position.x = center[0] + radius * math.sin(yaw)
object_new_pose.position.y = center[1] + radius * math.cos(yaw)
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, -yaw)
object_new_pose.orientation.x = quat[0]
object_new_pose.orientation.y = quat[1]
object_new_pose.orientation.z = quat[2]
object_new_pose.orientation.w = quat[3]
# spawn new model
model_state = ModelState()
model_state.model_name = self.name
model_state.pose = object_new_pose
model_state.reference_frame = 'world'
# publish message
self.pub.publish(model_state)
# call service
req = SetModelStateRequest()
req.model_state = model_state
#res = self.srv_set_model_state(req)
#if not res.success:
# print "something went wrong in service call"
yaw += yaw_step
self.rate.sleep()
def teleport(self, pose, robot_name):
model_state_req = SetModelStateRequest()
model_state_req.model_state = ModelState()
model_state_req.model_state.model_name = robot_name
model_state_req.model_state.pose = pose
model_state_req.model_state.twist.linear.x = 0.0
model_state_req.model_state.twist.linear.y = 0.0
model_state_req.model_state.twist.linear.z = 0.0
model_state_req.model_state.twist.angular.x = 0.0
model_state_req.model_state.twist.angular.y = 0.0
model_state_req.model_state.twist.angular.z = 0.0
model_state_req.model_state.reference_frame = 'world'
self.model_state_proxy(model_state_req)
def freezeBase(self, flag):
#toggle gravity
req_reset_gravity = SetPhysicsPropertiesRequest()
#ode config
req_reset_gravity.time_step = 0.001
req_reset_gravity.max_update_rate = 1000
req_reset_gravity.ode_config.sor_pgs_iters = 50
req_reset_gravity.ode_config.sor_pgs_w = 1.3
req_reset_gravity.ode_config.contact_surface_layer = 0.001
req_reset_gravity.ode_config.contact_max_correcting_vel = 100
req_reset_gravity.ode_config.erp = 0.2
req_reset_gravity.ode_config.max_contacts = 20
if (flag):
req_reset_gravity.gravity.z = 0.0
else:
req_reset_gravity.gravity.z = -9.81
self.reset_gravity(req_reset_gravity)
# create the message
req_reset_world = SetModelStateRequest()
#create model state
model_state = ModelState()
model_state.model_name = "hyq"
model_state.pose.position.x = 0.0
model_state.pose.position.y = 0.0
model_state.pose.position.z = 0.8
model_state.pose.orientation.w = 1.0
model_state.pose.orientation.x = 0.0
model_state.pose.orientation.y = 0.0
model_state.pose.orientation.z = 0.0
model_state.twist.linear.x = 0.0
model_state.twist.linear.y = 0.0
model_state.twist.linear.z = 0.0
model_state.twist.angular.x = 0.0
model_state.twist.angular.y = 0.0
model_state.twist.angular.z = 0.0
req_reset_world.model_state = model_state
# send request and get response (in this case none)
self.reset_world(req_reset_world)
def move_on_line(self, start, goal):
dx = goal[0] - start[0]
dy = goal[1] - start[1]
segment_length = math.sqrt(dx**2 + dy**2)
segment_time = segment_length / self.vel
yaw = math.atan2(dy, dx)
segment_step_count = int(segment_time * self.node_frequency)
if segment_step_count == 0:
return
segment_time = segment_length / self.vel / segment_step_count
for step in numpy.linspace(0, segment_length, segment_step_count):
step_x = start[0] + step * math.cos(yaw)
step_y = start[1] + step * math.sin(yaw)
object_new_pose = Pose()
object_new_pose.position.x = step_x
object_new_pose.position.y = step_y
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
object_new_pose.orientation.x = quat[0]
object_new_pose.orientation.y = quat[1]
object_new_pose.orientation.z = quat[2]
object_new_pose.orientation.w = quat[3]
# spawn new model
model_state = ModelState()
model_state.model_name = self.name
model_state.pose = object_new_pose
model_state.reference_frame = 'world'
# publish message
self.pub.publish(model_state)
# call service
req = SetModelStateRequest()
req.model_state = model_state
#res = self.srv_set_model_state(req)
#if not res.success:
# print "something went wrong in service call"
# sleep until next step
self.rate.sleep()
# 4.2.1c Change position of drone according to new selected starting position
pose=Pose()
pose.position.x, pose.position.y, starting_height, yaw = sample_new_position(starting_positions)
# pose.position.x, pose.position.y, starting_height, yaw=0,0,1,0
print("[run_script]: x: {0}, y: {1}, z: {2}, yaw:{3}".format(pose.position.x, pose.position.y, starting_height, yaw))
# some yaw to quaternion re-orientation code:
pose.orientation.z=np.sin(yaw)
pose.orientation.w=np.cos(yaw)
pose.position.z = 0.1
model_state = ModelState()
model_state.model_name = 'quadrotor' if FLAGS.robot.startswith('drone') else 'turtlebot3_burger'
model_state.pose=pose
state_request = SetModelStateRequest()
state_request.model_state = model_state
retvals = model_state_gazebo_service(state_request)
rospy.set_param('starting_height', starting_height)
print("Changed pose with return values: {0}".format(retvals))
time.sleep(1) #CHANGED
unpause_physics_client(EmptyRequest())
else:
# 4.2.2 Launch Gazebo again
# 4.2.2a Ensure previous Gazebo is not running anymore
if gazebo_popen!=None:
kill_popen('gazebo', gazebo_popen)
wait_for_gazebo()
prev_environment_arguments = new_environment_arguments
