def publish(self):
preX = recv['preX']
preY = recv['preY']
x = recv['x']
y = recv['y']
cp = CameraPlacement()
# 眼睛，也就是人所在的点。朝focus点看
# 设为预瞄-当前连线的延长线上，延长d [m]
d = 15.
distPre = np.sqrt((preX - x)**2 + (preY - y)**2)
if distPre < 0.001:
scale = 0.
else:
scale = d / distPre
eye = Point()
eye.x = x - scale * (preX - x)
eye.y = y - scale * (preY - y)
eye.z = 10. # 因为大地水平坐标系方向和常规不一样，所以为负
cp.eye.point = eye
cp.eye.header.frame_id = "base_link"
# 设置视觉焦点
focus = Point()
focus.x = recv['preX']
focus.y = recv['preY']
focus.z = 0.
cp.focus.point = focus
cp.focus.header.frame_id = "base_link"
up = Vector3(0, 0, 1) # 因为大地水平坐标系方向和常规不一样，所以为负
cp.up.vector = up
cp.up.header.frame_id = "base_link"
cp.time_from_start = rospy.Duration(1.0 / rate_float)
pub.publish(cp)
def cameraPlacement(self): #TIME = 0.05 TIME = 1.0 / 40 * 2.0 view_point = self.viewPoint() placement = CameraPlacement() placement.interpolation_mode = CameraPlacement.LINEAR placement.time_from_start = rospy.Duration(TIME) placement.eye.header.stamp = rospy.Time(0.0) placement.eye.point.x = view_point[0] placement.eye.point.y = view_point[1] placement.eye.point.z = view_point[2] placement.focus.header.stamp = rospy.Time(0.0) placement.focus.point.x = self.focus[0] placement.focus.point.y = self.focus[1] placement.focus.point.z = self.focus[2] placement.up.header.stamp = rospy.Time(0.0) placement.up.vector.x = self.z_up[0] placement.up.vector.y = self.z_up[1] placement.up.vector.z = self.z_up[2] placement.mouse_interaction_mode = CameraPlacement.ORBIT return placement
def cameraPlacement(self):
#TIME = 0.05
TIME = 1.0 / 40 * 2.0
view_point = self.viewPoint()
placement = CameraPlacement()
placement.interpolation_mode = CameraPlacement.LINEAR
placement.time_from_start = rospy.Duration(TIME)
placement.eye.header.stamp = rospy.Time(0.0)
placement.eye.point.x = view_point[0]
placement.eye.point.y = view_point[1]
placement.eye.point.z = view_point[2]
placement.focus.header.stamp = rospy.Time(0.0)
placement.focus.point.x = self.focus[0]
placement.focus.point.y = self.focus[1]
placement.focus.point.z = self.focus[2]
placement.up.header.stamp = rospy.Time(0.0)
placement.up.vector.x = self.z_up[0]
placement.up.vector.y = self.z_up[1]
placement.up.vector.z = self.z_up[2]
placement.mouse_interaction_mode = CameraPlacement.ORBIT
return placement
def __init__(self):
self._target_frame = rospy.get_param('~target_frame', 'world')
tap_timeout = rospy.get_param('~tap_timeout', 0.5)
self._altitude = rospy.get_param('~initial_altitude', 0.4)
self._altitude_scale = rospy.get_param('~altitude_scale', 1)
self._azimuth = rospy.get_param('~initial_azimuth', 0.4)
self._azimuth_scale = rospy.get_param('~azimuth_scale', -2)
self._distance = rospy.get_param('~initial_distance', 10)
self._distance_scale = rospy.get_param('~distance_scale', 8)
rate = rospy.get_param('~rate', 10)
self._min_interval = rospy.Duration.from_sec(1.0 / rate)
self._last_pub_time = rospy.Time(0)
topic_name = rospy.get_param('~topic_name', '/rviz/camera_placement')
self._touchpad = TrackpadHandler(tap_timeout=tap_timeout)
self._placement_msg = CameraPlacement()
self._placement_msg.interpolation_mode = CameraPlacement.SPHERICAL
self._placement_msg.time_from_start = self._min_interval
self._placement_msg.target_frame = self._target_frame
self._placement_msg.eye.header.frame_id = self._target_frame
self._placement_msg.focus.header.frame_id = self._target_frame
self._placement_msg.focus.point.x = 0
self._placement_msg.focus.point.y = 0
self._placement_msg.focus.point.z = 0
self._placement_msg.up.header.frame_id = self._target_frame
self._placement_msg.up.vector.x = 0
self._placement_msg.up.vector.y = 0
self._placement_msg.up.vector.z = 1.0
self._pub = rospy.Publisher(topic_name,
CameraPlacement,
queue_size=50,
latch=True)
self._sub = rospy.Subscriber('status',
Status,
self.cb_status,
queue_size=1)
self.publish_current_placement()
def __init__(self):
# parameters
self.hz = 20 # hz fot CameraPlacement Topic
self.Kp = 0.3 # Coefficient for position control
self.Kr = 2 # Coefficient for rotation control
# local variable
self.publisher = rospy.Publisher('camera',
CameraPlacement,
queue_size=10)
self.twist = None # subscribed message
self.lock = False
self.camera = CameraPlacement()
# First Publishe
rospy.sleep(0.1) # wait for connection
self.initialize()
# start listening
self.camera.time_from_start = rospy.Duration.from_sec(1 / self.hz / 2)
self.subscriber = rospy.Subscriber('spacenav_twist', Twist,
self.spacenav_callback)
def place_camera(target_frame, focus_point, eye_point):
cp = CameraPlacement()
cp.target_frame = target_frame
cp.focus.point = focus_point
cp.focus.header.frame_id = target_frame
cp.eye.point = eye_point
cp.eye.header.frame_id = target_frame
cp.up.vector = Vector3(0, 0, 1)
cp.up.header.frame_id = target_frame
cp.time_from_start = rospy.Duration(2)
cp.interpolation_mode = CameraPlacement.SPHERICAL
cp.interaction_disabled = False
# Unused options:
# cp.mouse_interaction_mode
# cp.allow_free_yaw_axis
cp_pub.publish(cp)
pict.header.frame_id = 'odom'
pict.pose.position = Point(px, py, 0.8)
pict.pose.orientation = Quaternion(0.5, 0.5, 0.5, -0.5)
pict.action = 4
pict.mode = 1
pict.character = ps
pict.size = 1
pict.speed = 0.25
pict.color.r = 1.0
pict.color.g = 0.0
pict.color.b = 0.0
pict.color.a = 1.0
pic.publish(pict)
cp = CameraPlacement()
cp.target_frame = 'odom'
cp.time_from_start = rospy.Duration(1.0 / rate_float)
cp.eye.header.frame_id = 'odom'
cp.eye.point = eye_point = auto_eye_point = Point(view[0][0], view[0][1],
view[0][2])
cp.focus.header.frame_id = 'odom'
cp.focus.point = focus_point = auto_focus_point = Point(0, 0, 0)
cp.up.header.frame_id = 'odom'
cp.up.vector = Vector3(0, 0, 0)
while not rospy.is_shutdown():
t = rospy.get_time()
r = 10