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