def test_pose_init(self):
""" Test if pose class can be initialized as expected. """
# Create from quaternion
p = Point(1, 3, 2)
o = Quaternion(math.sqrt(1 / 2), 0, 0, math.sqrt(1 / 2))
pose = Pose(p, o)
# Create from angle
pose2 = Pose(p, math.pi / 2)
self.assertEqual(pose, pose2)
g_pose = g_msgs.Pose()
g_pose.position = p.to_geometry_msg()
g_pose.orientation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_pose), pose)
# Create from geometry_msgs/transform
g_tf = g_msgs.Transform()
g_tf.translation = p.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.assertEqual(Pose(g_tf), pose)
def test_make_topic_strings_quaterion(self):
strings = ez_model.make_topic_strings(geo_msgs.Quaternion(),
'/pose/orientation')
self.assertEqual(len(strings), 4)
strings = ez_model.make_topic_strings(geo_msgs.Quaternion(),
'/pose/orientation',
modules=[quaternion_module.QuaternionModule()])
self.assertEqual(len(strings), 1)
self.assertEqual(strings[0], '/pose/orientation')
def array_to_pose_array(xyz_arr, frame_id, quat_arr=None):
assert quat_arr is None or len(xyz_arr) == len(quat_arr)
pose_array = gm.PoseArray()
for index, xyz in enumerate(xyz_arr):
pose = gm.Pose()
pose.position = gm.Point(*xyz)
pose.orientation = gm.Quaternion(0,0,0,1) if quat_arr is None else gm.Quaternion(*(quat_arr[index]))
pose_array.poses.append(pose)
pose_array.header.frame_id = frame_id
pose_array.header.stamp = rospy.Time.now()
return pose_array
def Quaternion(x=0, y=0, z=0, w=0, roll=0, pitch=0, yaw=0):
"""Supply either at least one of x, y, z, w
or at least one of roll, pitch, yaw."""
if x or y or z or w:
return gm.Quaternion(x, y, z, w)
elif roll or pitch or yaw:
quat_parts = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
quat = Quaternion(*quat_parts)
return quat
else:
# Assume unit quaternion
return gm.Quaternion(0.0, 0.0, 0.0, 1.0)
def pub_target_timer_callback(self):
(xyz, rpy) = planner_instance._target_pose
q = tf.transformations.quaternion_from_euler(*rpy)
msg = geometry_msgs.Pose(geometry_msgs.Point(*xyz),
geometry_msgs.Quaternion(*q))
publisher.publish(msg)
def to_ros_orientation(orientation):
"""Convert standard orientation as 4x4 matrix to ROS geometry msg quaternion
:param orientation: ndarray, standard pose matrix representing a single orientation
:return: geometry_msgs.Quaternion
"""
if isinstance(orientation, np.ndarray):
msg = GeometryMsg.Quaternion()
if orientation.shape == (4, 4):
q = transform.quaternion_from_matrix(orientation)
msg.x = q[0]
msg.y = q[1]
msg.z = q[2]
msg.w = q[3]
return msg
elif orientation.shape == (3, 3):
i = transform.identity_matrix()
i[:3, :3] = orientation
return to_ros_orientation(i)
elif orientation.size == 4:
msg.x = orientation[0]
msg.y = orientation[1]
msg.z = orientation[2]
msg.w = orientation[3]
return msg
else:
raise NotImplementedError
else:
raise NotImplementedError
def make_pose_stamped(position, orientation, frame='/body'):
wrench = geometry_msgs.WrenchStamped(
header=make_header(frame),
pose=geometry_msgs.Pose(
force=geometry_msgs.Vector3(*position),
orientation=geometry_msgs.Quaternion(*orientation)))
return wrench
def test_tf_funcs(self):
v = Vector(1, 3, 2)
# Create from angle
tf = Transform(v, math.pi / 2)
self.assertAlmostEqual(tf.get_angle(), math.pi / 2)
# Extensively test the angle function:
for _ in range(0, 100):
angle = random.randrange(-100, 100)
self.assert_equal_angles(Transform(v, angle).get_angle(), angle)
g_tf = g_msgs.Transform()
g_tf.translation = v.to_geometry_msg()
g_tf.rotation = g_msgs.Quaternion(0, 0, math.sqrt(1 / 2),
math.sqrt(1 / 2))
self.geom_tf_almost_eq(g_tf, tf.to_geometry_msg())
# Should return 90 degrees
tf2 = Transform(Vector(1, 0, 0), Quaternion(1, 0, 0, 1).normalised)
self.assertEqual(tf2.get_angle(), math.pi / 2)
# Multiply transforms
self.assertEqual(tf * tf2, Transform(Vector(1, 4, 2), math.pi))
def from_dict(self, dict_, root=None):
"""
This function creates a XForm tree from a dictionary object
:param dict_: The dictionary to create the XForm tree from
:type dict_: dict
:param root: The current root XForm of the tree
:type root: XForm
"""
if not root:
root = self
dict_ = dict_[dict_.keys()[0]]
for k, v in dict_.items():
if isinstance(v, dict):
child = XForm(root, name=k, ref_frame=dict_[k]['ref_frame'])
child.translation = gm_msg.Vector3(
x=dict_[k]['translation'][0],
y=dict_[k]['translation'][1],
z=dict_[k]['translation'][2])
child.rotation = gm_msg.Quaternion(x=dict_[k]['rotation'][0],
y=dict_[k]['rotation'][1],
z=dict_[k]['rotation'][2],
w=dict_[k]['rotation'][3])
self.from_dict(dict_[k], child)
def getObjects(mapInfo):
with open(mapInfo, 'r') as stream:
try:
yamled = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
# deletes info
del yamled['info']
# Populates dictionary with location names and their poses
objDict = yamled.values()
objLocations = {}
objNames = {}
for item in objDict:
itemName = item['name']
if itemName[0:4] != "wall":
x_loc = item['centroid_x'] + (item['width'] / 2 + .6) * math.cos(
math.radians(item['orientation']))
y_loc = item['centroid_y'] + (item['length'] / 2 + .6) * math.sin(
math.radians(item['orientation']))
quat = tf.transformations.quaternion_from_euler(
0, 0, item['orientation'] - 180)
itemLoc = geo_msgs.PoseStamped(
std_msgs.Header(),
geo_msgs.Pose(
geo_msgs.Point(x_loc, y_loc, 0),
geo_msgs.Quaternion(quat[0], quat[1], quat[2], quat[3])))
objLocations[itemName] = itemLoc
objNames[itemName] = ([item['value']])
return objLocations, objNames
def numpy_matrix_to_quaternion(np_matrix):
'''Given a 3x3 rotation matrix, return its geometry_msgs Quaternion'''
assert np_matrix.shape == (3, 3), "Must submit 3x3 rotation matrix"
pose_mat = np.eye(4)
pose_mat[:3, :3] = np_matrix
np_quaternion = transformations.quaternion_from_matrix(pose_mat)
return geometry_msgs.Quaternion(*np_quaternion)
def set_cart_target(self, quat, xyz, ref_frame):
ps = gm.PoseStamped()
ps.header.frame_id = ref_frame
ps.header.stamp = rospy.Time(0)
ps.pose.position = gm.Point(xyz[0], xyz[1], xyz[2])
ps.pose.orientation = gm.Quaternion(*quat)
self.cart_command.publish(ps)
def publishGraph(pubNodes,
pub_ns='net',
stamp=None,
frame_id='world',
size=0.03,
numLmks=0):
global ndb
marker = Marker(type=Marker.LINE_LIST, ns=pub_ns, action=Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = size
marker.scale.y = size
marker.color.b = 0.3
marker.color.a = 1.0
marker.color.g = 0.7
marker.pose.position = geom_msg.Point(0, 0, 0)
marker.pose.orientation = geom_msg.Quaternion(0, 0, 0, 1)
edg = ndb.get_all_edges()
marker.points = []
for e in edg:
marker.points.extend([
geom_msg.Point(e[0][0], e[0][1], 0),
geom_msg.Point(e[1][0], e[1][1], 0)
])
marker.lifetime = rospy.Duration(1)
pubNodes.publish(marker)
def msg(self):
q = msg.Quaternion()
q.x = self.x
q.y = self.y
q.z = self.z
q.w = self.w
return q
def to_message(self):
"""Convert current data to a trajectory point message.
> *Returns*
Trajectory point message as `uuv_control_msgs/TrajectoryPoint`
"""
p_msg = TrajectoryPointMsg()
# FIXME Sometimes the time t stored is NaN
(secs, nsecs) = float_sec_to_int_sec_nano(self.t)
p_msg.header.stamp = rclpy.time.Time(seconds=secs,
nanoseconds=nsecs).to_msg()
p_msg.pose.position = geometry_msgs.Point(x=self.p[0],
y=self.p[1],
z=self.p[2])
p_msg.pose.orientation = geometry_msgs.Quaternion(x=self.q[0],
y=self.q[1],
z=self.q[2],
w=self.q[3])
p_msg.velocity.linear = geometry_msgs.Vector3(x=self.v[0],
y=self.v[1],
z=self.v[2])
p_msg.velocity.angular = geometry_msgs.Vector3(x=self.w[0],
y=self.w[1],
z=self.w[2])
p_msg.acceleration.linear = geometry_msgs.Vector3(x=self.a[0],
y=self.a[1],
z=self.a[2])
p_msg.acceleration.angular = geometry_msgs.Vector3(x=self.alpha[0],
y=self.alpha[1],
z=self.alpha[2])
return p_msg
def list_to_quaternion(l):
q = gmsg.Quaternion()
q.x = l[0]
q.y = l[1]
q.z = l[2]
q.w = l[3]
return q
def publish_pose(self):
'''TODO:
Publish velocity in body frame
'''
linear_vel = self.body.getRelPointVel((0.0, 0.0, 0.0))
# TODO: Not 100% on this transpose
angular_vel = self.orientation.transpose().dot(self.angular_vel)
quaternion = self.body.getQuaternion()
translation = self.body.getPosition()
header = sub8_utils.make_header(frame='/world')
pose = geometry.Pose(
position=geometry.Point(*translation),
orientation=geometry.Quaternion(-quaternion[1], -quaternion[2], -quaternion[3], quaternion[0]),
)
twist = geometry.Twist(
linear=geometry.Vector3(*linear_vel),
angular=geometry.Vector3(*angular_vel)
)
odom_msg = Odometry(
header=header,
child_frame_id='/body',
pose=geometry.PoseWithCovariance(
pose=pose
),
twist=geometry.TwistWithCovariance(
twist=twist
)
)
self.truth_odom_pub.publish(odom_msg)
def _process_localize_goal(self):
goal = self._as_localize.accept_new_goal()
self._distortion = goal.distortion
self.loginfo("Received Localize goal %s" % str(goal))
if self._initialise:
message = 'robot is initialising already. Ignore the command'
self._send_result(False, message)
else:
if self.param['simulation']:
pose_msg = geometry_msgs.PoseWithCovarianceStamped()
pose_msg.header.frame_id = self.param['sim_global_frame']
pose_msg.header.stamp = rospy.Time.now() - rospy.Duration(
0.2) # TODO: get latest common time
pose_msg.pose.pose.position.x = self.param['sim_x']
pose_msg.pose.pose.position.y = self.param['sim_y']
pose_msg.pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(
0, 0, self.param['sim_a'])
pose_msg.pose.pose.orientation = geometry_msgs.Quaternion(
*quat)
self._pub_init_pose.publish(pose_msg)
# send success right away
self._send_result(True, 'Initialisation done in simulation.')
elif goal.command == goal.STAND_AND_LOCALIZE:
self._thread = threading.Thread(
target=self._stand_and_localize)
self._thread.start()
elif goal.command == goal.SPIN_AND_LOCALIZE:
self._thread = threading.Thread(target=self._spin_and_localize)
self._thread.start()
else:
message = 'Invalid command %s' % str(goal.command)
self._send_result(False, message)
def set_orientation(self, model_name, w, x, y, z):
model_state_raw = self._call_get_model_state(model_name=model_name)
model_state = gazebo_msg.ModelState(model_name=model_name,
pose=model_state_raw.pose,
twist=model_state_raw.twist)
model_state.pose.orientation = geometry_msg.Quaternion(x, y, z, w)
self._call_set_model_state(model_state=model_state)
return True
def __transform_from_pose_2d(ps):
msg = gms.Transform()
msg.translation = gms.Vector3(x=ps.x, y=ps.y, z=__HEIGHT)
euler = (0.0, 0.0, ps.theta)
qa = t3de.euler2quat(*euler)
quat = gms.Quaternion(w=qa[0], x=qa[1], y=qa[2], z=qa[3])
msg.rotation = quat
return msg
def copDetection():
copName = "deckard"
robberName = "roy"
# Get list of objects and their locations
mapInfo = 'map2.yaml'
objLocations = getObjects(mapInfo)
vertexes = objLocations.values()
vertexKeys = objLocations.keys()
# TODO: GetPlan not working because move base does not provide the service (EITHER FIX IT OR FIND A NEW WAY TO GET DISTANCE)
# tol = .1
# robberName = "roy"
# robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,0,1)))
# destination = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1)))
# print(robLoc)
# print(destination)
# # Get plan from make_plan service
# #rospy.wait_for_service("/" + robberName + "move_base/make_plan")
# try:
# planner = rospy.ServiceProxy("/" + robberName + "/move_base/make_plan", nav_srv.GetPlan)
# plan = planner(robLoc, destination, tol)
# poses = plan.plan.poses
# print(poses)
# except rospy.ServiceException, e:
# print "GetPlan service call failed: %s"%e
# return 0
# rospy.Subscriber("/" + copName + "/", geo_msgs.Pose, getCopLocation)
# rospy.Subscriber("/" + robberName + "/", geo_msgs.Pose, getRobberLocation)
copLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,1,0)))
pastCopLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,1,0)))
robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,1,0)))
test_path = [geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1))),
geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,0,1)))]
test_plans = {"kitchen": nav_msgs.Path(std_msgs.Header(), test_path)}
curCost, curDestination = chooseDestination(test_plans, copLoc, robLoc, pastCopLoc)
print("Minimum Cost: " + str(curCost) + " at " + curDestination)
#move base to curDestination
# state = mover_base.get_state()
# pathFailure = False
# while (state!=3 and pathFailure==False): # SUCCESSFUL
# #wait a second rospy.sleep(1)
# newCost = evaluatePath(test_plans[curDestination])
# if newCost > curCost*2:
# pathFailure = True
# rospy.loginfo("Just Reached " + vertexKeys[i])
# Floyd warshall stuff
mapGrid = np.load('mapGrid.npy')
floydWarshallCosts = np.load('floydWarshallCosts.npy')
evaluateFloydCost(copLoc, robLoc, floydWarshallCosts, mapGrid)
def move_to_pos_quat(group, pos, quat):
pose_goal = gm.Pose(gm.Point(*pos), gm.Quaternion(*quat))
group.set_pose_target(pose_goal)
plan_success = group.go(wait=True)
group.stop()
group.clear_pose_targets()
return plan_success
def cmd_vel_callback(msg):
global pose_lock, saved_pose, last_time, new_time
pose_lock.acquire()
ps = cp.copy(saved_pose)
pose_lock.release()
#if last_time is None:
# last_time = rp.get_rostime()
#else:
# last_time = cp.copy(new_time)
#new_time = rp.get_rostime()
#time_step = float(new_time.secs-last_time.secs) + 1e-9*np.float128(new_time.nsecs-last_time.nsecs)
#rp.logwarn(new_time)
#rp.logwarn(time_step)
if ps is None:
return
twist = gms.Twist()
translation = gms.Vector3()
# if ps.position[0] <= XLIM[0] and msg.linear[0] <= 0.0:
# twist.linear.x = 0.0
# translation.x = XLIM[0]
# elif ps.position[0] >= XLIM[1] and msg.linear[0] >= 0.0:
# twist.linear.x = 0.0
# translation.x = XLIM[1]
# else:
# twist.linear.x = msg.linear[0]
# translation.x = ps.position[0]
# if ps.position[1] <= YLIM[0] and msg.linear[1] <= 0.0:
# twist.linear.y = 0.0
# translation.y = YLIM[0]
# elif ps.position[1] >= YLIM[1] and msg.linear[1] >= 0.0:
# twist.linear.y = 0.0
# translation.y = YLIM[1]
# else:
# twist.linear.y = msg.linear[1]
# translation.y = ps.position[1]
twist.linear.x = msg.linear[0]
translation.x = ps.position[0]
twist.linear.y = msg.linear[1]
translation.y = ps.position[1]
twist.linear.z = 0.0
twist.angular.z = msg.angular
translation.z = ALTITUDE
rot_mat = np.eye(4)
rot_mat[0:2,0] = np.array(ps.orientation)
rot_mat[0:2,1] = utl.ccws_perp(ps.orientation)
quat = tft.quaternion_from_matrix(rot_mat)
rotation = gms.Quaternion()
rotation.x = quat[0]
rotation.y = quat[1]
rotation.z = quat[2]
rotation.w = quat[3]
transform = gms.Transform(translation, rotation)
cmd_traj = tms.MultiDOFJointTrajectory()
cmd_traj.points.append(tms.MultiDOFJointTrajectoryPoint())
cmd_traj.points[0].transforms.append(transform)
cmd_traj.points[0].velocities.append(twist)
cmd_traj_pub.publish(cmd_traj)
def geom_pose_from_rt(rt):
msg = geom_msg.Pose()
msg.position = geom_msg.Point(x=rt.tvec[0], y=rt.tvec[1], z=rt.tvec[2])
xyzw = rt.quat.to_xyzw()
msg.orientation = geom_msg.Quaternion(x=xyzw[0],
y=xyzw[1],
z=xyzw[2],
w=xyzw[3])
return msg
def start_tf_msg(_start):
_msg = geom.TransformStamped()
_msg.header.stamp = rospy.Time.now()
_msg.header.frame_id = "map"
_msg.child_frame_id = "base_link"
_msg.transform.translation.x = _start.x
_msg.transform.translation.y = _start.y
_msg.transform.translation.z = 0
_msg.transform.rotation = geom.Quaternion(0, 0, 0, 1)
return _msg
def publishTagMarkers(tags, publisher):
"""Republishes the tag_markers topic"""
SHIFT = 0.05
h = std_msgs.Header(stamp=rospy.Time.now(), frame_id='map')
ca = std_msgs.ColorRGBA(r=1, a=1)
cb = std_msgs.ColorRGBA(g=1, a=1)
ct = std_msgs.ColorRGBA(b=1, a=1)
sa = geometry_msgs.Vector3(x=0.1, y=0.5, z=0.5)
sb = geometry_msgs.Vector3(x=0.1, y=0.25, z=0.25)
st = geometry_msgs.Vector3(x=1, y=1, z=1)
ma = visualization_msgs.MarkerArray()
ma.markers.append(
visualization_msgs.Marker(
header=h, id=-1, action=visualization_msgs.Marker.DELETEALL))
for i, t in enumerate(tags):
th = t['th_deg'] * math.pi / 180.
q = tf.transformations.quaternion_from_euler(0, 0, th)
q = geometry_msgs.Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t['x'], y=t['y']),
orientation=q),
scale=sa,
color=ca))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 1,
type=visualization_msgs.Marker.CUBE,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(
x=t['x'] + SHIFT * math.cos(th),
y=t['y'] + SHIFT * math.sin(th)),
orientation=q),
scale=sb,
color=cb))
ma.markers.append(
visualization_msgs.Marker(
header=h,
id=i * 3 + 2,
type=visualization_msgs.Marker.TEXT_VIEW_FACING,
action=visualization_msgs.Marker.ADD,
pose=geometry_msgs.Pose(
position=geometry_msgs.Point(x=t[1], y=t[2], z=0.5),
orientation=q),
scale=st,
color=ct,
text=str(t['id'])))
publisher.publish(ma)
def publish_line_segments(pub_ns,
_arr1,
_arr2,
c='b',
stamp=None,
frame_id='camera',
flip_rb=False,
size=0.002):
"""
Publish point cloud on:
pub_ns: Namespace on which the cloud will be published
arr: numpy array (N x 3) for point cloud data
c: Option 1: 'c', 'b', 'r' etc colors accepted by matplotlibs color
Option 2: float ranging from 0 to 1 via matplotlib's jet colormap
Option 3: numpy array (N x 3) with r,g,b vals ranging from 0 to 1
s: supported only by matplotlib plotting
alpha: supported only by matplotlib plotting
"""
arr1, carr = copy_pointcloud_data(_arr1, c, flip_rb=flip_rb)
arr2 = (deepcopy(_arr2)).reshape(-1, 3)
marker = vis_msg.Marker(type=vis_msg.Marker.LINE_LIST,
ns=pub_ns,
action=vis_msg.Marker.ADD)
if not arr1.shape == arr2.shape: raise AssertionError
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = size
marker.scale.y = size
marker.color.b = 1.0
marker.color.a = 1.0
marker.pose.position = geom_msg.Point(0, 0, 0)
marker.pose.orientation = geom_msg.Quaternion(0, 0, 0, 1)
# Handle 3D data: [ndarray or list of ndarrays]
arr12 = np.hstack([arr1, arr2])
inds, = np.where(~np.isnan(arr12).any(axis=1))
marker.points = []
for j in inds:
marker.points.extend([
geom_msg.Point(arr1[j, 0], arr1[j, 1], arr1[j, 2]),
geom_msg.Point(arr2[j, 0], arr2[j, 1], arr2[j, 2])
])
# RGB (optionally alpha)
marker.colors = [
std_msg.ColorRGBA(carr[j, 0], carr[j, 1], carr[j, 2], 1.0)
for j in inds
]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def to_message(self):
"""Convert current data to a trajectory point message."""
p_msg = TrajectoryPointMsg()
p_msg.header.stamp = rospy.Time(self.t)
p_msg.pose.position = geometry_msgs.Vector3(*self.p)
p_msg.pose.orientation = geometry_msgs.Quaternion(*self.q)
p_msg.velocity.linear = geometry_msgs.Vector3(*self.v)
p_msg.velocity.angular = geometry_msgs.Vector3(*self.w)
p_msg.acceleration.linear = geometry_msgs.Vector3(*self.a)
p_msg.acceleration.angular = geometry_msgs.Vector3(*self.alpha)
return p_msg
def list_to_quaternion(list_):
"""
This function creates a Quaternion object from a orientation list
:param list_: A list of x, y, z, w attributes
:type list_: list
:return: A Quaterion object
:rtype: Quaternion
"""
return gm_msg.Quaternion(x=list_[0], y=list_[1], z=list_[2], w=list_[3])
def get_posestamped(self):
pose = geo.Pose()
pose.position = geo.Point(
*
[float(vr.value()) for vr in [self.xline, self.yline, self.zline]])
pose.orientation = geo.Quaternion(*tr.quaternion_from_euler(*[
float(np.radians(vr.value()))
for vr in [self.phi_line, self.theta_line, self.psi_line]
]))
ps = stamp_pose(pose, str(self.frame_box.currentText()))
return ps