def reset_pose(): set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) pose_robot = Pose() pose_robot.position.x = np.random.randint(1,5) / 10.0 pose_robot.position.y = np.random.randint(1,5) / 10.0 pose_robot.position.z = 0.12 alpha = 2 * math.pi * random.random() robot_quaternion = quaternion_from_euler(0, 0, alpha) #print("robot_quaternion: " + str(robot_quaternion)) pose_robot.orientation.x = robot_quaternion[0] pose_robot.orientation.y = robot_quaternion[1] pose_robot.orientation.z = robot_quaternion[2] pose_robot.orientation.w = robot_quaternion[3] state_model_robot = ModelState() state_model_robot.model_name = "robot1" state_model_robot.pose = pose_robot resp_robot = set_state(state_model_robot) pose_ball = Pose() pose_ball.position.x = -np.random.randint(10,15) / 10.0 pose_ball.position.y = np.random.randint(-10,10) / 10.0 pose_ball.position.z = 0.12 pose_ball.orientation.x = 0 pose_ball.orientation.y = 0 pose_ball.orientation.z = 0 pose_ball.orientation.w = 0 state_model_ball = ModelState() state_model_ball.model_name = "football" state_model_ball.pose = pose_ball resp_ball = set_state(state_model_ball)
def position_gazebo_models(self): modelstate = ModelState() # return object to previous if self._object_type != 0: modelstate.model_name = "object" + str(self._object_type) modelstate.reference_frame = "world" modelstate.pose = Pose(position=Point(x=-3.0, y=0.0, z=0.0)) req = self._obj_state(modelstate) # Randomise object type and orientation object_angle = random.uniform(0, math.pi) # orientation as quaternion object_q_z = math.sin(object_angle / 2) object_q_w = math.cos(object_angle / 2) # Position object_x = random.uniform(-0.1, 0.1) + 0.625 object_y = random.uniform(-0.1, 0.1) + 0.7975 # Type of object self._object_type = random.randint(1, 3) modelstate.model_name = "object" + str(self._object_type) # Place object for pick-up modelstate.reference_frame = "world" object_pose = Pose(position=Point(x=object_x, y=object_y, z=0.8), orientation=Quaternion(x=0.0, y=0.0, z=object_q_z, w=object_q_w)) modelstate.pose = object_pose req = self._obj_state(modelstate)
def moving_tile_y_short_path(self): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) model_original_pose_y = model.pose[0].position.y for i in range(len(model.name)): if model.name[i] == self.model_name and round( (model.pose[i].position.y), 1) <= round( self.y_model_pose, 1) + 1.0 and self.flag1 == 0: obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = 1.3 obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif model.name[i] == self.model_name and round( model.pose[i].position.y, 1) != round( self.y_model_pose, 1): self.flag1 = 1 obstacle.model_name = self.model_name obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.linear.y = -1.3 obstacle.twist.angular.z = 0 self.pub_model.publish(obstacle) elif round(model.pose[i].position.y, 1) == round(self.y_model_pose, 1): self.flag1 = 0
def reset_simulation(robot_x=0, robot_y=0, robot_angle=0, ball_x=1, ball_y=0): rospy.wait_for_service('/gazebo/reset_simulation') rospy.wait_for_service('/gazebo/set_model_state') # ServiceProxy and call means `rosservice call /gazebo/simulation_world` srv = rospy.ServiceProxy('/gazebo/reset_simulation', std_srvs.srv.Empty) srv.call() # set the robot model_pose = Pose() model_pose.position.x = robot_x model_pose.position.y = robot_y model_pose.orientation.z = np.sin(robot_angle / 2.0) model_pose.orientation.w = np.cos(robot_angle / 2.0) modelstate = ModelState() modelstate.model_name = 'mobile_base' modelstate.reference_frame = 'world' modelstate.pose = model_pose set_model_srv = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) set_model_srv.call(modelstate) # set the ball model_pose = Pose() model_pose.position.x = ball_x model_pose.position.y = ball_y modelstate = ModelState() modelstate.model_name = 'soccer_ball' modelstate.reference_frame = 'world' modelstate.pose = model_pose set_model_srv = rospy.ServiceProxy('gazebo/set_model_state', SetModelState) set_model_srv.call(modelstate) rospy.loginfo("reset simulation")
def main(): # Initiate node for allocating a model at a certain position rospy.init_node('set_model_position', anonymous=True) # Setup Services for pausing & resuming gazebo physics # and change model states mypause = rospy.ServiceProxy('/gazebo/pause_physics', Empty) myunpause = rospy.ServiceProxy('/gazebo/unpause_physics', Empty) set_pos = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) # Set pose of the model pose = Pose() pose.position.x = 0.0 pose.position.y = 2.0 pose.position.z = 1.65 pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 1.0 # Set robot ought to be moved state = ModelState() state.model_name = "ball" state.pose = pose state.twist.linear.x = 0.0 state.twist.linear.y = 0.0 state.twist.linear.z = 0.0 state.twist.angular.x = 0.0 state.twist.angular.y = 0.0 state.twist.angular.z = 0.0 state.reference_frame = 'world' rate = rospy.Rate(2) # 25hz i = 0 while not rospy.is_shutdown(): #Pause physics try: mypause() except Exception, e: rospy.logerr('Error on Calling Service: %s', str(e)) # Renew the position of the model pose.position.z = 1.65 + math.sin(i / 3.) pose.position.x = math.cos(i / 3.) state.pose = pose # Call the Service to publish position info #rospy.wait_for_service('/gazebo/set_model_state') try: return_msg = set_pos(state) # print return_msg.status_message except Exception, e: rospy.logerr('Error on Calling Service: %s', str(e))
def reset_world(self, x, y, theta): # rospy.wait_for_service('/ackermann_vehicle/gazebo/reset_world') # reset_world = rospy.ServiceProxy('/ackermann_vehicle/gazebo/reset_world', Empty) # resp = reset_world() try: rospy.wait_for_service('/ackermann_vehicle/gazebo/set_model_state') set_model_pose = rospy.ServiceProxy( '/ackermann_vehicle/gazebo/set_model_state', SetModelState) state = ModelState() pose = Pose() # Fixed Car 1 pose.position.x = 0.7 pose.position.y = -2 pose.position.z = 0.1 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1.0 state.model_name = 'ack_c1' state.pose = pose resp1 = set_model_pose(state) # Fixed Car 2 pose.position.x = -0.7 pose.position.y = -2 pose.position.z = 0.1 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = 0 pose.orientation.w = 1.0 state.model_name = 'ack_c2' state.pose = pose resp2 = set_model_pose(state) # Agent Car pose.position.x = x pose.position.y = y pose.position.z = 0.105 pose.orientation.x = 0 pose.orientation.y = 0 pose.orientation.z = np.sin(theta / 2.0) pose.orientation.w = np.cos(theta / 2.0) state.model_name = 'ackermann_vehicle' state.pose = pose resp3 = set_model_pose(state) except rospy.ServiceException, e: print "Service call failed: %s" % e
def reset(self): # # Resets the state of the environment and returns an initial observation. # rospy.wait_for_service('/gazebo/reset_world') # try: # #reset_proxy.call() # self.reset_proxy() # except (rospy.ServiceException) as e: # print ("/gazebo/reset_world service call failed") sim = rospy.get_param('/use_sim_time') if sim is True: rospy.loginfo( 'Waiting until simulated robot is prepared for the task...') # rospy.wait_for_message('/sim_robot_init', EmptyMsg) rospy.loginfo("Pausing physics") self.pause_physics_client() rospy.loginfo("Reset vehicle") # define initial pose for later use pose1 = Pose() pose1.position.x = 0.0 pose1.position.y = 0.0 pose1.position.z = -0.30 pose1.orientation.x = 0.0 pose1.orientation.y = 0.0 pose1.orientation.z = 0.0 pose1.orientation.w = 0.0 # set initial model state model_state1 = ModelState() model_state1.model_name = "catvehicle1" model_state1.pose = pose1 self.set_model_state_client(model_state1) pose2 = pose1 pose2.position.x = -20.0 pose2.position.z = -0.30 model_state2 = ModelState() model_state2.model_name = "catvehicle2" model_state2.pose = pose2 self.set_model_state_client(model_state2) msg = String() msg.data = "123" self.reset_sim_pub1.publish(msg) self.reset_sim_pub2.publish(msg) rospy.loginfo("Unpausing physics") self.unpause_physics_client()
def movingAt(self, random_bot=False, random_bot_rotate=False): #print("bot",random_bot,random_bot_rotate) #pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1) obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[ i] == 'dog': # the model name is defined in .xacro file obstacle.model_name = 'dog' obstacle.pose = model.pose[i] #print(self.goalTable[p][0]) if random_bot: obstacle.pose.position.x = float( random.randint(-10, 10) / 10.0) obstacle.pose.position.y = float( random.randint(-10, 10) / 10.0) if random_bot_rotate: obstacle.twist = Twist() obstacle.twist.angular.z = float( random.randint(0, 314) / 100.0) if random_bot or random_bot_rotate: self.pub_model.publish(obstacle) if random_bot and not random_bot_rotate: rospy.loginfo("Random bot location") elif random_bot or random_bot_rotate: rospy.loginfo("Random bot location & rotation")
def _reset(self, car_model_state): camera_model_pose = self._get_initial_camera_pose(car_model_state) camera_model_state = ModelState() camera_model_state.model_name = self.topic_name camera_model_state.pose = camera_model_pose self.last_camera_state = camera_model_state self.model_state_client(camera_model_state)
def setPoseDefault(self): self.list_cup_pos = rospy.get_param('table_params/list_cup_pos') cup_index = self.list_cup_pos[self.idx_current_pos] # rospy.loginfo('Current index: %s'%str(self.idx_current_pos)) # rospy.loginfo('Current cup position: %s'%str(pre_index)) rospy.set_param('table_params/cup_pos', cup_index) grid_size = rospy.get_param('table_params/grid_size') table_size = rospy.get_param('table_params/table_size') margin_size = rospy.get_param('table_params/margin_size') x, y, z = getXYZFromIJ(cup_index[0], cup_index[1], grid_size, table_size, margin_size) target_pose_cup = Pose() target_pose_cup.position.x = x target_pose_cup.position.y = y target_pose_cup.position.z = z mat_index = rospy.get_param('table_params/mat_pos') x, y, z = getXYZFromIJ(mat_index[0], mat_index[1], grid_size, table_size, margin_size) target_pose_mat = Pose() target_pose_mat.position.x = x target_pose_mat.position.y = y target_pose_mat.position.z = z model_info_mat = ModelState() model_info_mat.model_name = 'mat' model_info_mat.reference_frame = 'world' model_info_mat.pose = target_pose_mat self.pub.publish(model_info_mat) self.setPose(target_pose_cup)
def launch_torpedo(self, srv): ''' Find position of sub and launch the torpedo from there. TODO: - Test to make sure it always fires from the right spot in the right direction. (It seems to but I haven't tested from all rotations.) ''' sub_state = self.get_model(model_name='sub8') sub_pose = msg_helpers.pose_to_numpy(sub_state.pose) # Translate torpedo init velocity so that it first out of the front of the sub. muzzle_vel = np.array([10, 0, 0]) v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0), sub_pose[1]) launch_twist = Twist() launch_twist.linear.x = v[0] launch_twist.linear.y = v[1] launch_twist.linear.z = v[2] # This is offset so it fires approx at the torpedo launcher location. launch_pos = geometry_helpers.rotate_vect_by_quat(np.array([.4, -.15, -.3, 0]), sub_pose[1]) model_state = ModelState() model_state.model_name = 'torpedo' model_state.pose = msg_helpers.numpy_quat_pair_to_pose(sub_pose[0] + launch_pos, sub_pose[1]) model_state.twist = launch_twist self.set_torpedo(model_state) self.launched = True return True
def jumpToBall(): get_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) pose = Pose() state = ModelState() state.model_name = "pioneer3at" state.reference_frame = "map" rospy.wait_for_service("/gazebo/get_model_state") get_state = rospy.ServiceProxy("/gazebo/get_model_state", GetModelState) response = get_state("robocup_3d_goal", "") response_2 = get_state("soccer_ball", "") quaternion = ( response.pose.orientation.x, response.pose.orientation.y, response.pose.orientation.z, response.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] yaw = yaw - 3.6 quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw) pose.position.x = -2.8 pose.position.y = 8.2 pose.orientation.x = quaternion[0] pose.orientation.y = quaternion[1] pose.orientation.z = quaternion[2] pose.orientation.w = quaternion[3] state.pose = pose ret = set_state(state)
def _get_offtrack_car_reset_model_state(self, car_pose): cur_dist = self._data_dict_['current_progress'] * \ self._track_data_.get_track_length() / 100.0 closest_object_dist, closest_obstacle_pose = self._get_closest_obj( cur_dist, const.OBSTACLE_NAME_PREFIX) if closest_obstacle_pose is not None: # If static obstacle is in circumference of reset position, # put the car to opposite lane and 1m back. cur_dist = closest_object_dist - const.RESET_BEHIND_DIST cur_center_pose, inner_reset_pose, outer_reset_pose = self._get_reset_poses( dist=cur_dist) is_object_inner = self._is_obstacle_inner( obstacle_pose=closest_obstacle_pose) new_pose = outer_reset_pose if is_object_inner else inner_reset_pose else: cur_center_pose, inner_reset_pose, outer_reset_pose = self._get_reset_poses( dist=cur_dist) # If there is no obstacle interfering reset position, then # put the car back to closest lane from the off-track position. inner_distance = pose_distance(inner_reset_pose, car_pose) outer_distance = pose_distance(outer_reset_pose, car_pose) new_pose = inner_reset_pose if inner_distance < outer_distance else outer_reset_pose car_model_state = ModelState() car_model_state.model_name = self._agent_name_ car_model_state.pose = new_pose car_model_state.twist.linear.x = 0 car_model_state.twist.linear.y = 0 car_model_state.twist.linear.z = 0 car_model_state.twist.angular.x = 0 car_model_state.twist.angular.y = 0 car_model_state.twist.angular.z = 0 return car_model_state
def set_start(self,location): # rospy.loginfo("got to set_start") zero_twist = Twist() gazebo_state = ModelState() zero_twist.linear.x = 0.0 zero_twist.linear.x = 0.0 zero_twist.linear.z = 0.0 zero_twist.angular.x = 0.0 zero_twist.angular.y = 0.0 zero_twist.angular.z = 0.0 gazebo_state.model_name = self.robot gazebo_state.pose = self._gazebo_pose(location) gazebo_state.twist = zero_twist gazebo_state.reference_frame = self.g_frame amcl_initial_pose = PoseWithCovarianceStamped() amcl_initial_pose.pose = self._amcl_pose(location) amcl_initial_pose.header.frame_id = self.a_frame amcl_initial_pose.header.stamp = rospy.Time.now() self.set_gazebo_state(gazebo_state) rospy.sleep(1) attempts = 0 while attempts < 10: self.publisher.publish(amcl_initial_pose) rospy.sleep(1) if self.localized(amcl_initial_pose.pose.pose): return 0 return 1
def init_car(self, x, y, theta): # init function is for value iteration # since car model for value iteration is 3D, so we only initialize x, y, theta car_pose = Pose() car_pose.position.x = x car_pose.position.y = y # car_pose.position.z = 0.1 car_quat_x, car_quat_y, car_quat_z, car_quat_w = quaternion_from_euler(0, 0, theta) car_pose.orientation.x = car_quat_x car_pose.orientation.y = car_quat_y car_pose.orientation.z = car_quat_z car_pose.orientation.w = car_quat_w car_twist = Twist() car_twist.linear.x = 0 car_twist.linear.y = 0 car_twist.linear.z = 0 car_state = ModelState() car_state.model_name = 'mobile_base' car_state.pose = car_pose car_state.twist= car_twist rospy.ServiceProxy('gazebo/set_model_state', SetModelState)(car_state) return True
def setModelState(self, currState, targetState): control = self.rearWheelFeedback(currState, targetState) a = Float32MultiArray() a.data = [control.speed,control.steering_angle] self.controlPub.publish(a) values = self.rearWheelModel(control) newState = ModelState() newState.model_name = 'gem' newState.pose = currState.pose newState.pose.position.x = values[0] newState.pose.position.y = values[1] newState.pose.position.z = 0.006 q = self.euler_to_quaternion([values[2],0,0]) newState.pose.orientation.x = q[0] newState.pose.orientation.y = q[1] newState.pose.orientation.z = q[2] newState.pose.orientation.w = q[3] newState.twist.linear.x = 0 newState.twist.linear.y = 0 newState.twist.linear.z = 0 newState.twist.angular.x = 0 newState.twist.angular.y = 0 newState.twist.angular.z = 0 self.modelStatePub.publish(newState)
def move_sphere(self, coords_list): model_state_msg = ModelState() # check msg structure with: rosmsg info gazebo_msgs/ModelState # It is composed of 4 sub-messages: # model_name of type string # pose of type geometry_msgs/Pose # twist of type geometry_msgs/Twist # reference_frame of type string pose_msg = Pose() # rosmsg info geometry_msgs/Pose # It is composed of 2 sub-messages # position of type geometry_msgs/Point # orientation of type geometry_msgs/Quaternion point_msg = Point() # rosmsg info geometry_msgs/Point # It is composed of 3 sub-messages # x of type float64 # y of type float64 # z of type float64 point_msg.x = coords_list[0] point_msg.y = coords_list[1] point_msg.z = coords_list[2] pose_msg.position = point_msg model_state_msg.model_name = "my_sphere3" model_state_msg.pose = pose_msg model_state_msg.reference_frame = "world" # print(model_state_msg) self.pub.publish(model_state_msg)
def run_shortly_while_freezing_drone(): # make drone's velocity zero but keep it at the same position get_model_state_service = rospy.ServiceProxy('/gazebo/get_model_state', GetModelState) old_model_state = get_model_state_service.call( model_name=rospy.get_param('/robot/model_name')) set_model_state_service = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() model_state.pose = old_model_state.pose model_state.model_name = rospy.get_param('/robot/model_name') set_model_state_service.wait_for_service() set_model_state_service.call(model_state) # set fsm to overtake state so control mapper won't publish speed args = shlex.split("rostopic pub /fsm/overtake std_msgs/Empty '{}'") overtake_p = subprocess.Popen(args) # publish speed zero args = shlex.split("rostopic pub /cmd_vel geometry_msgs/Twist '{}'") speed_p = subprocess.Popen(args) environment._clear_experience_values() environment._pause_period = 1 / 100. while environment.observation is None: set_model_state_service.call(model_state) environment._run_shortly() overtake_p.terminate() speed_p.terminate()
def place_randomly(self): ''' Move station aligning and facing robot exactly ''' MAX_DYNAMIC_POS = 0.7 self.is_up_or_bottom = True if random.random() > 0.5 else False bottom_or_right = 1.0 if random.random() > 0.5 else -1.0 top_or_left = 1.0 if random.random() > 0.5 else -1.0 x = STATION_POS * bottom_or_right y = (random.random() % MAX_DYNAMIC_POS) * top_or_left if not self.is_up_or_bottom: x, y = y, x model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[i] == self.station_name: mark = ModelState() mark.model_name = model.name[i] mark.pose = model.pose[i] mark.pose.position.x, mark.pose.position.y = x, y self.station_position.position.x = mark.pose.position.x self.station_position.position.y = mark.pose.position.y mark.pose.orientation = self.__station_orientation__() self.pub_model.publish(mark) time.sleep(0.02) break
def place_station_in_front_randomly(self): ''' Move station aligning and facing robot exactly ''' MAX_DYNAMIC_POS = 0.7 x = STATION_POS top_or_left = 0.0 #1.0 if random.random() > 0.5 else -1.0 y = (random.random() % MAX_DYNAMIC_POS) * top_or_left model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[i] == self.station_name: mark = ModelState() mark.model_name = model.name[i] mark.pose = model.pose[i] mark.pose.position.x, mark.pose.position.y = x, y self.station_position.position.x = mark.pose.position.x self.station_position.position.y = mark.pose.position.y #mark.pose.orientation = self.__station_orientation__() self.pub_model.publish(mark) time.sleep(0.05) break
def launch_torpedo(self, srv): ''' Find position of sub and launch the torpedo from there. TODO: - Test to make sure it always fires from the right spot in the right direction. (It seems to but I haven't tested from all rotations.) ''' sub_state = self.get_model(model_name='sub8') sub_pose = msg_helpers.pose_to_numpy(sub_state.pose) # Translate torpedo init velocity so that it first out of the front of the sub. muzzle_vel = np.array([10, 0, 0]) v = geometry_helpers.rotate_vect_by_quat(np.append(muzzle_vel, 0), sub_pose[1]) launch_twist = Twist() launch_twist.linear.x = v[0] launch_twist.linear.y = v[1] launch_twist.linear.z = v[2] # This is offset so it fires approx at the torpedo launcher location. launch_pos = geometry_helpers.rotate_vect_by_quat( np.array([.4, -.15, -.3, 0]), sub_pose[1]) model_state = ModelState() model_state.model_name = 'torpedo' model_state.pose = msg_helpers.numpy_quat_pair_to_pose( sub_pose[0] + launch_pos, sub_pose[1]) model_state.twist = launch_twist self.set_torpedo(model_state) self.launched = True return True
def set_model_pose(self, pose, twist=None, model='sub8'): ''' Set the position of 'model' to 'pose'. It may be helpful to kill the sub before moving it. TODO: - Deprecate kill stuff ''' set_state = yield self.nh.get_service_client('/gazebo/set_model_state', SetModelState) if twist is None: twist = numpy_to_twist([0, 0, 0], [0, 0, 0]) model_state = ModelState() model_state.model_name = model model_state.pose = pose model_state.twist = twist if model == 'sub8': # TODO: Deprecate kill stuff (Zach's PR upcoming) kill = self.nh.get_service_client('/set_kill', SetKill) yield kill(SetKillRequest(kill=Kill(id='initial', active=False))) yield kill(SetKillRequest(kill=Kill(active=True))) yield self.nh.sleep(.1) yield set_state(SetModelStateRequest(model_state)) yield self.nh.sleep(.1) yield kill(SetKillRequest(kill=Kill(active=False))) else: set_state(SetModelStateRequest(model_state))
def detection_to_state(self, detection): wamv_state_msg = ModelState() wamv_state_msg.model_name = self.wamv_model_name wamv_state_msg.pose = detection.pose.pose.pose #print "original pose: ", wamv_state_msg.pose trans_mat = tf.transformations.compose_matrix(None, None, self.tf_rotation, self.tf_translation, None) #print "transformation matrix: ", trans_mat pos = wamv_state_msg.pose.position quan = wamv_state_msg.pose.orientation original_position = [pos.z, -1 * pos.x, 0.1] original_rotation = tf.transformations.euler_from_quaternion([quan.x, quan.y, quan.z, quan.w]) original_rotation = [original_rotation[0], original_rotation[2], -1 * original_rotation[1]] original_mat = tf.transformations.compose_matrix(None, None, original_rotation, original_position, None) #print "original pose matrix: ", original_mat if(self.verbose): print "original original_rotation: ", original_position transformed_mat = np.dot(original_mat, trans_mat) #print "transformed pose matrix: ", transformed_mat [ scale, shear, angles, trans, persp] = tf.transformations.decompose_matrix(transformed_mat) angles = tf.transformations.quaternion_from_euler(angles[0], angles[1], angles[2]) wamv_state_msg.pose.position.x = trans[0] wamv_state_msg.pose.position.y = trans[1] wamv_state_msg.pose.position.z = trans[2] wamv_state_msg.pose.orientation.x = angles[0] wamv_state_msg.pose.orientation.y = angles[1] wamv_state_msg.pose.orientation.z = angles[2] wamv_state_msg.pose.orientation.w = angles[3] self.cb_path(wamv_state_msg.pose) self.pub_wamv_state.publish(wamv_state_msg)
def random_apples(self): """Put apples in random places.""" new_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) i = 0 model_state = ModelState() for i in range(0, 9): apple_name = 'cricket_ball_' + str(i) rospy.loginfo('Random %s', apple_name) model_state.model_name = apple_name twist = Twist() twist = self._set_twist(twist) model_state.twist = twist pose = Pose() pose.position.x = random.uniform(-1.8, 1.8) pose.position.y = random.uniform(-1.8, 1.8) pose.position.z = 0.0 pose.orientation.x = 0.0 pose.orientation.y = 0.0 pose.orientation.z = 0.0 pose.orientation.w = 0.0 model_state.pose = pose model_state.reference_frame = 'world' new_model_state(model_state)
def move_box_model(box_model): model_state = ModelState() pose_state = Pose() twist_state = Twist() pose_state.position = box_model.object_pose.pose.position pose_state.orientation = box_model.object_pose.pose.orientation twist_state.linear.x = 0.0 twist_state.linear.y = 0.0 twist_state.linear.z = 0.0 twist_state.angular.x = 0.0 twist_state.angular.y = 0.0 twist_state.angular.z = 0.0 model_state.pose = pose_state model_state.twist = twist_state model_state.model_name = box_model.object_name model_state.reference_frame = box_model.object_pose.header.frame_id rospy.wait_for_service('/gazebo/set_model_state') try: set_model_srv = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp1 = set_model_srv(model_state) print resp1.status_message return resp1.success except rospy.ServiceException, e: print "Service call failed: %s" % e
def move_robot_model(pose): model_state = ModelState() pose_state = Pose() twist_state = Twist() pose_state = pose twist_state.linear.x = 0.0 twist_state.linear.y = 0.0 twist_state.linear.z = 0.0 twist_state.angular.x = 0.0 twist_state.angular.y = 0.0 twist_state.angular.z = 0.0 model_state.pose = pose_state model_state.twist = twist_state model_state.model_name = "tiago_steel" model_state.reference_frame = "world" rospy.wait_for_service('/gazebo/set_model_state') try: set_model_srv = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) resp1 = set_model_srv(model_state) print resp1.status_message return resp1.success except rospy.ServiceException, e: print "Service call failed: %s" % e
def moving(self): t = 0 while not rospy.is_shutdown(): obstacle = ModelState() model = rospy.wait_for_message('gazebo/model_states', ModelStates) for i in range(len(model.name)): if model.name[i] == 'obstacle': obstacle.model_name = 'obstacle' obstacle.pose = model.pose[i] obstacle.twist = Twist() obstacle.twist.angular.z = 0.2 # # move back and forth if int(t / 50) % 2 == 0: # omega = -np.pi / 120 # r = 30 # theta = np.pi + omega * t # r_vec = np.array([r*np.cos(theta), r*np.sin(theta), 0]) # w_vec = np.array([0, 0, omega]) # v = np.cross(w_vec, r_vec) # print("r", r_vec) # print("w", w_vec) # print("v", v) # obstacle.twist.linear.x = v[0] # obstacle.twist.linear.y = v[1] obstacle.twist.linear.y = -0.5 elif int(t / 50) % 2 == 1: obstacle.twist.linear.y = 0.5 if t < 30: obstacle.twist.linear.y = 0 obstacle.twist.linear.x = 0 self.pub_model.publish(obstacle) t += 1 time.sleep(0.1)
def move_models(self, names, positions, isBot=False): rospy.wait_for_service("/gazebo/set_model_state") g_set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState) for i in xrange(0, len(positions)): state = ModelState() pose = Pose() pose.position.x = positions[i][0] - self.map_offset pose.position.y = positions[i][1] - self.map_offset if isBot: pose.position.z = 0.2 else: pose.position.z = 0 quats = tf.transformations.quaternion_from_euler(0, 0, 0) pose.orientation.x = quats[0] pose.orientation.y = quats[1] pose.orientation.z = quats[2] pose.orientation.w = quats[3] state.model_name = names[i] state.pose = pose try: ret = g_set_state(state) print names[i], ret.status_message # self.r.sleep() except Exception, e: rospy.logerr('Error on calling service: %s', str(e))
def make_model_state_msg(model_name=None, pose=None, scale=None, twist=None, reference_frame=None): """ ModelState messages factory """ msg = ModelState() msg.model_name = model_name if (model_name is not None) else 'model_name' if pose is None: pose = Pose() pose.position = Point(0, 0, 0) pose.orientation = Quaternion(0, 0, 0, 1) msg.pose = pose msg.scale = scale if (scale is not None) else Vector3(1, 1, 1) msg.twist = twist if (twist is not None) else Twist( Vector3(0, 0, 0), Vector3(0, 0, 0)) if reference_frame is None or reference_frame == '': msg.reference_frame = 'world' else: msg.reference_frame = reference_frame return msg
def _get_car_reset_model_state(self, start_dist): '''get avaliable car reset model state when reset is allowed Args: start_dist (float): start distance Returns: ModelState: start state ''' closest_object_dist, _ = self._get_closest_obj(start_dist) # Check to place behind car if needed if closest_object_dist is not None: start_dist = closest_object_dist - const.RESET_BEHIND_DIST # Compute the start pose start_pose = self._track_data_._center_line_.interpolate_pose(start_dist, reverse_dir=self._reverse_dir_, finite_difference=FiniteDifference.FORWARD_DIFFERENCE) start_model_state = ModelState() start_model_state.model_name = self._agent_name_ start_model_state.pose = start_pose start_model_state.twist.linear.x = 0 start_model_state.twist.linear.y = 0 start_model_state.twist.linear.z = 0 start_model_state.twist.angular.x = 0 start_model_state.twist.angular.y = 0 start_model_state.twist.angular.z = 0 return start_model_state
def create_modelstate_message(coords, yaw, model_name='/'): """ Create a model state message for husky robot """ pose = Pose() p = Point() p.x = coords[0] p.y = coords[1] p.z = coords[2] pose.position = p qua = quaternion_from_euler(0, 0, yaw) q = Quaternion() q.x = qua[0] q.y = qua[1] q.z = qua[2] q.w = qua[3] pose.orientation = q twist = Twist() twist.linear = Vector3(0, 0, 0) twist.angular = Vector3(0, 0, 0) ms = ModelState() ms.model_name = model_name ms.pose = pose ms.twist = twist ms.reference_frame = 'sand_mine' return ms
def publish(self): gaz = ModelState() gaz.model_name = self.model_name gaz.pose = self.pose gaz.twist = self.vel self.pub.publish(gaz) rospy.loginfo(gaz) self.pose, self.vel = None, None
def _cb_linkdata(self, msg): for pos, item in enumerate(msg.name): twist = msg.twist[pos] pose = msg.pose[pos] self.model_twists[item] = twist self.model_poses[item] = pose ms = ModelState() ms.pose = pose ms.twist = twist ms.model_name = item self.model_states[item] = ms
def reset_simulation(): global current_model_x, current_model_y, current_model_z, has_fallen # Reset the joints new_angles = {} new_angles['j_ankle1_l'] = 0.0 new_angles['j_ankle1_r'] = 0.0 new_angles['j_ankle2_l'] = 0.0 new_angles['j_ankle2_r'] = 0.0 new_angles['j_gripper_l'] = 0.0 new_angles['j_gripper_r'] = 0.0 new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body new_angles['j_low_arm_l'] = 0.0 new_angles['j_low_arm_r'] = 0.0 new_angles['j_pan'] = 0.0 new_angles['j_pelvis_l'] = 0.0 new_angles['j_pelvis_r'] = 0.0 new_angles['j_shoulder_l'] = 0.0 new_angles['j_shoulder_r'] = 0.0 new_angles['j_thigh1_l'] = 0.0 new_angles['j_thigh1_r'] = 0.0 new_angles['j_thigh2_l'] = 0.0 new_angles['j_thigh2_r'] = 0.0 new_angles['j_tibia_l'] = 0.0 new_angles['j_tibia_r'] = 0.0 new_angles['j_tilt'] = 0.0 new_angles['j_wrist_l'] = 0.0 new_angles['j_wrist_r'] = 0.0 # Default delay of setting angles is 2 seconds darwin.set_angles_slow(new_angles) # Reset the robot position # Send model to x=0,y=0 pose = Pose() pose.position.z = 0.31 twist = Twist() md_state = ModelState() md_state.model_name = model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = 'world' # Service call to reset model try: response = set_model_state(md_state) except rospy.ServiceException, e: print "Darwin model state initialization service call failed: %s" % e
def reset_simulation(self): # Reset the joints new_angles = {} new_angles['j_ankle1_l'] = 0.0 new_angles['j_ankle1_r'] = 0.0 new_angles['j_ankle2_l'] = 0.0 new_angles['j_ankle2_r'] = 0.0 new_angles['j_gripper_l'] = 0.0 new_angles['j_gripper_r'] = 0.0 new_angles['j_high_arm_l'] = 1.50 # Arm by the side of the body new_angles['j_high_arm_r'] = 1.50 # Arm by the side of the body new_angles['j_low_arm_l'] = 0.0 new_angles['j_low_arm_r'] = 0.0 new_angles['j_pan'] = 0.0 new_angles['j_pelvis_l'] = 0.0 new_angles['j_pelvis_r'] = 0.0 new_angles['j_shoulder_l'] = 0.0 new_angles['j_shoulder_r'] = 0.0 new_angles['j_thigh1_l'] = 0.0 new_angles['j_thigh1_r'] = 0.0 new_angles['j_thigh2_l'] = 0.0 new_angles['j_thigh2_r'] = 0.0 new_angles['j_tibia_l'] = 0.0 new_angles['j_tibia_r'] = 0.0 new_angles['j_tilt'] = -0.262 # Tilt the head forward a little new_angles['j_wrist_l'] = 0.0 new_angles['j_wrist_r'] = 0.0 # Default delay of setting angles is 2 seconds self.darwin.set_angles_slow(new_angles) rospy.sleep(5) # Reset the robot position # Send model to x=0,y=0 pose = Pose() pose.position.z = 0.31 twist = Twist() md_state = ModelState() md_state.model_name = self.model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = self.relative_entity_name # Service call to reset model try: response = self.set_model_state(md_state) except rospy.ServiceException, e: self.log_write("Darwin model state initialization service call failed: " + str(e))
def handle_position_change(self,pose): model_state = ModelState() model_state.model_name = "teresa_robot" model_state.pose = pose.pose model_state.reference_frame="map" for i in range(25): self.gazebo_pub.publish(model_state) rospy.sleep(0.03) for i in range(25): amcl_pose = PoseWithCovarianceStamped() amcl_pose.pose.pose = pose.pose amcl_pose.header.frame_id = "map" amcl_pose.header.stamp = rospy.get_rostime() self.amcl_pub.publish(amcl_pose) rospy.sleep(0.03)
def _cb_modeldata(self, msg): self.data_to_publish = {} for pos, item in enumerate(msg.name): twist = msg.twist[pos] pose = msg.pose[pos] self.model_twists[item] = twist self.model_poses[item] = pose ms = ModelState() ms.pose = pose ms.twist = twist ms.model_name = item self.model_states[item] = ms op_str = item + ", pose" val_str = str(ms.pose) self.data_to_publish[op_str] = str(val_str) if item != self.name: continue self.pose = msg.pose[pos]
def reset_simulation(self): # Empty the self.model_polled_z list self.model_polled_z = [] # Send model to x=0,y=0 model_name = 'darwin' pose = Pose() pose.position.z = 0.31 twist = Twist() reference_frame = 'world' md_state = ModelState() md_state.model_name = model_name md_state.pose = pose md_state.twist = twist md_state.reference_frame = reference_frame # Service call to reset model try: response = self.set_model_state(md_state) except rospy.ServiceException, e: print "Service call failed: %s"%e
def on_set_to_position_clicked_(self): success = True set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) model_state = ModelState() model_state.model_name = str(self._widget.comboBoxObjectList.currentText()).split('::')[0] model_state.pose = Pose() model_state.twist = Twist() model_state.reference_frame = "world" model_state.pose.position.x = float(str(self._widget.lineEdit_4.text())) model_state.pose.position.y = float(str(self._widget.lineEdit_5.text())) model_state.pose.position.z = float(str(self._widget.lineEdit_6.text())) try: resp1 = set_model_state(model_state) except rospy.ServiceException: success = False if success: if not resp1.success: success = False if not success: QMessageBox.warning(self._widget, "Warning", "Could not set model state.")
print msg status = (status + 1) % 15 elif key == 'k': x = 0 th = 0 control_speed = 0 control_turn = 0 elif key == ' ': # move human up stairs old_state = get_state_client.call("human_movable", "world") new_state = ModelState() new_state.model_name = "human_movable" new_state.reference_frame = "world" new_state.pose = old_state.pose new_state.twist = old_state.twist new_state.pose.position.x -= 0.5 new_state.pose.position.z += 0.15 set_state_client.call(new_state) else: count += 1 if count > 4: x = 0 th = 0 if key == '\x03': break target_speed = speed * x target_turn = turn * th
pose = Pose() pose.position.x = rospy.get_param('~position_x') pose.position.y = rospy.get_param('~position_y') pose.position.z = rospy.get_param('~position_z') pose.orientation.x = rospy.get_param('~orientation_x') pose.orientation.y = rospy.get_param('~orientation_y') pose.orientation.z = rospy.get_param('~orientation_z') pose.orientation.w = rospy.get_param('~orientation_w') state = ModelState() state.model_name = "robot" state.pose = pose rospy.loginfo("Moving robot") try: ret = g_set_state(state) print ret.status_message except Exception, e: rospy.logerr('Error on calling service: %s',str(e))