def freezeBase(self, flag):
#toggle gravity
req_reset_gravity = SetPhysicsPropertiesRequest()
#ode config
req_reset_gravity.time_step = 0.001
req_reset_gravity.max_update_rate = 1000
req_reset_gravity.ode_config.sor_pgs_iters = 50
req_reset_gravity.ode_config.sor_pgs_w = 1.3
req_reset_gravity.ode_config.contact_surface_layer = 0.001
req_reset_gravity.ode_config.contact_max_correcting_vel = 100
req_reset_gravity.ode_config.erp = 0.2
req_reset_gravity.ode_config.max_contacts = 20
if (flag):
req_reset_gravity.gravity.z = 0.0
else:
req_reset_gravity.gravity.z = -9.81
self.reset_gravity(req_reset_gravity)
# create the message
req_reset_world = SetModelStateRequest()
#create model state
model_state = ModelState()
model_state.model_name = "hyq"
model_state.pose.position.x = 0.0
model_state.pose.position.y = 0.0
model_state.pose.position.z = 0.8
model_state.pose.orientation.w = 1.0
model_state.pose.orientation.x = 0.0
model_state.pose.orientation.y = 0.0
model_state.pose.orientation.z = 0.0
model_state.twist.linear.x = 0.0
model_state.twist.linear.y = 0.0
model_state.twist.linear.z = 0.0
model_state.twist.angular.x = 0.0
model_state.twist.angular.y = 0.0
model_state.twist.angular.z = 0.0
req_reset_world.model_state = model_state
# send request and get response (in this case none)
self.reset_world(req_reset_world)
def move_on_line(self, start, goal):
dx = goal[0] - start[0]
dy = goal[1] - start[1]
segment_length = math.sqrt(dx**2 + dy**2)
segment_time = segment_length / self.vel
yaw = math.atan2(dy, dx)
segment_step_count = int(segment_time * self.node_frequency)
if segment_step_count == 0:
return
segment_time = segment_length / self.vel / segment_step_count
for step in numpy.linspace(0, segment_length, segment_step_count):
step_x = start[0] + step * math.cos(yaw)
step_y = start[1] + step * math.sin(yaw)
object_new_pose = Pose()
object_new_pose.position.x = step_x
object_new_pose.position.y = step_y
quat = tf.transformations.quaternion_from_euler(0.0, 0.0, yaw)
object_new_pose.orientation.x = quat[0]
object_new_pose.orientation.y = quat[1]
object_new_pose.orientation.z = quat[2]
object_new_pose.orientation.w = quat[3]
# spawn new model
model_state = ModelState()
model_state.model_name = self.name
model_state.pose = object_new_pose
model_state.reference_frame = 'world'
# publish message
self.pub.publish(model_state)
# call service
req = SetModelStateRequest()
req.model_state = model_state
#res = self.srv_set_model_state(req)
#if not res.success:
# print "something went wrong in service call"
# sleep until next step
self.rate.sleep()
def talker():
reset_pub = rospy.Publisher('/gazebo/set_model_state',
ModelState,
queue_size=10)
wp_pub = rospy.Publisher('/mybot/set_goal', Vector3, queue_size=10)
rospy.init_node('gazebo_reset', anonymous=True)
rate = rospy.Rate(10) # 10hz
reset_msg = ModelState()
reset_msg.model_name = 'mybot'
reset_msg.pose.position.x = 0
reset_msg.pose.position.y = 0
reset_msg.pose.position.z = 0
reset_msg.pose.orientation.x = 0
reset_msg.pose.orientation.y = 0
reset_msg.pose.orientation.z = 0
reset_msg.pose.orientation.w = 0
reset_msg.twist.linear.x = 0
reset_msg.twist.linear.y = 0
reset_msg.twist.linear.z = 0
reset_msg.twist.angular.x = 0
reset_msg.twist.angular.y = 0
reset_msg.twist.angular.z = 0
reset_msg.reference_frame = ''
wp_msg = Vector3()
wp_msg.x = 0
wp_msg.y = 0
wp_msg.z = 0
while not rospy.is_shutdown():
# hello_str = "hello world %s" % rospy.get_time()
# rospy.loginfo(hello_str)
# pub.publish(hello_str)
#while t1-t0<=1:
reset_pub.publish(reset_msg)
wp_pub.publish(wp_msg)
rate.sleep()
def rover_reset(self):
# Reset Rover-related Episodic variables
rospy.wait_for_service('gazebo/set_model_state')
self.x = INITIAL_POS_X
self.y = INITIAL_POS_Y
# Put the Rover at the initial position
model_state = ModelState()
model_state.pose.position.x = INITIAL_POS_X
model_state.pose.position.y = INITIAL_POS_Y
model_state.pose.position.z = INITIAL_POS_Z
model_state.pose.orientation.x = INITIAL_ORIENT_X
model_state.pose.orientation.y = INITIAL_ORIENT_Y
model_state.pose.orientation.z = INITIAL_ORIENT_Z
model_state.pose.orientation.w = INITIAL_ORIENT_W
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.model_name = 'rover'
self.gazebo_model_state_service(model_state)
self.last_collision_threshold = sys.maxsize
self.last_position_x = self.x
self.last_position_y = self.y
time.sleep(SLEEP_AFTER_RESET_TIME_IN_SECOND)
self.distance_travelled = 0
self.current_distance_to_checkpoint = INITIAL_DISTANCE_TO_CHECKPOINT
self.steps = 0
self.reward_in_episode = 0
self.collision = False
self.closer_to_checkpoint = False
self.power_supply_range = MAX_STEPS
self.reached_midpoint = False
# First clear the queue so that we set the state to the start image
_ = self.image_queue.get(block=True, timeout=None)
self.set_next_state()
def teleportRandom(self):
'''
Teleport agent return new x and y point
return agent posX, posY in list
'''
model_state_msg = ModelState()
model_state_msg.model_name = self.agent_model_name
# maze 1
xy_list = [[-2, 2], [-2, 1], [2, -2], [2, 2], [-1, 2], [-1, 1],
[1, -1], [1, 2]]
# Get random position for agent
pose = Pose()
pose.position.x, pose.position.y = random.choice(xy_list)
model_state_msg.pose = pose
model_state_msg.twist = Twist()
model_state_msg.reference_frame = "world"
# Start teleporting in Gazebo
isTeleportSuccess = False
for i in range(5):
if not isTeleportSuccess:
try:
rospy.wait_for_service('/gazebo/set_model_state')
telep_model_prox = rospy.ServiceProxy(
'/gazebo/set_model_state', SetModelState)
telep_model_prox(model_state_msg)
isTeleportSuccess = True
break
except Exception as e:
rospy.logfatal("Error when teleporting agent " + str(e))
else:
rospy.logwarn("Trying to teleporting agent..." + str(i))
time.sleep(2)
if not isTeleportSuccess:
rospy.logfatal("Error when teleporting agent")
return "Err", "Err"
return pose.position.x, pose.position.y
def __init__(self,
car_id,
start_dist,
speed,
lanes_list_np,
change_lane_freq_sec=0):
# Wait for ros services
rospy.wait_for_service('/gazebo/set_model_state')
rospy.wait_for_service('/gazebo/spawn_urdf_model')
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.spawn_model_client = rospy.ServiceProxy('gazebo/spawn_urdf_model',
SpawnModel)
# Spawn the new car
robot_description = rospy.get_param('robot_description_bot')
car_initial_pose = Pose()
car_initial_pose.position.x = 0
car_initial_pose.position.y = 0
car_initial_pose.position.z = 0
self.car_name = "racecar_{}".format(car_id)
self.resp = self.spawn_model_client(self.car_name, robot_description,
'/{}'.format(self.car_name),
car_initial_pose, '')
logger.info('Spawn status: {}'.format(self.resp.status_message))
# Initialize member data and reset car
self.car_speed = speed
# Initializing the lane that the bot car runs
self.lanes_list_np = lanes_list_np
self.lanes_one_hot = np.zeros(len(lanes_list_np), dtype=bool)
self.lanes_one_hot[0] = True
self.shapely_lane = lanes_list_np[self.lanes_one_hot][0]
print('self.shapely_lane.length = ', self.shapely_lane.length)
# Change the lanes after certain time
self.change_lane_freq_sec = change_lane_freq_sec
self.reverse_dir = False
self.car_model_state = ModelState()
self.reset_sim(start_dist)
def _get_car_start_model_state(self, start_dist):
'''get avaliable car start model state when starting a new lap
Args:
start_dist (float): start distance
Returns:
ModelState: start state
'''
_, closest_object_pose = self._get_closest_obj(start_dist)
# Compute the start pose
start_pose = self._track_data_.center_line.interpolate_pose(
start_dist, finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
# Check to place in inner or outer lane
if closest_object_pose is not None:
object_point = Point([
closest_object_pose.position.x, closest_object_pose.position.y
])
object_nearest_pnts_dict = self._track_data_.get_nearest_points(
object_point)
object_nearest_dist_dict = self._track_data_.get_nearest_dist(
object_nearest_pnts_dict, object_point)
object_is_inner = object_nearest_dist_dict[TrackNearDist.NEAR_DIST_IN.value] < \
object_nearest_dist_dict[TrackNearDist.NEAR_DIST_OUT.value]
if object_is_inner:
start_pose = self._track_data_.outer_lane.interpolate_pose(
self._track_data_.outer_lane.project(
Point(start_pose.position.x, start_pose.position.y)),
finite_difference=FiniteDifference.FORWARD_DIFFERENCE)
else:
start_pose = self._track_data_.inner_lane.interpolate_pose(
self._track_data_.inner_lane.project(
Point(start_pose.position.x, start_pose.position.y)),
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 gen_ball(self):
x, y, z = self.gen_ball_loc()
l_vel_x, l_vel_y, a_vel = self.gen_ball_vel((x, y))
# print("x:{} y:{} x:{}".format(x, y, z))
# print("a_vel: {} l_vel_x: {} l_vel_y: {}".format(a_vel, l_vel_x, l_vel_y))
ball_pose = self.gen_pose(x, y)
ball_name = self.dodgeball_prefix + str(self.ball_num)
self.ball_names.append(ball_name) # Add the ball to keep track of it
ball_twist = Twist(linear=Vector3(x=l_vel_x, y=l_vel_y),
angular=Vector3(z=0))
model_state = ModelState(model_name=ball_name,
pose=ball_pose,
twist=ball_twist)
self.spawn_model(model_name=ball_name,
model_xml=random.choice(self.dodgeballs),
robot_namespace='',
initial_pose=ball_pose)
self.set_model(model_state)
self.ball_num += 1
def resetobjects(self):
self.land.publish(Empty())
self.land4.publish(Empty())
#del_ball_prox = rospy.ServiceProxy('gazebo/delete_model', DeleteModel)
#del_ball_prox('volleyball')
#reset world
rospy.wait_for_service('/gazebo/reset_world')
reset_world = rospy.ServiceProxy('/gazebo/reset_world', EM)
#invoke
reset_world()
#reset ball
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 = 'volleyball'
model_state.pose = self.ball_pos
model_state.reference_frame = 'world'
set_model_state(model_state)
#UPDATE BANNER
rospy.wait_for_service('gazebo/spawn_sdf_model')
spawn_baner_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model',
SpawnModel)
banner_left = "gradebanner" + str(self.scoreA)
banner_right = "_gradebanner" + str(self.scoreB)
print banner_left
print banner_right
fn1 = '../models/' + banner_left + '/model.sdf'
fn2 = '../models/' + banner_right + '/model.sdf'
ff1 = open(fn1, 'r')
sdff1 = ff1.read()
ff2 = open(fn2, 'r')
sdff2 = ff2.read()
spawn_baner_prox(banner_left, sdff1, "myworld", self.left_banner_pos,
"world")
spawn_baner_prox(banner_right, sdff2, "myworld", self.right_banner_pos,
"world")
rospy.sleep(0.5)
self.takeoff.publish(Empty())
self.takeoff4.publish(Empty())
rospy.sleep(0.5)
def set_position(self, x, y, z):
state_msg = ModelState()
state_msg.model_name = 'three_pi'
state_msg.pose.position.x = x
state_msg.pose.position.y = y
state_msg.pose.position.z = z
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 0
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 move(time_, mvX, mvY, mvZ):
ball_pose.position.x += mvX
ball_pose.position.y += mvY
ball_pose.position.z += mvZ
state = ModelState()
state.model_name = "table_tennis_ball"
state.pose = ball_pose
out_msg = PoseStamped()
out_msg.header.frame_id = "/tt_ball"
out_msg.header.stamp = rospy.Time.now()
out_msg.pose = ball_pose
ret = set_model_state(state)
pub.publish(out_msg)
print ret.status_message
rospy.sleep(time_)
def set_aruco_position(self, x=0.7, y=-0.43, z=1):
state_msg = ModelState()
state_msg.model_name = 'aruco_cube'
state_msg.pose.position.x = x
state_msg.pose.position.y = y
state_msg.pose.position.z = z
state_msg.pose.orientation.x = 0
state_msg.pose.orientation.y = 0
state_msg.pose.orientation.z = 0
state_msg.pose.orientation.w = 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 movingTo(self,goal_x,goal_y):
#pub_model = rospy.Publisher('gazebo/set_model_state', ModelState, queue_size=1)
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
#print(model.name)
#print(model.twist[2])
# print(type(obstacle))
#time.sleep(5)
for i in range(len(model.name)):
if model.name[i] == 'target': # the model name is defined in .xacro file
obstacle.model_name = 'target'
obstacle.pose = model.pose[i]
obstacle.pose.position.x = float(goal_x)
obstacle.pose.position.y = float(goal_y)
# obstacle.twist = Twist()
# obstacle.twist.angular.z = 0.5
self.pub_model.publish(obstacle)
def factoryModelStateBegins(modelState=ModelState(),
pos=[0, 0, 0],
ori=[0, 0, 0, 0],
vl=[0, 0, 0],
va=[0, 0, 0]):
modelState.pose.position.x = pos[0]
modelState.pose.position.y = pos[1]
modelState.pose.position.z = pos[2]
modelState.pose.orientation.x = ori[0]
modelState.pose.orientation.y = ori[1]
modelState.pose.orientation.z = ori[2]
modelState.pose.orientation.w = ori[3]
modelState.twist.linear.x = vl[0]
modelState.twist.linear.y = vl[1]
modelState.twist.linear.z = vl[2]
modelState.twist.angular.x = va[0]
modelState.twist.angular.y = va[1]
modelState.twist.angular.z = va[2]
return modelState
def reset_base(self):
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 = self.base_name
self.reset_model_state(model_state)
set_model_state(model_state)
self.shutdown()
self.terminal = False
self.crashed = False
self.subscriptions_ready = np.zeros(2)
self.wait_for_subscriptions()
self.initial_time = time.time()
def _reset(self, car_model_state):
camera_model_state = ModelState()
camera_model_state.model_name = self.topic_name
# Calculate target Camera position based on nearest center track from the car.
# 1. Project the car position to 1-d global distance of track
# 2. Minus camera offset from the point of center track
near_dist = self.track_data._center_line_.project(
Point(car_model_state.pose.position.x,
car_model_state.pose.position.y))
near_pnt_ctr = self.track_data._center_line_.interpolate(near_dist)
yaw = self.track_data._center_line_.interpolate_yaw(
distance=near_dist,
normalized=False,
reverse_dir=False,
position=near_pnt_ctr)
quaternion = np.array(
euler_to_quaternion(pitch=self.look_down_angle_rad, yaw=yaw))
target_camera_location = np.array([near_pnt_ctr.x,
near_pnt_ctr.y,
0.0]) + \
apply_orientation(quaternion, np.array([-self.cam_dist_offset, 0, 0]))
# Calculate camera rotation quaternion based on lookAt yaw with respect to
# current camera position and car position
look_at_yaw = utils.get_angle_between_two_points_2d_rad(
Point(target_camera_location[0], target_camera_location[1]),
car_model_state.pose.position)
cam_quaternion = euler_to_quaternion(pitch=self.look_down_angle_rad,
yaw=look_at_yaw)
camera_model_state.pose.position.x = target_camera_location[0]
camera_model_state.pose.position.y = target_camera_location[1]
camera_model_state.pose.position.z = self.cam_fixed_height
camera_model_state.pose.orientation.x = cam_quaternion[0]
camera_model_state.pose.orientation.y = cam_quaternion[1]
camera_model_state.pose.orientation.z = cam_quaternion[2]
camera_model_state.pose.orientation.w = cam_quaternion[3]
self.model_state_client(camera_model_state)
self.last_camera_state = camera_model_state
self.last_yaw = yaw
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
pose = Pose()
pose.position.x = 0.0
pose.position.y = 0.0
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
# set initial model state
model_state = ModelState()
model_state.model_name = "catvehicle"
model_state.pose = pose
self.set_model_state_client(model_state)
msg = String()
msg.data = "123"
self.reset_sim_pub.publish(msg)
rospy.loginfo("Unpausing physics")
self.unpause_physics_client()
def __init__(self,
hack_index=1,
x=0,
y=0,
z=0,
theta=0,
radius=0.5,
mass=68.0,
max_speed=1.34,
fov=180,
agent_type=Type.person,
model=Model.person_1_walking_lidar):
self.hack_index = hack_index
self.id = self.create_id()
self.model_state = ModelState()
self.agent_type = agent_type
self.model = model
self.name = self.model + "_" + str(self.id)
self.max_speed = max_speed
self.fov = fov
self.radius = radius
self.mass = mass
self.x = x
self.y = y
self.z = z
self.theta = theta
self.force = 0.0
self.pose = Pose(position=Point(x, y, z))
self.velocity = Twist()
self.last_update = None
quaternion = quaternion_from_euler(0, 0, theta)
self.pose.orientation.x = quaternion[0]
self.pose.orientation.y = quaternion[1]
self.pose.orientation.z = quaternion[2]
self.pose.orientation.w = quaternion[3]
self.model_state.model_name = self.name
self.model_state.reference_frame = 'world'
self.model_state.pose = self.pose
def __init__(self):
self.initNode()
self.rate = rospy.Rate(1)
# TODO Why do we need this?
self.correctionFact = 1. # 0.92
self.sin30 = 0.5
self.cos30 = 0.86602540378
self.runVals = np.zeros(4)
self.riseVals = np.zeros(4)
self.prevTwistCmd = 0
# Slope values for opposite sonars
self.slopePoint = Point()
self.slope02 = 0
self.slope13 = 0
self.relYaw = 0
self.sonarVals = Dvl()
self.setStateMsg = ModelState()
self.getStateMsg = ModelStates()
self.keyboardTwist = Twist()
# Subscribers
# Sonar
self.dvlSub = rospy.Subscriber('/whn/dvl', Dvl, self.dvlCB)
# Model state
self.modelStateSub = rospy.Subscriber('/gazebo/model_states',
ModelStates, self.getStateCB)
# Get command vel
self.keyboardTwistSub = rospy.Subscriber('/cmd_vel', Twist,
self.keyTwistCB)
# Services
self.serviceName = '/gazebo/set_model_state'
self.setStateSrv = rospy.ServiceProxy(self.serviceName, SetModelState)
# Publisher
self.gradPointPub = rospy.Publisher('dvl_gradient',
Point,
queue_size=10)
def moving_tile_x_long_path1(self):
obstacle = ModelState()
model = rospy.wait_for_message('gazebo/model_states', ModelStates)
model_original_pose_x = model.pose[0].position.x
for i in range(len(model.name)):
if model.name[i] == self.model_name and self.flag==1 and self.flag1 == 0 and self.flag2==0 and self.flag3 == 1 and round(model.pose[i].position.x) < round(self.x_model_pose)+3 :
obstacle.model_name = self.model_name
obstacle.pose = model.pose[i]
obstacle.twist = Twist()
obstacle.twist.linear.x = speed
obstacle.twist.angular.z = 0
self.pub_model.publish(obstacle)
elif model.name[i] == self.model_name and round(model.pose[i].position.x) >= 4.0:
print("1")
self.flag4 = 1
self.flag3 = 0
def racecar_reset(self):
rospy.wait_for_service('gazebo/set_model_state')
waypoints = list(self.waypoints)
random_waypoint = int(np.random.choice(len(waypoints), 1))
closest_waypoint = self.get_closest_waypoint()
x, y = waypoints[(random_waypoint) % len(waypoints)]
x_next, y_next = waypoints[(random_waypoint + 1) % len(waypoints)]
orientation = np.arctan2(y_next - y, x_next - x)
modelState = ModelState()
modelState.pose.position.z = 0
modelState.pose.orientation.x = 0
modelState.pose.orientation.y = 0
modelState.pose.orientation.z = orientation
# Use this to randomize the orientation of the car
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 = '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
# modelState.pose.position.x = x
# modelState.pose.position.y = y
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 reset_pose(self):
stop = Twist()
time = rospy.get_time()
while rospy.get_time() - time < 2:
self.velcmd_pub.publish(stop)
self.stop_rate.sleep()
modelstate = ModelState()
modelstate.model_name = self.name
modelstate.reference_frame = "world"
x, y, z, w = rostf.transformations.quaternion_from_euler(
self.init_pose[0][3], self.init_pose[0][4], self.init_pose[0][5])
modelstate.pose.position.x = self.init_pose[0][0]
modelstate.pose.position.y = self.init_pose[0][1]
modelstate.pose.position.z = self.init_pose[0][2]
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 __init__(self):
# initiliaze
# self.LaunchGazebo()
rospy.init_node('GazeboWorld', anonymous=False, disable_signals=True)
#------------Params--------------------
self.world_start_time = time.time()
self.sim_time = Clock().clock
self.model_names = None
self.model_poses = None
#-----------Default Robot State-----------------------
self.default_state = ModelState()
self.default_state.model_name = None
self.default_state.pose.position.x = 0.
self.default_state.pose.position.y = 0.
self.default_state.pose.position.z = 0.
self.default_state.pose.orientation.x = 0.0
self.default_state.pose.orientation.y = 0.0
self.default_state.pose.orientation.z = 0.0
self.default_state.pose.orientation.w = 1.0
self.default_state.twist.linear.x = 0.
self.default_state.twist.linear.y = 0.
self.default_state.twist.linear.z = 0.
self.default_state.twist.angular.x = 0.
self.default_state.twist.angular.y = 0.
self.default_state.twist.angular.z = 0.
self.default_state.reference_frame = 'world'
#-----------Publisher and Subscriber-------------
self.set_state = rospy.Publisher('gazebo/set_model_state',
ModelState,
queue_size=100)
self.model_state_sub = rospy.Subscriber('gazebo/model_states',
ModelStates,
self.ModelStateCallBack)
self.sim_clock = rospy.Subscriber('clock', Clock,
self.SimClockCallBack)
while self.model_names is None and not rospy.is_shutdown():
pass
print "Env intialised"
def random_obstacle(self):
for n in range(0, 10):
state_msg = ModelState()
state_msg.model_name = 'obstacle_' + str(n)
#state_msg.pose.position.x = random.randint(1,9)
if n < 5:
state_msg.pose.position.x = n + 1
else:
state_msg.pose.position.x = 4 - n
state_msg.pose.position.y = random.uniform(-1, 1)
#state_msg.twist.linear.x = random.uniform(-0.3,0.3)
#state_msg.twist.linear.y = random.uniform(-0.3,0.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) as e:
print("Service call failed: %s" % e)
def reset_turtlebot(self):
self.start_turtlebot()
req = ModelState()
req.model_name = "turtlebot3"
req.twist.linear.x = 0.0
req.twist.linear.y = 0.0
req.twist.linear.z = 0.0
req.twist.angular.x = 0.0
req.twist.angular.y = 0.0
req.twist.angular.z = 0.0
req.pose.position.x = 0.0
req.pose.position.y = 0.0
req.pose.position.z = 0.0
req.pose.orientation.x = 0.0
req.pose.orientation.y = 0.0
req.pose.orientation.z = 0.0
req.pose.orientation.w = 0.0
self.set_state(req)
self.stop_turtlebot()
def main():
rospy.init_node('reading_laser')
sub = rospy.Subscriber('/m2wr/laser/scan', LaserScan, clbk_laser)
model_state = ModelState()
model_state.model_name = 'wheel'
model_state.pose.position.x = 0
model_state.pose.position.y = 7
#model_state.pose.position.z = 0
rospy.wait_for_service('/gazebo/set_model_state')
srv_client_set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
resp = srv_client_set_model_state(model_state)
rospy.spin()
def __init__(self):
"""initialize move_group API"""
rospy.init_node('gazebo_world')
"""init parm"""
self.Robot_model = {'fetch': {'init': [0, 0, 0]}}
self.Obstacle_model = {'obstacle': {'init': [0.7, 0, 0.75]}}
self.Move_Obstacle_model = {'obstacle': {'init': [1.7, 0, 0.75]}}
self.Fetch_init_pose_joints = [0.0, 1.5, 1.1, 0.0, -2.2, -1.8, 2, 0.4]
self.camera = RGB()
self.fetch = Robot()
self.robot_pose = ModelState()
self.pose_pub = rospy.Publisher("/gazebo/set_model_state",
ModelState,
queue_size=1)
self.pose_get = rospy.ServiceProxy('/gazebo/get_link_state',
GetLinkState)
self.dis = 100
def __init__(self):
# Get human's pose using service /gazebo/get_model_state
self.get_model_state = rospy.ServiceProxy('/gazebo/get_model_state',
GetModelState)
self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1)
rospy.wait_for_service("/gazebo/set_model_state", timeout=2)
msg_model_state = ModelState()
msg_model_state.model_name = "turtlebot3_burger"
msg_model_state.reference_frame = "world"
msg_model_state.pose.position.x = 0
msg_model_state.pose.position.y = 0
msg_model_state.pose.position.z = 0
msg_model_state.pose.orientation.x = 0
msg_model_state.pose.orientation.y = 0
msg_model_state.pose.orientation.z = 0
msg_model_state.pose.orientation.w = 1
msg_model_state.twist.linear.x = 0
msg_model_state.twist.linear.y = 0
msg_model_state.twist.linear.z = 0
msg_model_state.twist.angular.x = 0
msg_model_state.twist.angular.y = 0
msg_model_state.twist.angular.z = 0
rospy.wait_for_service("/gazebo/set_model_state", timeout=2)
res_set = self.set_model_state(msg_model_state)
print("set success? ", res_set)
self.vel = Twist()
self.vel.linear.x = 0
self.vel.linear.y = 0
self.vel.linear.z = 0
self.vel.angular.x = 0
self.vel.angular.y = 0
self.vel.angular.z = 0
for _ in range(10):
self.pub.publish(self.vel)
rospy.sleep(0.1)
self.integral = 0
self.goal_world = np.array([0, 0])
def move_model(self,
model_name,
pos,
rot=[0, 0, 0],
reference_frame='world'):
assert 'set_model_state' in self._services
assert isinstance(model_name, str), 'Input model_name must be a string'
assert model_name in self.get_model_names(), \
'Model {} does not exist'.format(model_name)
assert isinstance(pos, collections.Iterable)
assert len(list(pos)) == 3, 'Position vector must have three ' \
'components, (x, y, z)'
for elem in pos:
assert isinstance(elem, float) or isinstance(elem, int), \
'{} is not a valid number'.format(elem)
assert isinstance(rot, collections.Iterable), \
'Rotation vector must be iterable'
assert len(list(rot)) == 3 or len(list(rot)) == 4
for elem in rot:
assert isinstance(elem, float) or isinstance(elem, int), \
'{} is not a valid number'.format(elem)
if len(list(rot)) == 3:
rot = PosePCG.rpy2quat(*rot)
state = ModelState()
state.model_name = model_name
state.pose.position.x = pos[0]
state.pose.position.y = pos[1]
state.pose.position.z = pos[2]
state.pose.orientation.x = rot[0]
state.pose.orientation.y = rot[1]
state.pose.orientation.z = rot[2]
state.pose.orientation.w = rot[3]
state.reference_frame = reference_frame
status = self._services['set_model_state'](state)
return status.success
def kickBall():
state = ModelState()
state.model_name = "pioneer3at"
state.reference_frame = "pioneer3at"
set_state = rospy.ServiceProxy("/gazebo/set_model_state", SetModelState)
pose = Pose()
twist = Twist()
time.sleep(2)
twist.linear.x = -3
twist.linear.y = 0
twist.angular.z = 0
state.twist = twist
ret = set_state(state)
time.sleep(1)
twist.linear.x = 0
twist.linear.y = 0
twist.angular.z = 0
state.twist = twist
ret = set_state(state)
