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 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 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 _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 __init__(self):
self.layout = rospy.get_param('/thruster_layout/thrusters') # Add thruster layout from ROS param set by mapper
assert self.layout is not None, 'Could not load thruster layout from ROS param'
'''
Create MarkerArray with an arrow marker for each thruster at index node_id.
The position of the marker is as specified in the layout, but the length of the arrow
will be set at each thrust callback.
'''
self.markers = MarkerArray()
for i in xrange(len(self.layout)):
# Initialize each marker (color, scale, namespace, frame)
m = Marker()
m.header.frame_id = '/base_link'
m.type = Marker.ARROW
m.ns = 'thrusters'
m.color.a = 0.8
m.scale.x = 0.01 # Shaft diameter
m.scale.y = 0.05 # Head diameter
m.action = Marker.DELETE
m.lifetime = rospy.Duration(0.1)
self.markers.markers.append(m)
for key, layout in self.layout.iteritems():
# Set position and id of marker from thruster layout
idx = layout['node_id']
pt = numpy_to_point(layout['position'])
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].points.append(pt)
self.markers.markers[idx].id = idx
# Create publisher for marker and subscribe to thrust
self.pub = rospy.Publisher('/thrusters/markers', MarkerArray, queue_size=5)
self.thrust_sub = rospy.Subscriber('/thrusters/thrust', Thrust, self.thrust_cb, queue_size=5)
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 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 get_thruster_info(self, srv):
''' Get the thruster info for a particular thruster name '''
query_name = srv.thruster_name
info = self.ports[
self.thruster_to_port_map[query_name]].thruster_info[query_name]
thruster_info = ThrusterInfoResponse(
node_id=info.node_id,
min_force=info.thrust_bounds[0],
max_force=info.thrust_bounds[1],
position=numpy_to_point(info.position),
direction=Vector3(*info.direction))
return thruster_info
def get_thruster_info(self, srv):
''' Get the thruster info for a particular thruster name '''
query_name = srv.thruster_name
info = self.ports[self.thruster_to_port_map[query_name]].thruster_info[query_name]
thruster_info = ThrusterInfoResponse(
node_id=info.node_id,
min_force=info.thrust_bounds[0],
max_force=info.thrust_bounds[1],
position=numpy_to_point(info.position),
direction=Vector3(*info.direction)
)
return thruster_info
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 _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 thrust_cb(self, thrust):
'''
Each thrust callback, update the length of the arrow
based on the commanded thrust (in newtons).
Also update the color of the arrow based on thrust, from green to yellow, with
red being reserved for bounds.
'''
for cmd in thrust.thruster_commands:
if cmd.name not in self.layout: # Don't draw marker if thruster is not in layout
continue
layout = self.layout[cmd.name]
idx = layout['node_id']
bounds = layout['thrust_bounds']
# Select an arrow length based on commanded thrust, max thrust (from layout), and the MAX_LENGTH constant
if cmd.thrust < 0:
scale = -cmd.thrust / bounds[0]
else:
scale = cmd.thrust / bounds[1]
if np.isclose(scale, 0.0): # Avoid sending 0 length disk-like markers
self.markers.markers[idx].action = Marker.DELETE
continue
else:
self.markers.markers[idx].action = Marker.ADD
# Set color of marker based on thrust
if (cmd.thrust < 0 and cmd.thrust == bounds[0]) or cmd.thrust == bounds[1]:
self.markers.markers[idx].color.r = 1.0
self.markers.markers[idx].color.g = 0.0
else:
self.markers.markers[idx].color.r = abs(scale)
self.markers.markers[idx].color.g = 1.0
# Select endpoint for arrow based on length and direction vector from layout
direction = np.array(layout['direction'])
direction = direction / np.linalg.norm(direction)
pt2 = np.array(layout['position']) + self.MAX_LENGTH * scale * direction
self.markers.markers[idx].points[1] = numpy_to_point(pt2)
self.markers.markers[idx].header.stamp = rospy.Time.now()
self.pub.publish(self.markers)
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
#!/usr/bin/env python
import rospy
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(), rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground', rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
rate.sleep()
import numpy as np
import tf
import mil_ros_tools
from sensor_msgs.msg import PointCloud
rospy.init_node("ground_finder")
pub = rospy.Publisher("ground_est", PointCloud, queue_size=1)
listener = tf.TransformListener()
pc = PointCloud()
pc.header = mil_ros_tools.make_header(frame="map")
pc.points = []
rate = rospy.Rate(1.0)
while not rospy.is_shutdown():
try:
t = listener.waitForTransform('/map', '/ground', rospy.Time.now(),
rospy.Duration(1))
(trans, rot) = listener.lookupTransform('/map', '/ground',
rospy.Time(0))
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logwarn("TF waitForTransform timeout")
continue
pc.points.append(mil_ros_tools.numpy_to_point(np.array(trans)))
pub.publish(pc)
rate.sleep()
