mirror of
https://github.com/davesarmoury/GLaDOS.git
synced 2025-09-26 22:31:26 +08:00
Updated description
This commit is contained in:
parent
90226a7f6f
commit
f598a2b4bc
@ -11,7 +11,7 @@ import math
|
||||
HEAD_DISTANCE = 0.6
|
||||
|
||||
def det_callback(msg):
|
||||
global broadcaster, world_to_cam, world_to_view, last_pos
|
||||
global broadcaster, world_to_cam, view_to_world, world_to_view, last_pos
|
||||
|
||||
if len(msg.objects) > 0:
|
||||
min_distance = 99999.0
|
||||
@ -25,7 +25,8 @@ def det_callback(msg):
|
||||
|
||||
o_mtx = compose_matrix(translate=[o.position[0], o.position[1], o.position[2]])
|
||||
world_to_o = np.matmul(world_to_cam, o_mtx)
|
||||
scale, shear, angles, translate, perspective = decompose_matrix(world_to_o)
|
||||
view_to_o = np.matmul(view_to_world, world_to_o)
|
||||
scale, shear, angles, translate, perspective = decompose_matrix(view_to_o)
|
||||
|
||||
horizontal_distance = math.sqrt(translate[0]*translate[0] + translate[1]*translate[1])
|
||||
yaw = math.atan2(translate[1], translate[0])
|
||||
@ -34,18 +35,19 @@ def det_callback(msg):
|
||||
t_mtx = compose_matrix(angles=[0,pitch,yaw])
|
||||
|
||||
t_mtx = np.matmul(t_mtx, h_mtx)
|
||||
t_mtx = np.matmul(world_to_view, t_mtx)
|
||||
|
||||
scale, shear, angles2, translate2, perspective = decompose_matrix(t_mtx)
|
||||
quat = quaternion_from_euler(angles2[0], angles2[1], angles2[2])
|
||||
|
||||
|
||||
broadcaster.sendTransform(translate2,
|
||||
quat,
|
||||
rospy.Time.now(),
|
||||
"target_" + str(o.label_id),
|
||||
"spray_origin_link")
|
||||
"world")
|
||||
|
||||
def main():
|
||||
global broadcaster, world_to_cam, world_to_view, last_pos
|
||||
global broadcaster, world_to_cam, view_to_world, world_to_view, last_pos
|
||||
rospy.init_node('glados_watcher')
|
||||
broadcaster = tf.TransformBroadcaster()
|
||||
|
||||
@ -58,10 +60,11 @@ def main():
|
||||
listener.waitForTransform('world', 'view_link', rospy.Time(0), rospy.Duration(0.5))
|
||||
trans, rot = listener.lookupTransform('world', 'view_link', rospy.Time(0))
|
||||
world_to_view = compose_matrix(translate=trans, angles=euler_from_quaternion(rot))
|
||||
view_to_world = np.linalg.inv(world_to_view)
|
||||
|
||||
listener.waitForTransform('world', 'eye_link', rospy.Time(0), rospy.Duration(0.5))
|
||||
trans, rot = listener.lookupTransform('world', 'eye_link', rospy.Time(0))
|
||||
last_pos = compose_matrix(translate=trans, angles=euler_from_quaternion(rot))
|
||||
# listener.waitForTransform('world', 'eye_link', rospy.Time(0), rospy.Duration(0.5))
|
||||
# trans, rot = listener.lookupTransform('world', 'eye_link', rospy.Time(0))
|
||||
# last_pos = compose_matrix(translate=trans, angles=euler_from_quaternion(rot))
|
||||
|
||||
rospy.loginfo(last_pos)
|
||||
|
||||
|
@ -13,11 +13,11 @@
|
||||
<xacro:arg name="camera_name" default="zed2" />
|
||||
<xacro:arg name="camera_model" default="zed2" />
|
||||
<xacro:arg name="base_frame" default="world" />
|
||||
<xacro:arg name="cam_pos_x" default="-0.5" />
|
||||
<xacro:arg name="cam_pos_x" default="0.1" />
|
||||
<xacro:arg name="cam_pos_y" default="0.0" />
|
||||
<xacro:arg name="cam_pos_z" default="1.0" />
|
||||
<xacro:arg name="cam_pos_z" default="-0.025" />
|
||||
<xacro:arg name="cam_roll" default="3.14159" />
|
||||
<xacro:arg name="cam_pitch" default="0.0" />
|
||||
<xacro:arg name="cam_pitch" default="-0.78539816339" />
|
||||
<xacro:arg name="cam_yaw" default="0.0" />
|
||||
<xacro:arg name="gnss_frame" default="" />
|
||||
|
||||
|
@ -63,6 +63,10 @@ Visualization Manager:
|
||||
Expand Link Details: false
|
||||
Expand Tree: false
|
||||
Link Tree Style: Links in Alphabetic Order
|
||||
eye_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
head_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
@ -117,6 +121,10 @@ Visualization Manager:
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
Value: true
|
||||
view_link:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
Show Trail: false
|
||||
world:
|
||||
Alpha: 1
|
||||
Show Axes: false
|
||||
@ -185,6 +193,20 @@ Visualization Manager:
|
||||
{}
|
||||
Update Interval: 0
|
||||
Value: false
|
||||
- Class: rviz_plugin_zed_od/ZedOdDisplay
|
||||
Enabled: true
|
||||
Joint Radius: 0.10000000149011612
|
||||
Label Scale: 5
|
||||
Link Size: 0.05000000074505806
|
||||
Name: ZedOdDisplay
|
||||
Queue Size: 10
|
||||
Show Bounding Boxes: true
|
||||
Show Labels: true
|
||||
Show Skeletons: true
|
||||
Topic: ""
|
||||
Transparency: 0.25
|
||||
Unreliable: false
|
||||
Value: true
|
||||
Enabled: true
|
||||
Global Options:
|
||||
Background Color: 255; 192; 192
|
||||
|
Loading…
Reference in New Issue
Block a user