def ground_truth_pose_callback(self, data):
self.cb_check_1 = True
self.pose_x = data.pose.pose.position.x
self.pose_y = data.pose.pose.position.y
self.yaw = e2q([
data.pose.pose.orientation.x, data.pose.pose.orientation.y,
data.pose.pose.orientation.z, data.pose.pose.orientation.w
])[2]
def spawn_box(size, pos, rpy, name):
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = pose[0]
p.pose.position.y = pose[1]
p.pose.position.z = pose[2] - 0.93
quat = e2q(*rpy)
p.pose.orientation = Quaternion(*quat)
print('Spawning box %s with position:\n%s \nand orientation:\n%s' % (name, p.pose.position, p.pose.orientation))
scene.add_box(name, p, size)
def extract(mat, rot_type='rpy'):
"""takes homogenous 4x4 matrix, returns translation and rotation parts.
rot can be 'rpy', 'quat' or 'matrix' for different representations"""
position = mat[:3, 3]
rotation = mat[:3, :3]
if 'rpy' in rot_type:
orientation = mat2rpy(rotation)
return position, orientation
elif 'quat' in rot_type:
orientation = mat2rpy(rotation)
quat = e2q(*orientation)
return position, quat
elif 'mat' in rot_type:
return position, orientation
def get_random_state(self):
ags = {}
for agent_name in self.agents.keys():
m = ModelState()
m.model_name = agent_name
m.pose.position.x = np.random.random() * 10 - 5
m.pose.position.y = np.random.random() * 10 - 5
ags[agent_name] = m
continue
x, y, z, w = e2q(0, 0, np.random.uniform(-np.pi, np.pi))
m.pose.orientation.x = x
m.pose.orientation.y = y
m.pose.orientation.z = z
m.pose.orientation.w = w
return ags
def gt_cb(self, data):
self.gt_cb_pozvan = True
self.yaw_gt = e2q([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z,
data.pose.pose.orientation.w])[2]
self.x_gt = data.pose.pose.position.x
self.y_gt = data.pose.pose.position.y
def gt_cb(self, data):
self.yaw_gt = e2q([data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.pose.pose.orientation.w])[2]
def generate_route(self, step_size, width, depth):
x_right = np.arange(0.0, width + step_size, step_size)
y_forward = np.arange(0.0, depth + step_size, step_size)
x_forward = np.ones_like(y_forward) * width
x_left = x_right[::-1]
x_back = np.zeros_like(y_forward)
y_right = np.zeros_like(x_right)
y_left = np.ones_like(x_right) * depth
y_back = y_forward[::-1]
yaw_delay = 10
z_setpt = 2.0
x_right1 = np.concatenate((np.ones(yaw_delay) * 0.0, x_right))
x_forward1 = np.concatenate(
(np.ones(yaw_delay) * x_right[-1], x_forward)
)
x_left1 = np.concatenate((np.ones(yaw_delay) * x_forward[-1], x_left))
x_forward12 = np.concatenate((np.ones(yaw_delay) * x_left[-1], x_back))
x_right2 = np.concatenate((np.ones(yaw_delay) * 0.0, x_right))
x_forward2 = np.concatenate(
(np.ones(yaw_delay) * x_right[-1], x_forward)
)
x_left2 = np.concatenate((np.ones(yaw_delay) * x_forward[-1], x_left))
x_forward22 = np.concatenate((np.ones(yaw_delay) * x_left[-1], x_back))
y_right1 = np.concatenate((np.ones(yaw_delay) * 0.0, y_right))
y_forward1 = np.concatenate(
(np.ones(yaw_delay) * y_right[-1], y_forward)
)
y_left1 = np.concatenate((np.ones(yaw_delay) * y_forward[-1], y_left))
y_forward12 = np.concatenate(
(np.ones(yaw_delay) * y_left[-1], y_back[::-1] + 1 * depth)
)
y_right2 = np.concatenate(
(np.ones(yaw_delay) * y_back[0] + 1 * depth, y_right + 2 * depth)
)
y_forward2 = np.concatenate(
(
np.ones(yaw_delay) * y_right[-1] + 2 * depth,
y_forward + 2 * depth,
)
)
y_left2 = np.concatenate(
(np.ones(yaw_delay) * y_forward[-1] + 2 * depth, y_left + 2 * depth)
)
y_forward22 = np.concatenate(
(
np.ones(yaw_delay) * y_left[-1] + 2 * depth,
y_back[::-1] + 3 * depth,
)
)
x = np.concatenate(
(
x_right1,
x_forward1,
x_left1,
x_forward12,
x_right2,
x_forward2,
x_left2,
x_forward22,
)
)
y = np.concatenate(
(
y_right1,
y_forward1,
y_left1,
y_forward12,
y_right2,
y_forward2,
y_left2,
y_forward22,
)
)
z = np.ones_like(x) * z_setpt
q_east = e2q(0, 0, 0)
q_north = e2q(0, 0, pi / 2)
q_west = e2q(0, 0, pi)
q_x = np.concatenate(
(
np.ones_like(x_right1) * q_east[0],
np.ones_like(x_forward1) * q_north[0],
np.ones_like(x_left1) * q_west[0],
np.ones_like(x_forward12) * q_north[0],
np.ones_like(x_right2) * q_east[0],
np.ones_like(x_forward2) * q_north[0],
np.ones_like(x_left2) * q_west[0],
np.ones_like(x_forward22) * q_north[0],
)
)
q_y = np.concatenate(
(
np.ones_like(x_right1) * q_east[1],
np.ones_like(x_forward1) * q_north[1],
np.ones_like(x_left1) * q_west[1],
np.ones_like(x_forward12) * q_north[1],
np.ones_like(x_right2) * q_east[1],
np.ones_like(x_forward2) * q_north[1],
np.ones_like(x_left2) * q_west[1],
np.ones_like(x_forward22) * q_north[1],
)
)
q_z = np.concatenate(
(
np.ones_like(x_right1) * q_east[2],
np.ones_like(x_forward1) * q_north[2],
np.ones_like(x_left1) * q_west[2],
np.ones_like(x_forward12) * q_north[2],
np.ones_like(x_right2) * q_east[2],
np.ones_like(x_forward2) * q_north[2],
np.ones_like(x_left2) * q_west[2],
np.ones_like(x_forward22) * q_north[2],
)
)
q_w = np.concatenate(
(
np.ones_like(x_right1) * q_east[3],
np.ones_like(x_forward1) * q_north[3],
np.ones_like(x_left1) * q_west[3],
np.ones_like(x_forward12) * q_north[3],
np.ones_like(x_right2) * q_east[3],
np.ones_like(x_forward2) * q_north[3],
np.ones_like(x_left2) * q_west[3],
np.ones_like(x_forward22) * q_north[3],
)
)
return x, y, z, q_x, q_y, q_z, q_w
import rospy
from gazebo_msgs.srv import SpawnModel, DeleteModel
from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion
from std_msgs.msg import Empty
from tf.transformations import quaternion_from_euler as e2q
pi = 3.14159265
model_path = '/home/andrey/.gazebo/models/'
table_path = model_path + 'table/model.sdf'
table_pose = Pose(position=Point(x=1.0, y=0.0, z=0.0),
orientation=Quaternion(*e2q(0.0, 0.0, -pi/2)))
drum1_path = model_path + 'disk_part/static_model.sdf'
drum1_pose = Pose(position=Point(x=0.79, y=0.42, z=1.02))
drum2_path = model_path + 'cinder_block_2/static_model.sdf'
drum2_pose = Pose(position=Point(x=0.89, y=0.00, z=1.02))
drum3_path = model_path + 'pulley_part/static_model.sdf'
drum3_pose = Pose(position=Point(x=0.76, y=-0.40, z=1.02))
def spawn_models(*model_paths_and_poses):
"""takes series of (path,pose) pairs and spawns them"""
rospy.wait_for_service('/gazebo/spawn_sdf_model')
spawn_sdf = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel)
for tup in model_paths_and_poses: