def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(header=mil_ros_tools.make_header('/map'),
pose=Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(
orientation))))
self.distance_marker.header = mil_ros_tools.make_header('/map')
self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\
'Z: ' + str(self.target_depth) + 'm'
self.distance_marker.pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation))
self.target_distance_pub.publish(self.distance_marker)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = mil_msgs.MoveToGoal(
header=mil_ros_tools.make_header('/map'),
posetwist=mil_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def publish_target_pose(self, position, orientation):
self.target_pose_pub.publish(
PoseStamped(
header=mil_ros_tools.make_header('/map'),
pose=Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
)
)
self.distance_marker.header = mil_ros_tools.make_header('/map')
self.distance_marker.text = 'XY: ' + str(self.target_distance) + 'm\n' +\
'Z: ' + str(self.target_depth) + 'm'
self.distance_marker.pose = Pose(position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation))
self.target_distance_pub.publish(self.distance_marker)
def make_cylinder_marker(self,
base,
length,
color,
frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]),
**kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion(
[0.0, 0.0, 0.0, 1.0]))
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs)
return marker
def _send_debug_marker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the object.
This marker is a scaled cube with the demensions of the model.
Only called if debug_ros param == True
'''
if self.last3d is None or not self.found:
return
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = self.last_found_time_3D
m.ns = "orange_rectangle"
m.id = 0
m.type = 1
m.action = 0
# Real demensions of path marker
m.scale.x = self.rect_model.length
m.scale.y = self.rect_model.width
m.scale.z = 0.05
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(self.timeout_seconds)
self.markerPub.publish(m)
def sendDebugMarker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the path marker.
This marker is a scaled cube with the demensions and color of the actual marker.
Only called if debug_ros param == True
'''
m = Marker()
m.header.frame_id = self.cam.tfFrame()
m.header.stamp = self.last_found_time
m.ns = "path_markers"
m.id = 0
m.type = 1
m.action = 0
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
# Real demensions of path marker
m.scale.x = 1.2192
m.scale.y = 0.1524
m.scale.z = 0.05
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(0)
self.markerPub.publish(m)
def _send_debug_marker(self):
'''
Sends a rviz marker in the camera frame with the estimated pose of the object.
This marker is a scaled cube with the demensions of the model.
Only called if debug_ros param == True
'''
if self.last3d is None or not self.found:
return
m = Marker()
m.header.frame_id = '/map'
m.header.stamp = self.last_found_time_3D
m.ns = "orange_rectangle"
m.id = 0
m.type = 1
m.action = 0
# Real demensions of path marker
m.scale.x = self.rect_model.length
m.scale.y = self.rect_model.width
m.scale.z = 0.05
m.pose.position = numpy_to_point(self.last3d[0])
m.pose.orientation = numpy_to_quaternion(self.last3d[1])
m.color.r = 0.0
m.color.g = 0.5
m.color.b = 0.0
m.color.r = 1.0
m.color.a = 1.0
m.lifetime = rospy.Duration(self.timeout_seconds)
self.markerPub.publish(m)
def moveto_action(self, position, orientation):
self.client.cancel_goal()
rospy.logwarn("Going to waypoint")
rospy.logwarn("Found server")
# Stay under the water
if position[2] > 0.3:
rospy.logwarn("Waypoint set too high!")
position[2] = -0.5
goal = mil_msgs.MoveToGoal(
header=mil_ros_tools.make_header('/map'),
posetwist=mil_msgs.PoseTwist(
pose=geom_msgs.Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion(orientation)
)
),
speed=0.2,
linear_tolerance=0.1,
angular_tolerance=0.1
)
self.client.send_goal(goal)
# self.client.wait_for_result()
rospy.logwarn("Go to waypoint")
def cb_3d(self, req):
res = VisionRequestResponse()
if (self.last2d == None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = self.cam.tfFrame()
res.pose.header.stamp = self.last_found_time
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def draw_sphere(self, position, color, scaling=(0.11, 0.11, 0.11), _id=4, frame='/front_stereo'):
pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
id=_id,
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
self.rviz_pub.publish(marker)
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time -
self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def draw_sphere(position, color, scaling=(0.11, 0.11, 0.11), m_id=4, frame='/base_link'):
pose = Pose(
position=mil_ros_tools.numpy_to_point(position),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='wamv',
id=m_id,
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.SPHERE,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(*scaling),
lifetime=rospy.Duration(),
)
rviz_pub.publish(marker)
def make_cylinder_marker(self, base, length, color, frame='/base_link',
up_vector=np.array([0.0, 0.0, 1.0]), **kwargs):
'''Handle the frustration that Rviz cylinders are designated by their center, not base'''
center = base + (up_vector * (length / 2))
pose = Pose(
position=mil_ros_tools.numpy_to_point(center),
orientation=mil_ros_tools.numpy_to_quaternion([0.0, 0.0, 0.0, 1.0])
)
marker = visualization_msgs.Marker(
ns='sub',
header=mil_ros_tools.make_header(frame=frame),
type=visualization_msgs.Marker.CYLINDER,
action=visualization_msgs.Marker.ADD,
pose=pose,
color=ColorRGBA(*color),
scale=Vector3(0.2, 0.2, length),
lifetime=rospy.Duration(),
**kwargs
)
return marker
