def animate():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=3)
rospy.init_node('animate_ball')
rate = rospy.Rate(20) # 15 Hz
x1 = 0.3
y1 = 0.1
z = 0.12
ballState1 = ModelState()
ballState1.model_name = 'ball'
ballState1.pose.orientation.x = 0
ballState1.pose.orientation.y = 0
ballState1.pose.orientation.z = 0
ballState1.pose.orientation.w = 1
ballState1.reference_frame = 'world'
direction = 'left'
x2 = 0.3
y2 = 0.0
ballState2 = ModelState()
ballState2.model_name = 'yellow ball'
ballState2.pose.orientation.x = 0
ballState2.pose.orientation.y = 0
ballState2.pose.orientation.z = 0
ballState2.pose.orientation.w = 1
ballState2.reference_frame = 'world'
x3 = 0.3
y3 = -0.1
ballState3 = ModelState()
ballState3.model_name = 'red ball'
ballState3.pose.orientation.x = 0
ballState3.pose.orientation.y = 0
ballState3.pose.orientation.z = 0
ballState3.pose.orientation.w = 1
ballState3.reference_frame = 'world'
angle = 0
while not rospy.is_shutdown():
ballState1.pose.position.x = x1 + angle
ballState1.pose.position.y = y1 + angle / 2
ballState1.pose.position.z = z
ballState2.pose.position.x = x2 + angle
ballState2.pose.position.y = y2
ballState2.pose.position.z = z
ballState3.pose.position.x = x3 + angle
ballState3.pose.position.y = y3 - angle / 2
ballState3.pose.position.z = z
angle += 0.005
pub.publish(ballState1)
pub.publish(ballState2)
pub.publish(ballState3)
rate.sleep()
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 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 setRandomModelState(self, name):
if name == 'goal':
idx = np.random.randint(len(self.goalList) - 1)
goalPos = self.goalList[idx]
state = ModelState()
state.model_name = name
state.reference_frame = 'world'
state.pose.position.x = goalPos[0]
state.pose.position.y = goalPos[1]
state.pose.position.z = 2.001
elif name == 'navibot':
idx = np.random.randint(len(self.spawnList) - 1)
spawnPos = self.spawnList[idx]
state = ModelState()
state.model_name = name
state.reference_frame = 'world'
state.pose.position.x = spawnPos[0]
state.pose.position.y = spawnPos[1]
state.pose.position.z = 0.5
state.pose.orientation.z = np.random.random() * 2 - 1
state.pose.orientation.w = np.random.random() * 2 - 1
self.set_model_state_client(state)
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 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 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 _reset(self):
Set_model = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
State = ModelState()
State.model_name = "summitXl"
State.pose.position.x = 5 * np.random.random()
State.pose.position.y = 5 * np.random.random() - 10
State.pose.position.z = 0
State.pose.orientation.z = 3.14 * np.random.randn()
State.reference_frame = "world"
Set_done = Set_model(State)
print(Set_done)
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except (rospy.ServiceException) as e:
print("/gazebo/unpause_physics service call failed")
data = rospy.wait_for_message('/scan', LaserScan, timeout=5)
time = rospy.wait_for_message('/clock', Clock, timeout=5)
rospy.wait_for_service('/gazebo/pause_physics')
try:
self.pause()
except (rospy.ServiceException) as e:
pass
state, self.done, reward = self._deal_scan_data(data)
time = time.clock.secs
return state, time
def set_start(self, x, y, theta):
state = ModelState()
state.model_name = 'robot'
state.reference_frame = 'world' # ''ground_plane'
# pose
state.pose.position.x = x
state.pose.position.y = y
state.pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(0, 0, theta)
state.pose.orientation.x = quaternion[0]
state.pose.orientation.y = quaternion[1]
state.pose.orientation.z = quaternion[2]
state.pose.orientation.w = quaternion[3]
# twist
state.twist.linear.x = 0
state.twist.linear.y = 0
state.twist.linear.z = 0
state.twist.angular.x = 0
state.twist.angular.y = 0
state.twist.angular.z = 0
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = self.set_state
result = set_state(state)
assert result.success is True
except rospy.ServiceException:
print("/gazebo/get_model_state service call failed")
def actor_poses_callback(actors):
global agents_list
global xml_string
current_agents = []
for actor in actors.agent_states:
current_agents.append(actor.id)
actor_id = str(actor.id)
if actor.id not in agents_list: # spawn unless spawned before
req = SpawnModelRequest()
req.model_name = 'actor_' + actor_id
req.model_xml = xml_string
req.initial_pose = actor.pose
req.reference_frame = "world"
req.robot_namespace = 'actor_' + actor_id
# rospy.logerr("SPAWNING: {}".format(actor_id))
spawn_model(req)
else: # just update the pose if spawned before
state = ModelState()
state.model_name = 'actor_' + actor_id
state.pose = actor.pose
state.reference_frame = "world"
try:
# rospy.logwarn("{}: set_state service call".format(actor_id))
set_state(state)
except rospy.ServiceException, e:
rospy.logerr("set_state failed: %s" % e)
def _reset(self):
#cv2.destroyAllWindows()
# Resets the state of the environment and returns an initial observation.
# rospy.wait_for_service('/gazebo/reset_simulation')
# try:
# #reset_proxy.call()
# self.reset_proxy()
# except rospy.ServiceException, e:
# print ("/gazebo/reset_simulation service call failed")
rospy.wait_for_service('gazebo/set_model_state')
state = ModelState()
state.model_name = self.name_model
state.reference_frame = "world"
state.pose = self.turtlebot_state.pose
state.twist = self.turtlebot_state.twist
self.set_model(state)
self.hint_state = np.zeros((len(self.hint_pos[self.num_target])))
# self.num_target = np.random.randint(3)
# self.num_hint = len(self.hint_pos[self.num_target])
# self.setTarget()
# Unpause simulation to make observation
rospy.wait_for_service('/gazebo/unpause_physics')
try:
self.unpause()
except rospy.ServiceException, e:
print("/gazebo/unpause_physics service call failed")
def animate():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
rospy.init_node('animate_ball')
rate = rospy.Rate(20) # 15 Hz
x = 0.9
y = 0.0
z = 1.2
ballState = ModelState()
ballState.model_name = 'ball'
ballState.pose.orientation.x = 0
ballState.pose.orientation.y = 0
ballState.pose.orientation.z = 0
ballState.pose.orientation.w = 1
ballState.reference_frame = 'world'
direction = 'left'
step = 0.08
radius = 0.1
angle = 0
while not rospy.is_shutdown():
ballState.pose.position.x = x
ballState.pose.position.y = y + radius * cos(angle)
ballState.pose.position.z = z + radius * sin(angle)
angle = angle + step % pi
pub.publish(ballState)
rate.sleep()
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 reset_pose(self):
'''
Stop the robot before reset its pose,
It's difficult to reset the velocities
of all the links fo the robot.
'''
stop = Twist()
time = rospy.get_time()
while rospy.get_time()-time<2:
for i in xrange(self.n_worker):
self.velcmd_pubs[i].publish(
stop
)
self.stop_rate.sleep()
for i in xrange(self.n_worker):
modelstate = ModelState()
modelstate.model_name = "rosbot"+str(i+self.rtoffset)
modelstate.reference_frame = "world"
x,y,z,w = rostf.transformations.quaternion_from_euler(
self.joint_initpose[i][0][3],
self.joint_initpose[i][0][4],
self.joint_initpose[i][0][5]
)
modelstate.pose.position.x = self.joint_initpose[i][0][0]
modelstate.pose.position.y = self.joint_initpose[i][0][1]
modelstate.pose.orientation.x = x
modelstate.pose.orientation.y = y
modelstate.pose.orientation.z = z
modelstate.pose.orientation.w = w
reps = self.reset_client(modelstate)
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_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 reset_node_init():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
new_obstacle_state = ModelState()
new_obstacle_state.model_name = "robot"+str(ac_num)
# cc_x = 0.0 # coordinate correction
# cc_y = 0.0 # 使用其它的坐标方案
if ac_num == 1:
new_obstacle_state.pose.position.x = 1.0
new_obstacle_state.pose.position.y = 5.0
else:
gym.logger.warn("Error ac_num !")
new_obstacle_state.pose.position.z = 0
# 注意４元数的概念
new_obstacle_state.pose.orientation.x = 0 # z与w 控制了水平朝向？
new_obstacle_state.pose.orientation.y = 0
new_obstacle_state.pose.orientation.z = 0
new_obstacle_state.pose.orientation.w = 0
new_obstacle_state.twist.linear.x = 0
new_obstacle_state.twist.linear.y = 0
new_obstacle_state.twist.linear.z = 0
new_obstacle_state.twist.angular.x = 0
new_obstacle_state.twist.angular.y = 0
new_obstacle_state.twist.angular.z = 0
new_obstacle_state.reference_frame = "world"
return pub, new_obstacle_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 _set_model_pose(self,
model_name,
model_pose,
model_velocity=[0.0] * 6,
reference_frame='world'):
"""
Set model pose attribute in Gazebo
Arguments:
model_name {str} -- model name of object in the scene
model_pose {list} -- mode pose list, first three number for position, last three number for orientation
model_velocity {list} -- mode velocity list, first three number for linear, last three for angular
"""
model_state_msg = ModelState()
model_state_msg.model_name = model_name
model_state_msg.pose.position.x = model_pose[0]
model_state_msg.pose.position.y = model_pose[1]
model_state_msg.pose.position.z = model_pose[2]
model_state_msg.pose.orientation.x = model_pose[3]
model_state_msg.pose.orientation.y = model_pose[4]
model_state_msg.pose.orientation.z = model_pose[5]
model_state_msg.twist.linear.x = model_velocity[0]
model_state_msg.twist.linear.y = model_velocity[1]
model_state_msg.twist.linear.z = model_velocity[2]
model_state_msg.twist.angular.x = model_velocity[3]
model_state_msg.twist.angular.y = model_velocity[4]
model_state_msg.twist.angular.z = model_velocity[5]
model_state_msg.reference_frame = reference_frame
self.set_model_state(model_state_msg)
def setStationary(self, model_name):
# Where is the model right now?
(x, y, z) = self.whereAmI(model_name)
print 'NOW:', x, y, z
# How long do we want this model to stay still?
duration = random.randint(3, 5) # 3, 4, or 5 seconds
# At what time should we stop executing this command?
nextTime = rospy.Time.now() + rospy.Duration(duration)
# Initialize an empty 'ModelState' message.
# We have to send this type of message to the service to get the model to move.
cmd = ModelState()
# Add some details to the message:
cmd.model_name = model_name
cmd.reference_frame = 'world'
cmd.pose.position.x = min(max(x, MIN_X), MAX_X)
cmd.pose.position.y = min(max(y, MIN_Y), MAX_Y)
cmd.pose.position.z = min(max(z, MIN_Z), MAX_Z)
# NOTE: The linear accel. has been set to zero.
# print cmd
return (cmd, nextTime)
def my_set_target(self):
state = ModelState()
state.model_name = self.name_target
state.reference_frame = "world"
state.pose.position.x = self.target_pos[self.num_target][0]
state.pose.position.y = self.target_pos[self.num_target][1]
self.set_model(state)
def thymio_state_service_request(self, position, orientation):
"""Request the service (set thymio state values) exposed by
the simulated thymio. A teleportation tool, by default in gazebo world frame.
Be aware, this does not mean a reset (e.g. odometry values)."""
rospy.wait_for_service('/gazebo/set_model_state')
try:
model_state = ModelState()
model_state.model_name = self.thymio_name
model_state.reference_frame = '' # the frame for the pose information
model_state.pose.position.x = position[0]
model_state.pose.position.y = position[1]
model_state.pose.position.z = position[2]
qto = quaternion_from_euler(orientation[0],
orientation[0],
orientation[0],
axes='sxyz')
model_state.pose.orientation.x = qto[0]
model_state.pose.orientation.y = qto[1]
model_state.pose.orientation.z = qto[2]
model_state.pose.orientation.w = qto[3]
# a Twist can also be set but not recomended to do it in a service
gms = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
response = gms(model_state)
return response
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def reset_simulation(self):
self.action_pub.publish(0)
rospy.wait_for_service('/gazebo/set_model_state')
set_model_state = rospy.ServiceProxy("/gazebo/set_model_state",
SetModelState)
model_state = ModelState()
model_state.model_name = 'mobile_base'
model_state.pose.position.x = 0
model_state.pose.position.y = 0
model_state.pose.position.z = 0
model_state.pose.orientation.x = 0
model_state.pose.orientation.y = 0
model_state.pose.orientation.z = 0.3 * (random.random() - 0.5)
model_state.pose.orientation.w = 1
model_state.twist.linear.x = 0
model_state.twist.linear.y = 0
model_state.twist.linear.z = 0
model_state.twist.angular.x = 0
model_state.twist.angular.y = 0
model_state.twist.angular.z = 0
model_state.reference_frame = 'world'
try:
set_model_state(model_state)
except rospy.ServiceException as exc:
print("Service did not process request: " + str(exc))
def beam_object(self, x, y, roll, pitch, yaw):
'''
beams an available object to the desired position
:param x: x-Position in [m]
:param y: y-Position in [m]
:param roll: Euler angle transformation
:param pitch: Euler angle transformation
:param yaw: Euler angle transformation
:return: --
'''
# gazebo takes ModelState as msg type
model_state = ModelState()
model_state.model_name = self.name
model_state.reference_frame = 'world'
model_state.pose.position.x = x
model_state.pose.position.y = y
quaternion = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
model_state.pose.orientation.x = quaternion[0]
model_state.pose.orientation.y = quaternion[1]
model_state.pose.orientation.z = quaternion[2]
model_state.pose.orientation.w = quaternion[3]
rospy.loginfo('\033[92m' + '----Move Object Gazebo----' + '\033[0m')
# publish message
self.pub.publish(model_state)
def moveInGazeboH(x, y, z, color):
#calls gazebo service to place red pieces
#receives position to be placed
global green_model_name_h, yellow_model_name_h, red_model_name_h
state_msg = ModelState()
if color == "green":
state_msg.model_name = green_model_name_h.pop(0)
elif color == "yellow":
state_msg.model_name = yellow_model_name_h.pop(0)
elif color == "red":
state_msg.model_name = red_model_name_h.pop(0)
state_msg.reference_frame = 'world'
state_msg.pose.position.x = x
state_msg.pose.position.y = y
state_msg.pose.position.z = z
#q = quaternion_from_euler(0, -pi, 0)
#state_msg.pose.orientation = Quaternion(x = q[0], y = q[1], z = q[2], w = q[3])
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = set_state(state_msg)
assert resp.success is True
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def mission_gen():
global amcl_init_pose
ArucoSetPose = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
x_range = [-1.75, -0.25]
y_range = [[-11.4, -10.6], [-9.4, -8.6], [-7.4, -6.6]]
i = random.randint(0, 2)
x = random.uniform(x_range[0] + 0.1, x_range[1] - 0.1)
y = random.uniform(y_range[i][0] + 0.1, y_range[i][1] - 0.1)
print '----------------------------'
print 'New Cube position : [%s, %s]' % (str(x), str(y))
print '----------------------------'
aruco_object = ModelState()
aruco_object.model_name = 'aruco_cube'
aruco_object.pose.position.x = x
aruco_object.pose.position.y = y
aruco_object.pose.position.z = 1.001
aruco_object.reference_frame = 'world'
ArucoSetPose(aruco_object)
dist = map(
abs,
[x_range[0] - x, y_range[i][0] - y, x_range[1] - x, y_range[i][1] - y])
print 'Distance from table edge : %s' % str(min(dist))
print 'init_pose = %s' % str(amcl_init_pose)
xr = amcl_init_pose[0] - 1
yr = amcl_init_pose[1] - 1
yawr = dist.index(min(dist)) * pi / 2
rospy.loginfo(
"---------------------\nRobot pose goal (x, y, Y) generated : (" +
str(xr) + ", " + str(yr) + ", " + str(yawr) + ")\n")
return (xr, yr, yawr)
def reset_node_init():
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1)
new_obstacle_state = ModelState()
new_obstacle_state.model_name = "robot_description"
new_obstacle_state.pose.position.x = 0
new_obstacle_state.pose.position.y = 0
new_obstacle_state.pose.position.z = 0
new_obstacle_state.pose.orientation.x = 0
new_obstacle_state.pose.orientation.y = 0
new_obstacle_state.pose.orientation.z = 0
new_obstacle_state.pose.orientation.w = 0
new_obstacle_state.twist.linear.x = 0
new_obstacle_state.twist.linear.y = 0
new_obstacle_state.twist.linear.z = 0
new_obstacle_state.twist.angular.x = 0
new_obstacle_state.twist.angular.y = 0
new_obstacle_state.twist.angular.z = 0
new_obstacle_state.reference_frame = "world"
return pub, new_obstacle_state
def reset_obstacle_pose_srv(self):
new_obstacle_state = ModelState()
new_obstacle_state.pose.orientation.x = 0
new_obstacle_state.pose.orientation.y = 0
new_obstacle_state.pose.orientation.z = 0
new_obstacle_state.pose.orientation.w = 0
new_obstacle_state.twist.linear.x = 0
new_obstacle_state.twist.linear.y = 0
new_obstacle_state.twist.linear.z = 0
new_obstacle_state.twist.angular.x = 0
new_obstacle_state.twist.angular.y = 0
new_obstacle_state.twist.angular.z = 0
new_obstacle_state.reference_frame = "world"
for i in range(0, 15): # 10个方块
new_obstacle_state.model_name = "unit_box_" + str(i)
new_obstacle_state.pose.position.x = self.obstacle_pose[i][0]
new_obstacle_state.pose.position.y = self.obstacle_pose[i][1]
new_obstacle_state.pose.position.z = 0
self.set_obstacle_srv(new_obstacle_state)
for i in range(0, 5): # 5个圆柱体,不使用球体,容易到处乱滚
new_obstacle_state.model_name = "unit_cylinder_" + str(i)
new_obstacle_state.pose.position.x = self.obstacle_pose[i + 15][0]
new_obstacle_state.pose.position.y = self.obstacle_pose[i + 15][1]
new_obstacle_state.pose.position.z = 0
self.set_obstacle_srv(new_obstacle_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 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 set_random_robot_pose(self):
"""
sets the robot pose to a random pose within the box
"""
msg = ModelState()
msg.model_name = 'robot'
msg.reference_frame = 'world'
msg.scale.x = msg.scale.y = msg.scale.z = 1.0
initial_pose = np.array([np.random.uniform(-3, 3), np.random.uniform(-2.4, 2.4), 0.18,
np.random.uniform(0, np.pi)])
# random position within box
msg.pose.position.x = initial_pose[0]
msg.pose.position.y = initial_pose[1]
msg.pose.position.z = initial_pose[2]
# random orientation
quaternion = quaternion_from_euler(0, 0, initial_pose[3])
msg.pose.orientation.x = quaternion[0]
msg.pose.orientation.y = quaternion[1]
msg.pose.orientation.z = quaternion[2]
msg.pose.orientation.w = quaternion[3]
# publish message on ros topic
self._set_model_state.publish(msg)
return initial_pose
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 spawnMBase(x=DEF_X, y = DEF_Y):
mState = ModelState()
mState.model_name = BOT
pos = Point()
pos.x = x
pos.y = y
pos.z = 0.0
ori = Quaternion()
ori.x = 0.0
ori.y = 0.0
ori.z = 0.0
ori.w = 1.0
mState.pose.position = pos
mState.pose.orientation = ori
mState.twist = twist_no_vel()
mState.reference_frame = 'world'
stop()
status = serviceThis(SET_MODEL_STATE, SetModelState, mState)
mbState = mState
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.")
'''EXPLICIT LOCATIONS USED'''
RED = [0.701716, 0.400784, 0.83095]
BLUE = [0.701716, 0.000784, 0.83095]
GREEN = [0.701716, -0.400784, 0.83095]
TARGET_GREEN = [0.6, -0.405, 0.82]
ALIGN_GRASP_GREEN = [0.7, -0.405, 0.82]
RAISED_GREEN = [0.7, -0.405, 1.0]
RAISED_DEST_GREEN = [0.7, 0.0, 1.0]
DEST_GREEN = [0.7, 0.0, 0.83]
DEST_GREEN_REMOVED = [0.45, 0.0, 1.0]
_cylinder_name = "unit_object_Green"
_cylinder_model_state = ModelState()
_cylinder_model_state.model_name = _cylinder_name
_cylinder_model_state.reference_frame = ''
'''OUTPUT FILE TO WRITE TO'''
FILENAME = '/home/simon/experiment'
def delete_cylinder():
rospy.wait_for_service("/gazebo/delete_model")
try:
s = rospy.ServiceProxy("gazebo/delete_model", DeleteModel)
resp = s(_cylinder_name)
except rospy.ServiceException, e:
print "error when calling delete_cylinder: %s"%e
sys.exit(1)
def reset_cylinder():
rospy.wait_for_service("/gazebo/set_model_state")
print vels(speed, turn)
if status == 14:
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
def simulate_anomaly():
"""
Spawn and move the fire
"""
# {id: [ initial_pos, speed]}
anomalies = {'fogo4': [[-0.50, 0.30], [0.0051, 0.0]],
'fogo5': [[-0.4, -0.30], [0.0051, 0.0]],
'fogo6': [[-0.65, 0.2], [0.0051, 0.0]],
'fogo7': [[0.5, 0], [0.0051, 0.0]],
'fogo8': [[0, 0.4], [0.0051, 0.0]],
'fogo9': [[0, -0.35], [0.0051, 0.0]],
'fogo1': [[0.40, 0.40], [0.0051, 0.0]],
'fogo2': [[-2.30, 0.20], [0.0051, 0.0]],
}
# {id: initial_position, target, initial time}
anomalies = {'fogo4': [[-0.50, 0.30], [5.00, 0.0]],
'fogo5': [[-0.4, -1.60], [5.00, 0.0]],
'fogo6': [[-1.0, -2.02], [5.00, 0.0]],
'fogo7': [[1.0, -0.30], [5.00, 0.0]],
'fogo8': [[0.0, 0.4], [5.00, 0.0]],
'fogo9': [[-1.30, -0.90], [5.00, 0.0]],
'fogo1': [[0.80, -1.60], [5.00, 0.0]],
'fogo2': [[-0.8, 0.0], [5.00, 0.0]],
}
anomalies = {'fogo4': [[-0.50, 0.30], [.00, 0.0], 0],
'fogo5': [[-0.4, -1.60], [.00, 0.0], 0],
'fogo6': [[-1.0, -2.02], [.00, 0.0], 0],
'fogo7': [[1.0, -0.30], [.00, 0.0], 0],
'fogo8': [[0.0, 0.4], [.00, 0.0], 0],
'fogo9': [[-1.30, -0.90], [.00, 0.0], 0],
'fogo1': [[0.80, -1.60], [.00, 0.0], 0],
'fogo2': [[-0.8, 0.0], [.00, 0.0], 0],
}
anomalies = {'fogo4': [[-4.0, 3.0], [-4.0, 3.0], 40],
'fogo5': [[1.4, 1.80], [1.4, 1.80], 100],
'fogo6': [[-2.60, -1.2], [-2.60, -1.2], 150],
# 'fogo7': [[3.30, -2.40], [3.30, -2.40], 200]
}
anomalies = {'fogo4': [[-.0, .0], [-.0, .0], 0],
}
existent_anomalies = Set()
expand = rospy.get_param('~expand', False)
if expand:
for k, a in anomalies.items():
anomalies[k][0], anomalies[k][1] = anomalies[k][1], anomalies[k][0]
communicator = Communicator('Robot1')
# ## SPAWN
# for anom, data in anomalies.items():
# spawn_fire(anom, data[0][0], data[0][1])
# pass
# ### MOVE
service_name = '/gazebo/set_model_state'
rospy.init_node('anomaly_simulator')
rospy.wait_for_service(service_name)
client_ms = rospy.ServiceProxy(service_name, SetModelState)
rospy.sleep(0.1)
r = rospy.Rate(2) # 10hz
#
# # moving_x = 0
log = []
while not rospy.is_shutdown():
for anom, data in anomalies.items():
if data[2] > rospy.get_rostime().secs:
continue
else:
if not anom in existent_anomalies:
print "spawn fire ", anom
spawn_fire(anom, data[0][0], data[0][1])
existent_anomalies.add(anom)
x, y = data[0]
target_x, target_y = data[1]
angle = math.atan2(target_y - y, target_x - x)
v = 0.002
data[0][0] += v * math.cos(angle)
data[0][1] += v * math.sin(angle)
# Model state
ms = ModelState()
ms.model_name = anom
ms.pose.position.x, ms.pose.position.y = data[0]
ms.reference_frame = 'world'
# Set new model state
set_ms = SetModelState()
set_ms.model_state = ms
client_ms.call(ms)
rospy.sleep(0.05)
time = rospy.get_rostime().secs + rospy.get_rostime().nsecs / 1000000000.0
orobots, sensed_points, anomaly_polygons = communicator.read_inbox()
log.append((time, anomaly_polygons, copy.deepcopy(anomalies), sensed_points))
with open(LOG_FILE, 'wb') as output:
pickle.dump(log, output, pickle.HIGHEST_PROTOCOL)
r.sleep()
def publish_gazebo_model_state(self):
for i in range(0, len(self.vect_gazebo)):
try:
position_to_pub = ModelState()
# is the mark visible? if not we reach exception
# (trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
# self.vect_gazebo[i][1], MAP, rospy.Time(0))
# Where is the desired object
(trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
MAP, self.vect_gazebo[i][0], rospy.Time(0))
# create the special message for gazebo
position_to_pub.reference_frame = "world"
position_to_pub.pose.position.x = trans_to_pub[0]
position_to_pub.pose.position.y = trans_to_pub[1]
# TODO : this is to be sure that the table is on the ground
# but it can not be generalized because the chair origin
# is not on the ground, we have to decide wether to fix
# the origin of all objects gazebo on the ground or pass
# the Z in a parameter or just the marker info
if self.vect_gazebo[i][1] == "table":
quat_to_send = rot_to_pub
position_to_pub.pose.position.z = 0
elif i == 0:
euler = euler_from_quaternion(rot_to_pub)
quat_to_send = quaternion_from_euler(
math.pi - euler[2], 0, 0)
position_to_pub.pose.position.z = trans_to_pub[2]
else:
quat_to_send = rot_to_pub
position_to_pub.pose.position.z = trans_to_pub[2]
position_to_pub.pose.orientation.x = quat_to_send[0]
position_to_pub.pose.orientation.y = quat_to_send[1]
position_to_pub.pose.orientation.z = quat_to_send[2]
position_to_pub.pose.orientation.w = quat_to_send[3]
if i == 0:
position_to_pub.model_name = "/virtual_pepper"
self.pub.publish(position_to_pub)
(trans_to_pub, rot_to_pub) = self.listener.lookupTransform(
MAP, "/base_link", rospy.Time(0))
# self.broadcaster.sendTransform(
# (0, 0, 0), (0, 0, 0, 1), rospy.Time.now(),
# "/map", "/world")
self.broadcaster.sendTransform(
trans_to_pub, rot_to_pub, rospy.Time.now(),
"/virtual_pepper/base_link", MAP)
else:
position_to_pub.model_name = self.vect_gazebo[i][0]
self.pub_obj.publish(position_to_pub)
except Exception, exc:
print"publish gazebo", exc
