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 __init__(self, odometry_topic='/odom', pose_topic='/pose', duration=5):
super(MoveBase,
self).__init__(action_name="move_base",
action_type=move_base_msgs.MoveBaseAction,
worker=self.worker,
duration=duration)
self.odometry = nav_msgs.Odometry()
self.odometry.pose.pose.position = geometry_msgs.Point(0, 0, 0)
self.pose = geometry_msgs.PoseWithCovarianceStamped()
self.pose.pose.pose.position = geometry_msgs.Point(0, 0, 0)
latched = True
queue_size_five = 1
self.publishers = py_trees_ros.utilities.Publishers([
('pose', pose_topic, geometry_msgs.PoseWithCovarianceStamped,
latched, queue_size_five),
('odometry', odometry_topic, nav_msgs.Odometry, latched,
queue_size_five)
])
self.publishers.pose.publish(self.pose)
self.publishers.odometry.publish(self.odometry)
self.publishing_timer = rospy.Timer(period=rospy.Duration(0.5),
callback=self.publish,
oneshot=False)
self.start()
def straighline_planner(self, obs=None):
waypoints = linear_interp_traj([[self.start_x, self.start_y], [self.goal_x, self.goal_y]], 0.1)
waypoints = np.array(waypoints)
lin_x, lin_y, angu_z, ind, obs_lin_x, obs_lin_y = self.holonomic_controller.control(waypoints, self.start_x, self.start_y, self.start_yaw, self.goal_yaw, self.vel_rob[0], self.vel_rob[1], obs)
self.vel_msg.linear.x = lin_x
self.vel_msg.linear.y = lin_y
self.vel_msg.angular.z = 0.0
# self.vel_msg.angular.z = angu_z
# self.velocity_pub.publish(self.vel_msg)
robot_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
robot_marker.header.frame_id = 'world'
robot_marker.header.stamp = rospy.Time.now()
robot_marker.scale.x = 0.5
robot_marker.scale.y = 0.5
robot_marker.points = [geom_msg.Point(self.start_x, self.start_y, 0.0)]
robot_marker.colors = [std_msg.ColorRGBA(1.0, 0.0, 0.0, 1.0)]
self.robot_marker_pub.publish(robot_marker)
goal_marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, action=vis_msg.Marker.ADD)
goal_marker.header.frame_id = 'world'
goal_marker.header.stamp = rospy.Time.now()
goal_marker.scale.x = 0.5
goal_marker.scale.y = 0.5
goal_marker.points = [geom_msg.Point(self.goal_x, self.goal_y, 0.0)]
goal_marker.colors = [std_msg.ColorRGBA(0.0, 0.0, 1.0, 1.0)]
self.goal_marker_pub.publish(goal_marker)
return waypoints, lin_x, lin_y, ind, obs_lin_x, obs_lin_y
def execute_control(self, u):
if u is not None:
twist_msg = geometry_msgs.Twist(
linear=geometry_msgs.Point(u[0], u[1], 0.),
angular=geometry_msgs.Point(0., 0., 0.)
)
self.cmd_vel_pub.publish(twist_msg)
### publish marker for debugging
marker = visualization_msgs.Marker()
marker.header.frame_id = '/map'
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [
geometry_msgs.Point(0, 0, 0),
geometry_msgs.Point(u[0], u[1], 0)
]
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 1.0
self.cmd_vel_debug_pub.publish(marker)
else:
self.cmd_vel_pub.publish(None)
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 linepub():
rospy.init_node("rviz_marker", anonymous=True)
sc = ScanSubscriber()
rospy.Subscriber("/scan", senmsg.LaserScan, sc.scanCallBack)
pub_free = rospy.Publisher("free_space_polygon",
vismsg.Marker,
queue_size=1)
rate = rospy.Rate(25)
# mark_f - free space marker
mark_f = vismsg.Marker()
mark_f.header.frame_id = "/laser"
mark_f.type = mark_f.LINE_STRIP
mark_f.action = mark_f.ADD
mark_f.scale.x = 0.2
mark_f.color.r = 0.1
mark_f.color.g = 0.4
mark_f.color.b = 0.9
mark_f.color.a = 0.9 # 90% visibility
mark_f.pose.orientation.x = mark_f.pose.orientation.y = mark_f.pose.orientation.z = 0.0
mark_f.pose.orientation.w = 1.0
mark_f.pose.position.x = mark_f.pose.position.y = mark_f.pose.position.z = 0.0
while not rospy.is_shutdown():
if free_space is not None:
# marker line points
mark_f.points = []
pl1 = geomsg.Point()
pl1.x = -0.2
pl1.y = -1.0
pl1.z = 0.0 # x an y mismatch?
pl2 = geomsg.Point()
pl2.x = -0.2
pl2.y = 0.6
pl2.z = 0.0
mark_f.points.append(pl1)
mark_f.points.append(pl2)
try:
if type(free_space) is sg.polygon.Polygon:
for l in free_space.exterior.coords[
1:]: # the last point is the same as the first
p = geomsg.Point()
p.x = l[0]
p.y = l[1]
p.z = 0.0
mark_f.points.append(p)
except:
rospy.logwarn("exception")
mark_f.points.append(pl1)
# Publish the Markers
pub_free.publish(mark_f)
rate.sleep()
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 update_points(self):
self.marker.header.stamp = rospy.Time.now()
e1, p1 = self.get_point_loc(self.f1)
e2, p2 = self.get_point_loc(self.f2)
if e1 or e2:
p1 = (0,0,0)
p2 = (0,0,0)
self.marker.points = [
GM.Point(*p1),
GM.Point(*p2)
]
def step(self, action):
twist_msg = geometry_msgs.Twist(
linear=geometry_msgs.Point(action[1], -action[0],
action[2]), # forward, left, ascend
angular=geometry_msgs.Point(0., 0., action[3]))
self.cmd_vel_pub.publish(twist_msg)
self.rate.sleep()
while self.quad_to_obj_pos is None or \
self.quad_to_obj_rot is None or \
self.image is None:
rospy.sleep(.1)
obs = np.array(self.quad_to_obj_pos), np.array(
self.quad_to_obj_rot), self.image
return obs, None, None, None
def handle_initialization_request(req):
"rope initialization service"
xyz, _ = pc2xyzrgb(req.cloud)
xyz = np.squeeze(xyz)
obj_type = determine_object_type(xyz, plotting=False)
print "object type:", obj_type
if obj_type == "towel":
xyz = xyz[(xyz[:, 2] - np.median(xyz[:, 2])) < .05, :]
center, (height, width), angle = cv2.minAreaRect(
xyz[:, :2].astype('float32').reshape(1, -1, 2))
angle *= np.pi / 180
corners = np.array(center)[None, :] + np.dot(
np.array([[-height / 2, -width / 2], [-height / 2, width / 2],
[height / 2, width / 2], [height / 2, -width / 2]]),
np.array([[np.cos(angle), np.sin(angle)],
[-np.sin(angle), np.cos(angle)]]))
resp = bs.InitializationResponse()
resp.objectInit.type = "towel_corners"
resp.objectInit.towel_corners.polygon.points = [
gm.Point(corner[0], corner[1], np.median(xyz[:, 2]))
for corner in corners
]
resp.objectInit.towel_corners.header = req.cloud.header
print req.cloud.header
poly_pub.publish(resp.objectInit.towel_corners)
if obj_type == "rope":
total_path_3d = find_path_through_point_cloud(xyz, plotting=False)
resp = bs.InitializationResponse()
resp.objectInit.type = "rope"
rope = resp.objectInit.rope = bm.Rope()
rope.header = req.cloud.header
rope.nodes = [gm.Point(x, y, z) for (x, y, z) in total_path_3d]
rope.radius = .006
rospy.logwarn(
"TODO: actually figure out rope radius from data. setting to .4cm")
pose_array = gm.PoseArray()
pose_array.header = req.cloud.header
pose_array.poses = [
gm.Pose(position=point, orientation=gm.Quaternion(0, 0, 0, 1))
for point in rope.nodes
]
marker_handles[0] = rviz.draw_curve(pose_array, 0)
return resp
def visualize(self, samples, costs):
marker_array = vm.MarkerArray()
for i, (sample, cost) in enumerate(zip(samples, costs)):
if sample is None:
continue
origin = np.array([0., 0., 0.])
angle = 0.
for t in xrange(len(sample.get_U())):
marker = vm.Marker()
marker.id = i * len(sample.get_U()) + t
marker.header.frame_id = '/map'
marker.type = marker.ARROW
marker.action = marker.ADD
speed = sample.get_U(t=t, sub_control='cmd_vel')[0] / 5.
steer = sample.get_U(t=t, sub_control='cmd_steer')[0]
angle += (steer - 50.) / 100. * (np.pi / 2)
new_origin = origin + [
speed * np.cos(angle), speed * np.sin(angle), 0.
]
marker.points = [
gm.Point(*origin.tolist()),
gm.Point(*new_origin.tolist())
]
origin = new_origin
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.1
marker.scale.z = 0.1
if cost == min(costs):
rgba = (0., 1., 0., 1.)
marker.scale.x *= 2
marker.scale.y *= 2
marker.scale.z *= 2
else:
rgba = list(cm.hot(1 - cost))
rgba[-1] = 0.5
marker.color.r, marker.color.g, marker.color.b, marker.color.a = rgba
marker_array.markers.append(marker)
# for id in xrange(marker.id+1, int(1e4)):
# marker_array.markers.append(vm.Marker(id=id, action=marker.DELETE))
self.debug_cost_probcoll_pub.publish(marker_array)
def talker():
pub = rospy.Publisher('visualization_marker', vm.Marker, queue_size=10)
rospy.init_node('waypoint_course')
frame = rospy.get_param('~frame', 'odom')
dist = rospy.get_param('~size', 2)
XX = [0, dist, -dist, -dist, dist]
YY = [0, dist, dist, -dist, -dist]
m = vm.Marker()
m.type = m.SPHERE_LIST
m.action = m.ADD
s = 0.2
m.scale.x = s
m.scale.y = s
m.scale.z = s
m.color.r = 1.0
m.color.g = 0
m.color.a = 1.0
m.pose.orientation.w = 1.0
m.header.stamp = rospy.Time.now()
m.header.frame_id = frame
m.id = 1
for x, y, mid in zip(XX, YY, range(len(XX))):
p = gm.Point()
p.x = x
p.y = y
m.points.append(p)
cnt = 0
while ((cnt < 25) and (not rospy.is_shutdown())):
print("Publishing Sphere List")
pub.publish(m)
rospy.sleep(0.5)
cnt += 1
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 circle_marker(center,
radius,
scale,
color,
mframe,
z=.03,
duration=10.0,
m_id=0,
resolution=1.):
angles = np.matrix(np.arange(0, 360., resolution)).T
xs = center[0, 0] + np.cos(angles) * radius
ys = center[1, 0] + np.sin(angles) * radius
z = .03
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()['line_strip']
m.action = vm.Marker.ADD
m.points = [
gm.Point(xs[i, 0], ys[i, 0], z) for i in range(angles.shape[0])
]
m.colors = [
sdm.ColorRGBA(color[0, 0], color[1, 0], color[2, 0], color[3, 0])
for i in range(angles.shape[0])
]
m.scale.x = scale
m.lifetime = rospy.Duration(duration)
return m
def publish_octomap(pub_ns, arr, carr, size=0.1, stamp=None, frame_id='map', flip_rb=False):
"""
Publish cubes list:
"""
marker = vis_msg.Marker(type=vis_msg.Marker.CUBE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
# Point width, and height
marker.scale.x = size
marker.scale.y = size
marker.scale.z = size
N, D = arr.shape
# XYZ
inds, = np.where(~np.isnan(arr).any(axis=1))
marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]
# RGB (optionally alpha)
rax, bax = 0, 2
# carr = carr.astype(np.float32) * 1.0 / 255
if flip_rb: rax, bax = 2, 0
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,rax], carr[j,1], carr[j,bax], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
def look_at(self,
pt3d,
frame='base_link',
pointing_frame="wide_stereo_link",
pointing_axis=np.matrix([1, 0, 0.]).T,
wait=True):
g = pm.PointHeadGoal()
g.target.header.frame_id = frame
g.target.point = gm.Point(*pt3d.T.A1.tolist())
#pdb.set_trace()
g.pointing_frame = pointing_frame
g.pointing_axis.x = pointing_axis[0, 0]
g.pointing_axis.y = pointing_axis[1, 0]
g.pointing_axis.z = pointing_axis[2, 0]
g.min_duration = rospy.Duration(1.0)
g.max_velocity = 10.
self.head_client.send_goal(g)
if wait:
self.head_client.wait_for_result(rospy.Duration(1.))
if self.head_client.get_state() == amsg.GoalStatus.SUCCEEDED:
return True
else:
return False
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 line_marker(id, stamp, path, color, ns):
msg = visualization_msgs.Marker()
msg.header.frame_id = "my_frame"
msg.header.stamp = stamp
msg.ns = ns
msg.id = id
msg.type = 4
msg.action = 0
msg.points = []
for point in path:
position = geometry_msgs.Point()
position.x = point[0]
position.y = point[1]
position.z = point[2] + 0.05
msg.points.append(position)
#msg.points = path
msg.scale.x = 0.05
msg.scale.y = 0.05
msg.scale.z = 0.05
msg.color.r = color[0]
msg.color.g = color[1]
msg.color.b = color[2]
msg.color.a = 1.0
msg.frame_locked = True
return msg
def show_text_in_rviz_mullti(marker_publisher, text):
markers = MarkerArray()
for i in range(3):
marker = Marker(type=Marker.LINE_LIST,
ns='velodyne',
action=Marker.ADD)
marker.header.frame_id = "velodyne"
marker.header.stamp = rospy.Time.now()
# if self.bbox_data[i][0][0] == frame:
for n in range(8):
point = geom_msg.Point(self.bbox_data[i][n + 1][0],
self.bbox_data[i][n + 1][1],
self.bbox_data[i][n + 1][1])
marker.points.append(point)
marker.scale.x = 0.02
marker.lifetime = rospy.Duration.from_sec(0.1)
marker.color.a = 1.0
marker.color.r = 0.5
marker.color.g = 0.5
marker.color.b = 0.5
rospy.sleep(1)
markers.markers.append(marker)
marker_publisher.publish(markers)
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 publish_cloud_markers(pub_ns, _arr, c='b', stamp=None, flip_rb=False, frame_id='map'):
"""
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
"""
arr, carr = copy_pointcloud_data(_arr, c, flip_rb=flip_rb)
marker = vis_msg.Marker(type=vis_msg.Marker.SPHERE_LIST, ns=pub_ns, action=vis_msg.Marker.ADD)
marker.header.frame_id = frame_id
marker.header.stamp = stamp if stamp is not None else rospy.Time.now()
marker.scale.x = 0.01
marker.scale.y = 0.01
# XYZ
inds, = np.where(~np.isnan(arr).any(axis=1))
marker.points = [geom_msg.Point(arr[j,0], arr[j,1], arr[j,2]) for j in inds]
# RGB (optionally alpha)
N, D = arr.shape[:2]
if D == 3:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], 1.0)
for j in inds]
elif D == 4:
marker.colors = [std_msg.ColorRGBA(carr[j,0], carr[j,1], carr[j,2], carr[j,3])
for j in inds]
marker.lifetime = rospy.Duration()
_publish_marker(marker)
print 'Publishing marker', N
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 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 list_marker(points, colors, scale, mtype, mframe, duration=10.0, m_id=0):
m = vm.Marker()
m.header.frame_id = mframe
m.id = m_id
m.type = create_mdict()[mtype]
m.action = vm.Marker.ADD
m.points = [
gm.Point(points[0, i], points[1, i], points[2, i])
for i in range(points.shape[1])
]
#pdb.set_trace()
m.colors = [
sdm.ColorRGBA(colors[0, i], colors[1, i], colors[2, i], colors[3, i])
for i in range(colors.shape[1])
]
m.color.r = 1.
m.color.g = 0.
m.color.b = 0.
m.color.a = 1.
m.scale.x = scale[0]
m.scale.y = scale[1]
m.scale.z = scale[2]
m.lifetime = rospy.Duration(duration)
return m
def compute_point(self, time):
point = gms.Point()
self.__LOCK.acquire()
point.x = self.__x
point.y = self.__y
point.z = self.__z
self.__LOCK.release()
return point
def Point(x=0, y=0, z=0):
"""Make a Point
>>> Point(1.1, 2.2, 3.3)
x: 1.1
y: 2.2
z: 3.3
"""
return gm.Point(x, y, z)
def cmd_vel_callback(msg):
marker = vm.Marker()
marker.header.frame_id = '/odom'
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [
gm.Point(0, 0.7, 0),
gm.Point(msg.linear.x, 0.7 + msg.linear.y, 0)
]
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.r = 1.0
cmd_vel_debug_pub.publish(marker)
def odom_callback(msg):
marker = vm.Marker()
marker.header.frame_id = '/odom'
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [
gm.Point(-0.3, 0, 0),
gm.Point(-0.3 + msg.twist.twist.linear.x, msg.twist.twist.linear.y, 0)
]
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.g = 1.0
odom_debug_pub.publish(marker)
def speed_changed_callback(msg):
marker = vm.Marker()
marker.header.frame_id = '/odom'
marker.type = marker.ARROW
marker.action = marker.ADD
marker.points = [
gm.Point(0, -0.7, 0),
gm.Point(msg.speedX, -0.7 - msg.speedY, 0)
]
marker.lifetime = rospy.Duration()
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1.0
marker.color.b = 1.0
speed_changed_debug_pub.publish(marker)