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