def getDataSendContainer(data_topic): if data_topic == 'cmd_vel': return Twist() elif data_topic == 'husky_sim_speed': model_state = ModelState() model_state.model_name = 'husky' return model_state
def turtlebot3_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
self.x = 0
self.y = 0
# Put the turtlebot waffle at (0, 0)
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = 0
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'turtlebot3'
modelState.pose.position.x = self.x
modelState.pose.position.y = self.y
self.gazebo_model_state_service(modelState)
self.burger_x = 3.5
self.burger_y = random.uniform(-1, 1)
# Put the turtlebot burger at (2, 0)
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = random.uniform(0, 3)
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'turtlebot3_burger'
modelState.pose.position.x = self.burger_x
modelState.pose.position.y = self.burger_y
self.gazebo_model_state_service(modelState)
self.last_distance_of_turtlebot = sys.maxsize
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
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 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 cbEncoder(self, encoder_msg): delta_R = (encoder_msg.x - self.previousR) * self.d_PerCount delta_L= (encoder_msg.y - self.previousL) * self.d_PerCount delta_S = (delta_R + delta_L)/2 # print "encoder_msg.x = ",encoder_msg.x," encoder_msg.y = ",encoder_msg.y #print "previousR = "******" previousL = ",self.previousL self.th = ((delta_L - delta_R)/self.Length + self.th) self.x = delta_S * math.cos(self.th) + self.x self.y = delta_S * math.sin(self.th) + self.y model_state_msg = ModelState() model_state_msg.model_name = 'duckiebot_with_gripper' model_state_msg.pose.position.x = self.x model_state_msg.pose.position.y = self.y model_state_msg.pose.position.z = 0 new_quaternion = tf.transformations.quaternion_from_euler(0, 0, self.th) model_state_msg.pose.orientation.x = new_quaternion[0] model_state_msg.pose.orientation.y = new_quaternion[1] model_state_msg.pose.orientation.z = new_quaternion[2] model_state_msg.pose.orientation.w = new_quaternion[3] self.pub_gazebo.publish(model_state_msg) self.previousR = encoder_msg.x self.previousL = encoder_msg.y
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 racecar_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = 0
modelState.pose.orientation.w = 0 # Use this to randomize the orientation of the car
modelState.twist.linear.x = 0
modelState.twist.linear.y = 0
modelState.twist.linear.z = 0
modelState.twist.angular.x = 0
modelState.twist.angular.y = 0
modelState.twist.angular.z = 0
modelState.model_name = 'racecar'
if self.world_name.startswith(MEDIUM_TRACK_WORLD):
modelState.pose.position.x = -1.40
modelState.pose.position.y = 2.13
elif self.world_name.startswith(EASY_TRACK_WORLD):
modelState.pose.position.x = -1.44
modelState.pose.position.y = -0.06
elif self.world_name.startswith(HARD_TRACK_WORLD):
modelState.pose.position.x = 1.75
modelState.pose.position.y = 0.6
def toQuaternion(pitch, roll, yaw):
cy = np.cos(yaw * 0.5)
sy = np.sin(yaw * 0.5)
cr = np.cos(roll * 0.5)
sr = np.sin(roll * 0.5)
cp = np.cos(pitch * 0.5)
sp = np.sin(pitch * 0.5)
w = cy * cr * cp + sy * sr * sp
x = cy * sr * cp - sy * cr * sp
y = cy * cr * sp + sy * sr * cp
z = sy * cr * cp - cy * sr * sp
return [x, y, z, w]
#clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=np.pi)
#anti-clockwise
quaternion = toQuaternion(roll=0.0, pitch=0.0, yaw=0.0)
modelState.pose.orientation.x = quaternion[0]
modelState.pose.orientation.y = quaternion[1]
modelState.pose.orientation.z = quaternion[2]
modelState.pose.orientation.w = quaternion[3]
else:
raise ValueError("Unknown simulation world: {}".format(self.world_name))
self.racecar_service(modelState)
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
self.progress_at_beginning_of_race = self.progress
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 set_state(self, x, y, yaw):
set_quaternion = tf.transformations.quaternion_from_euler(0., 0., yaw)
next_state = ModelState()
next_state.model_name = 'pmb2'
next_state.pose.position.x = x
next_state.pose.position.y = y
next_state.pose.position.z = 0.001
next_state.pose.orientation.x = set_quaternion[0]
next_state.pose.orientation.y = set_quaternion[1]
next_state.pose.orientation.z = set_quaternion[2]
next_state.pose.orientation.w = set_quaternion[3]
resp_set = self.set_coordinates(next_state)
def talker(path):
rospy.init_node('talker', anonymous=True)
pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
rate = rospy.Rate(10) # 10hz
temp = path
x = ModelState()
x.model_name = "piano2"
x.pose = Pose()
x.pose.orientation.y = 0
x.pose.orientation.w = 1.0
x.pose.orientation.x = 0
x.pose.orientation.z = 0
while not rospy.is_shutdown():
#hello_str = "hello world %s" % rospy.get_time()
x.twist = Twist()
x.twist.linear.y = 0
x.twist.angular.y = 0
for i in range(0, len(temp)):
x.pose.position.x = temp[i][0]
x.pose.position.y = temp[i][1]
x.pose.position.z = temp[i][2]
#angle_to_turn = atan2 (temp[i+1][1]-temp[i][1], temp[i+1][0]-temp[i+1][0])*math.pi/180
x.pose.orientation.z = random.uniform(0, 1)
rot_q = x.pose.orientation
(roll, pitch, theta) = euler_from_quaternion(
[rot_q.x, rot_q.y, rot_q.z, rot_q.w])
if (i < len(temp) - 1):
angle_to_goal = atan2(temp[i + 1][1] - temp[i][1],
temp[i + 1][0] - temp[i + 1][0])
if abs(angle_to_goal - theta) > 0.1:
x.twist.linear.x = 0.0
x.twist.angular.z = 0.0
else:
x.twist.linear.x = 0.5
x.twist.angular.z = 0.0
#x.twist = Twist()
#x.twist.linear.x = randint(-60, 60)*math.pi/180
rospy.loginfo(x)
pub.publish(x)
time.sleep(0.3)
# print "one loop done"
exit()
def odomCallback (odomMsg):
state_msg = ModelState()
state_msg.model_name = 'turtlebot3_waffle'
state_msg.pose = odomMsg.pose.pose
#state_msg.twist = odomMsg.twist.twist
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state( state_msg )
except (rospy.ServiceException, e):
print ("Service call failed: %s" % e)
def _reset_obstacles(self):
for obstacle_name, obstacle_pose in zip(self.obstacle_names, self.obstacle_poses):
obstacle_state = ModelState()
obstacle_state.model_name = obstacle_name
obstacle_state.pose = obstacle_pose
obstacle_state.twist.linear.x = 0
obstacle_state.twist.linear.y = 0
obstacle_state.twist.linear.z = 0
obstacle_state.twist.angular.x = 0
obstacle_state.twist.angular.y = 0
obstacle_state.twist.angular.z = 0
SetModelStateTracker.get_instance().set_model_state(obstacle_state)
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 get_initial_state(self, id):
# start position
state_msg = ModelState()
state_msg.model_name = 'X1'
state_msg.pose.position.x = INITIAL_STATES[id][0]
state_msg.pose.position.y = INITIAL_STATES[id][1]
state_msg.pose.position.z = INITIAL_STATES[id][2]
state_msg.pose.orientation.x = INITIAL_STATES[id][3]
state_msg.pose.orientation.y = INITIAL_STATES[id][4]
state_msg.pose.orientation.z = INITIAL_STATES[id][5]
state_msg.pose.orientation.w = INITIAL_STATES[id][6]
return state_msg
def set_object(self, name, pos, ori):
msg = ModelState()
msg.model_name = name
msg.pose.position.x = pos[0]
msg.pose.position.y = pos[1]
msg.pose.position.z = pos[2]
msg.pose.orientation.w = ori[0]
msg.pose.orientation.x = ori[1]
msg.pose.orientation.y = ori[2]
msg.pose.orientation.z = ori[3]
msg.reference_frame = 'world'
self.set_mode_pub.publish(msg)
def create_model_state(x, y, z, angle):
model_state = ModelState()
model_state.model_name = 'jackal'
model_state.pose.position.x = x
model_state.pose.position.y = y
model_state.pose.position.z = z
e = qt(axis=[0, 0, 1], angle=angle).elements
model_state.pose.orientation = Quaternion(e[1], e[2], e[3], e[0])
model_state.reference_frame = "world"
return model_state
def main():
global regions_, position_, desired_position_, state_, yaw_,pitch_,roll_,throttle_, yaw_error_allowed_, pitch_error_allowed_, roll_error_allowed_, throttle_error_allowed_
global srv_client_go_to_point_, srv_client_wall_follower_
global count_state_time_, count_loop_
rospy.init_node('bug0')
sub_laser = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
sub_odom = rospy.Subscriber('/odom', Odometry, clbk_odom)
rospy.wait_for_service('/go_to_point_switch')
rospy.wait_for_service('/wall_follower_switch')
rospy.wait_for_service('/gazebo/set_model_state')
srv_client_go_to_point_ = rospy.ServiceProxy('/go_to_point_switch', SetBool)
srv_client_wall_follower_ = rospy.ServiceProxy('/wall_follower_switch', SetBool)
srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
# set robot position
model_state = ModelState()
model_state.model_name = 'm2wr'
model_state.pose.position.x = initial_position_.x
model_state.pose.position.y = initial_position_.y
model_state.pose.position.z = initial_position_.z
resp = srv_client_set_model_state(model_state)
# initialize going to the point
change_state(0)
rate = rospy.Rate(20)
while not rospy.is_shutdown():
if regions_ == None:
continue
distance_position_to_line = distance_to_line(position_)
if state_ == 0:
if regions_['front'] > 0.15 and regions_['front'] < 1:
change_state(1)
elif state_ == 1:
if count_state_time_ > 5 and \
distance_position_to_line < 0.1:
change_state(0)
count_loop_ = count_loop_ + 1
if count_loop_ == 20:
count_state_time_ = count_state_time_ + 1
count_loop_ = 0
rospy.loginfo("distance to line: [%.2f], position: [%.2f, %.2f]", distance_to_line(position_), position_.x, position_.y, position_.z)
rate.sleep()
def main():
rospy.init_node('set_pose')
steps_params = rospy.get_param("~steps", "0, 0, 0.005")
steps = [float(x.strip()) for x in steps_params.split(",")]
print(steps)
steps[0] = rospy.get_param("~x_step", steps[0])
steps[1] = rospy.get_param("~y_step", steps[1])
steps[2] = rospy.get_param("~z_step", steps[2])
max_pos_params = rospy.get_param("~max_pos", "1, 1, 5")
max_pos = [float(x.strip()) for x in max_pos_params.split(",")]
min_pos_params = rospy.get_param("~min_pos", "-1, -1, 0")
min_pos = [float(x.strip()) for x in min_pos_params.split(",")]
model_name = rospy.get_param("~model_name", "wall")
init_pos_params = rospy.get_param("~init_pos", "0, 0, 2")
pos = [float(x.strip()) for x in init_pos_params.split(",")]
print(steps)
print(max_pos)
print(min_pos)
print(model_name)
rospy.wait_for_service('/gazebo/set_model_state')
inc = steps[:] #copy the array? not sure if not having [:] copies by pointer.
while not rospy.is_shutdown():
state_msg = ModelState()
state_msg.model_name = model_name
state_msg.pose.position.x = pos[0]
state_msg.pose.position.y = pos[1]
state_msg.pose.position.z = pos[2]
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 0
for i in range(3):
if pos[i] >= max_pos[i]:
inc[i] = -1 * steps[i]
if pos[i] <= min_pos[i]:
inc[i] = steps[i]
pos[i] += inc[i]
#print(pos)
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = set_state(state_msg)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
rospy.sleep(0.001)
def moving(self):
while not rospy.is_shutdown():
# When training in the moving_obstacles module
while True:
gate = ModelState()
model = rospy.wait_for_message('gazebo/model_states',
ModelStates)
for i in range(len(model.name)):
if model.name[i] == 'gate_moving' or model.name[
i] == 'gate_moving_1':
gate.model_name = model.name[i]
gate.pose = model.pose[i]
gate.twist = Twist()
gate.twist.linear.z = GATE_MOVE_SPEED * self.gate_dir
# Gate is moving down and close to bottom, wait "shut"
if (model.pose[i].position.z < 0.3
and self.gate_dir == -1):
self.gate_dir = 1
gate.twist.linear.z = 0
gate.pose.position.z = 0.25
self.pub_model.publish(gate)
try:
rospy.sleep(GATE_WAIT_TIME)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
continue
# Gate is moving up and close to top, wait "open"
if (model.pose[i].position.z > 1
and self.gate_dir == 1):
self.gate_dir = -1
gate.twist.linear.z = 0
gate.pose.position.z = 1
self.pub_model.publish(gate)
try:
rospy.sleep(GATE_WAIT_TIME)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
continue
self.pub_model.publish(gate)
try:
rospy.sleep(0.1)
except rospy.exceptions.ROSTimeMovedBackwardsException, e:
print(
"Ros error due to reset during sleep, disregard"
)
def directMove(self):
# initiliaze
# note: the name tag in the Dmstar launch file will overwrite the name of this
# take the next waypoint, delete previous waypoint and publish the path
if not self.moveFlag:
return
if len(self.path.poses) == 1:
self.path.poses.append(self.path.poses[0])
current_pose = self.path.poses[0]
next_pose = self.path.poses[1]
# current_pose = self.path.poses[self.counter]
# next_pose = self.path.poses[self.counter + 1]
# print("agent %d" % self.selfID)
# print(next_pose)
if (len(self.path.poses) >= 2):
del (self.path.poses[0])
self.selfPathPub.publish(self.path)
# loop till the next point is reached
yaw_diff = next_pose.yaw - current_pose.yaw
# wrap up to 0 - pi/2
if yaw_diff >= 2:
yaw_diff -= 4
elif yaw_diff <= -2:
yaw_diff += 4
step = ((next_pose.x - current_pose.x) / self.frequency,
(next_pose.y - current_pose.y) / self.frequency,
(yaw_diff) / self.frequency)
# start looping
for i in range(self.frequency):
pose = Pose()
pose.position.x = 1.2 * (current_pose.x + i * step[0]) + 0.6
pose.position.y = -(1.2 * (current_pose.y + i * step[1]) + 0.6)
pose.position.z = 0.1
pose.orientation.x = 0
pose.orientation.y = 0
pose.orientation.z = math.sin(
-(current_pose.yaw + i * step[2] + 2) / 2 * math.pi / 2)
pose.orientation.w = math.cos(
-(current_pose.yaw + i * step[2] + 2) / 2 * math.pi / 2)
state = ModelState()
state.model_name = 'Husky_' + 'agent' + str(self.selfID)
state.pose = pose
self.modelStatePub.publish(state)
self.rate.sleep()
self.counter += 1
def main():
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
set_model_config = rospy.ServiceProxy('/gazebo/set_model_configuration',
SetModelConfiguration)
for i in range(10000):
model_msg = ModelState()
model_msg.model_name = "humanoid"
model_msg.reference_frame = "world"
model_msg.pose.position.x = 0.6
model_msg.pose.position.y = 0.0
model_msg.pose.position.z = -0.4
resp_set = set_model_state(model_msg)
def callback(data):
twist = Twist()
twist.linear.x = data.linear.x
twist.linear.y = data.linear.y
twist.linear.z = data.linear.z
twist.angular.x = data.angular.x
twist.angular.y = data.angular.y
twist.angular.z = data.angular.z
state = ModelState()
state.model_name = "youbot_2"
state.twist = twist
state.reference_frame = "youbot_2"
ret = set_state(state)
def set_target(self):
for i, pos in enumerate(self.target_pos):
rospy.wait_for_service('gazebo/set_model_state')
try:
state = ModelState()
state.model_name = self.name_target + str(i)
state.reference_frame = "world"
state.pose.position.x = pos[0]
state.pose.position.y = pos[1]
self.set_model(state)
except rospy.ServiceException, e:
print("/gazebo/set_model_state service call failed")
def _reset(self):
# print("Resetting ...")
rospy.wait_for_service('gazebo/set_model_state')
try:
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)
except rospy.ServiceException, e:
print("/gazebo/set_model_state service call failed")
def teleport(self, location_pose, agent='movo'):
"""
places movo in the desired location
Parameters:
-----------
location_pose: Pose data type for desired location of movo
"""
teleport_loc = ModelState()
teleport_loc.model_name = agent
teleport_loc.pose = location_pose
self.set_model_state(teleport_loc)
rospy.sleep(1)
def random_start(self):
state_msg = ModelState()
state_msg.model_name = 'robot'
state_msg.pose.position.x = random.randint(0, 9)
state_msg.pose.position.y = random.uniform(-1, 1)
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = set_state(state_msg)
except (rospy.ServiceException) as e:
print("Service call failed: %s" % e)
def setModelState(self, currState, targetState):
#control = AckermannDrive()
control = self.rearWheelFeedback(currState, targetState)
values = self.rearWheelModel(control)
newState = ModelState()
newState.model_name = 'polaris_ranger_ev'
newState.pose = currState.pose
newState.twist.linear.x = 0 #values[0]
newState.twist.linear.y = .5 #values[1]
newState.twist.angular.z = values[2]
self.modelStatePub.publish(newState)
def moving_y(self, distance):
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 = distance
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 = -distance
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 set_model_state_client(name, position, orientation):
state_msg = ModelState()
state_msg.model_name = name
state_msg.pose.position = position
state_msg.pose.orientation = orientation
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
response = set_model_state(state_msg)
return response
except rospy.ServiceException as e:
print("Service call failed: %s", e)
def moving_tile_y_alternate_long_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) <= round(self.y_model_pose)+6.0 and round(model.pose[i].position.y) >= -round(self.y_model_pose)+2. and self.flag1 == 0:
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = -1.
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.y) != round(self.y_model_pose):
self.flag1 = 1
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.y = 1.
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif round(model.pose[i].position.y) == round(self.y_model_pose):
self.flag1 = 0
def reset(self):
# Reset Turtlebot 1 position
state_msg = ModelState()
state_msg.model_name = 'tb3_0'
state_msg.pose.position.x = -1.0
state_msg.pose.position.y = 0.0
state_msg.pose.position.z = 0.0
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 0
# Reset Turtlebot 2 Position
state_msg2 = ModelState()
state_msg2.model_name = 'tb3_1' #'unit_sphere_0_0' #'unit_box_1' #'cube_20k_0'
state_msg2.pose.position.x = 1.0
state_msg2.pose.position.y = 0.0
state_msg2.pose.position.z = 0.0
state_msg2.pose.orientation.x = 0
state_msg2.pose.orientation.y = 0
state_msg2.pose.orientation.z = -0.2
state_msg2.pose.orientation.w = 0
rospy.wait_for_service('gazebo/reset_simulation')
# Request service to reset the position of robots
rospy.wait_for_service('/gazebo/set_model_state')
try:
set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
resp = set_state(state_msg)
resp = set_state(state_msg2)
#resp_targ = set_state(state_target_msg)
except rospy.ServiceException as e:
print("Service Call Failed: %s"%e)
for i in range(self.num_robots):
self.move_cmd[i].linear.x = 0.0
self.move_cmd[i].angular.z = 0.0
self.pub_tb3[i].publish(self.move_cmd[i])
def get_initial_state(self, name, id):
# start position
state_msg = ModelState()
state_msg.model_name = name
state_msg.pose.position.x = INITIAL_STATES[id][0]
state_msg.pose.position.y = INITIAL_STATES[id][1]
state_msg.pose.position.z = INITIAL_STATES[id][2]
quat = quaternion_from_euler(0, 0, np.random.uniform(-np.pi, np.pi))
state_msg.pose.orientation.x = quat[0]
state_msg.pose.orientation.y = quat[1]
state_msg.pose.orientation.z = quat[2]
state_msg.pose.orientation.w = quat[3]
return state_msg
def reset_ball(self):
while self.BallPosX != -680:
Ball_msg = ModelState()
Ball_msg.model_name = 'football'
Ball_msg.pose.position.x = -6.8 #-6 #-6.8
Ball_msg.pose.position.y = random.uniform(-3.3, 3.3)
# Ball_msg.pose.position.y = 0
Ball_msg.pose.position.z = 0.12
Ball_msg.pose.orientation.x = 0
Ball_msg.pose.orientation.y = 0
Ball_msg.pose.orientation.z = 0
Ball_msg.pose.orientation.w = 1
self.reset_pub.publish(Ball_msg)
def reset_A(self):
while self.NunbotAPosX != -830:
A_msg = ModelState()
A_msg.model_name = 'nubot1'
A_msg.pose.position.x = -8.3 #-8 #-8.5
A_msg.pose.position.y = random.uniform(-1.7, 1.7) #0
# A_msg.pose.position.y = 0
A_msg.pose.position.z = 0
A_msg.pose.orientation.x = 0
A_msg.pose.orientation.y = 0
A_msg.pose.orientation.z = 0
A_msg.pose.orientation.w = 1
self.reset_pub.publish(A_msg)
def set_velocity(self, model_name, vx, vy, vz):
rospy.wait_for_service("/gazebo/get_model_state")
state = self.get_model_state(model_name, "world")
print("velocity:\t" + str(state.pose.position.x) + "\t\t" + str(state.pose.position.y))
msg_model_state = ModelState()
msg_model_state.model_name = model_name
msg_model_state.reference_frame = "world"
msg_model_state.pose = state.pose
msg_model_state.twist.linear.x = vx
msg_model_state.twist.linear.y = vy
msg_model_state.twist.linear.z = vz
self.set_model_state(msg_model_state)
def _pause_car_model(self):
'''Pause agent immediately at the current position
'''
car_model_state = ModelState()
car_model_state.model_name = self._agent_name_
car_model_state.pose = self.car_model_state.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
self.set_model_state(car_model_state)
def moveInGazebo(x, y, color):
#calls gazebo service to place red pieces
#receives position to be placed
state_msg = ModelState()
if color=="red":
state_msg.model_name = red_model_name.pop(0)
elif color=="blue":
state_msg.model_name = blue_model_name.pop(0)
state_msg.reference_frame = 'world'
state_msg.pose.position.x = x
state_msg.pose.position.y = y
state_msg.pose.position.z = 0.20
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)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def modelStatesCb(self, msg):
'''
Callback for gazebo model states
@param msg: received message
@type msg: gazebo_msgs/ModelStates
'''
for i in range(len(msg.name)):
if msg.name[i] in self._gazebo_robots:
model_state = ModelState()
model_state.model_name = msg.name[i]
model_state.pose = msg.pose[i]
model_state.twist = msg.twist[i]
self._gazebo_robots[msg.name[i]]['model'].update(
model_state, rospy.Time.now())
elif msg.name[i] in self._gazebo_objects:
model_state = ModelState()
model_state.model_name = msg.name[i]
model_state.pose = msg.pose[i]
model_state.twist = msg.twist[i]
self._gazebo_objects[msg.name[i]]['model'].update(
model_state, rospy.Time.now())
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 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 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 spawnMBase(x, y):
mState = ModelState()
mState.model_name = 'mobile_base'
mState.pose.position.x = x
mState.pose.position.y = y
mState.pose.position.z = 0
mState.pose.orientation.x = 0
mState.pose.orientation.y = 0
mState.pose.orientation.z = 0
mState.pose.orientation.w = 1
rp.wait_for_service('/gazebo/set_model_state')
try:
sms = rp.ServiceProxy('/gazebo/set_model_state', SetModelState)
print 'Model State: ', str(mState)
sms(mState)
except rp.ServiceException as e:
print 'Service Exception:', e
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.")
count = 0
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
import utils #for teleop
'''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():
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))
if state < 5:
state += 1
time.sleep(7)
elif state == 5:
state = 0
time.sleep(7)
rospy.init_node('sin_path', anonymous=True)
swarm_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=1000)
container_pub = rospy.Publisher('/gazebo/set_model_state', ModelState, queue_size=10)
rospy.Subscriber('/joy', Joy, joy_callback)
container_msg = ModelState()
container_msg.model_name = 'box'
container_msg.pose.position.x = 0
container_msg.pose.position.y = 0
container_msg.pose.position.z = 0.5
swarm_msg = [ModelState() for _ in xrange(25)]
for i in xrange(25):
swarm_msg[i].model_name = 'quadrotor' + str(i)
dt = 0.1
x = 0
y = 0
z = 0
x_values = np.arange(-5, 5, 0.0009, dtype=None)
x_values_flip = np.arange(5, -5, -0.0009, dtype=None)
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
