def marker_publisher(self, max=None):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, self.path, [0.1, 1.0, 1.0],
"path")
self.markers_msg.markers = [msg]
self.markers_pub.publish(self.markers_msg)
def Lanecb(msg):
global pub
rate = rospy.Rate(10)
MarkerArr = viz.MarkerArray()
for i in range(len(msg.waypoints)):
Marker = viz.Marker()
Marker.header.frame_id = "map"
Marker.header.stamp = rospy.Time.now()
Marker.type = 0
Marker.action = 0
Marker.scale.x = 0.8
Marker.scale.y = 0.15
Marker.scale.z = 0.15
Marker.color.r = 0.1
Marker.color.g = 1.0
Marker.color.b = 0.0
Marker.color.a = 1.0
Marker.pose = msg.waypoints[i].pose.pose
Marker.id = i
MarkerArr.markers.append(Marker)
if pub is not None:
pub.publish(MarkerArr)
rate.sleep()
def create_viz_markers(waypoints):
marray= viz_msgs.MarkerArray()
for w in waypoints:
m_arrow = create_arrow(w)
m_text = create_text(w)
marray.markers.append(m_arrow)
marray.markers.append(m_text)
return marray
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
with open(FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
all_results = deepcopy(cache_data)
path = list(filter(lambda x: x["ns"] == PATH_NS,
all_results))[0]["path"]
#Create Environment
environment = PointCloudEnvironment(
self.print, "garage_terrain_assessment.dictionary", "garage.pcd",
False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))
current_max = 0
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max],
[0.1, 1.0, 1.0], PATH_NS)
self.markers_msg.markers = [msg]
current_max += 10
self.markers_pub.publish(self.markers_msg)
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 marker_publisher(self, max=None):
self.markers_msg = visualization_msgs.MarkerArray()
for idx, marker in enumerate(self.markers[0:max]):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"],
marker["color"], str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
self.markers_pub.publish(self.markers_msg)
def markcb(self, msg):
self.count += 1
if self.count%FREQ_DIV == 0:
# update all of the marker paths
for m in msg.markers:
if m.id not in self.paths.keys() and m.type == VM.Marker.SPHERE:
self.paths[m.id] = Path(m)
elif m.id in self.paths.keys():
self.paths[m.id].update_path(m)
# now we can assemble a MarkerArray and publish:
ma = VM.MarkerArray()
ma.markers = [a.marker for a in self.paths.values()]
self.path_pub.publish(ma)
return
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 marker_publisher(self, max=None):
self.markers_msg = visualization_msgs.MarkerArray()
self.last_id = 0
for idx, path in enumerate([]):
stamp = self.get_clock().now().to_msg()
if path.get("path", False) is False:
path["path"] = self.prev_file[idx]["path"]
msg = ROSMessage.line_marker(self.last_id, stamp, path["path"], [0.1,1.0,1.0], path["ns"])
self.markers_msg.markers.append(msg)
self.last_id += 1
self.markers_pub.publish(self.markers_msg)
def run(self):
while not rospy.is_shutdown():
if self.last_msg:
rospy.loginfo('Press enter to publish')
raw_input()
current_msg = vis_msg.MarkerArray()
current_msg.markers = self.last_msg.markers[
self.current_start:self.current_start + self.n]
for marker in current_msg.markers:
marker.lifetime = rospy.Duration(0)
marker.header.stamp = rospy.Time()
self.pub.publish(current_msg)
self._update()
print "published"
print "{0} out of {1} published".format(
self.current_start + self.n, len(self.last_msg.markers))
def send_transforms(self, event):
tnow = rospy.Time.now()
mlist = []
for i, con in enumerate(self.controllers):
quat = con.quat
if self.running_flag:
pos = con.act_pos
con.update_marker_pose(pos)
else:
pos = con.simpos
con.update_marker_pose(pos)
self.br.sendTransform(pos, quat, tnow, con.conframe, SIMWF)
con.marker.header.stamp = tnow
mlist.append(con.marker)
ma = VM.MarkerArray()
ma.markers = mlist
self.marker_pub.publish(ma)
return
def send_joint_states(self, event):
tnow = rospy.Time.now()
# first send the transform:
quat = tuple(tf.transformations.quaternion_from_euler(
self.sys.get_config('TorsoPsi').q,
self.sys.get_config('TorsoTheta').q,
self.sys.get_config('TorsoPhi').q,
'rzyx'))
point = tuple((
self.sys.get_config('TorsoX').q,
self.sys.get_config('TorsoY').q,
self.sys.get_config('TorsoZ').q
))
self.br.sendTransform(point, quat,
tnow,
'torso', 'world')
self.js.header.stamp = tnow
self.joint_pub.publish(self.js)
# send transforms
for i in 4,5: #hands
self.send_single_transform(i, tnow)
if self.shoulders_bool:
for i in 2,3: #shoulders
self.send_single_transform(i, tnow)
else: #body
self.send_single_transform(1, tnow)
if self.legs_bool:
for i in 6,7: #legs
self.send_single_transform(i, tnow)
# update the constraint visualization:
mlist = []
for c in self.con_vis:
c.update_points()
mlist.append(c.marker)
ma = VM.MarkerArray()
ma.markers = mlist
self.con_pub.publish(ma)
def main():
# Initialization
rospy.init_node('objrec_spoofer')
# Get spoofer parameters
frame_id = rospy.get_param('~frame_id','/world')
publish_period = rospy.get_param('~publish_period',1.0)
publish_rate = rospy.Rate(1.0/publish_period)
# Get the model info
models = rospy.get_param('~models')
stl_uris = {}
for model in models:
stl_uris[model] = rospy.get_param('~stl_uris/'+model)
# Get objects from rosparam
spoofed_objects = rospy.get_param('~spoofed_objects')
# Create the publishers
obj_pub = rospy.Publisher('/recognized_objects',objrec_msgs.RecognizedObjects)
obj_marker_pub = rospy.Publisher('/recognized_objects_markers',visualization_msgs.MarkerArray)
while not rospy.is_shutdown():
# Initialize objects message
objects_msg = objrec_msgs.RecognizedObjects()
objects_msg.header.stamp = rospy.Time.now()
objects_msg.header.frame_id = frame_id
# Initialize marker array message
marker_array = visualization_msgs.MarkerArray()
marker_id = 0
for obj in spoofed_objects:
# Construct and populate an object message
pss_msg = objrec_msgs.PointSetShape()
pss_msg.label = obj['label']
pss_msg.confidence = obj['confidence']
pss_msg.pose = geometry_msgs.Pose(geometry_msgs.Point(*obj['position']), geometry_msgs.Quaternion(*obj['orientation']))
# Store the message
objects_msg.objects.append(pss_msg)
# Construct and populate a marker message
marker = visualization_msgs.Marker()
marker.header = objects_msg.header
marker.ns = "objrec"
marker.type = visualization_msgs.Marker.MESH_RESOURCE
marker.action = visualization_msgs.Marker.ADD
marker.lifetime = rospy.Duration(publish_period)
marker.scale = geometry_msgs.Vector3(0.001, 0.001, 0.001)
marker.color = std_msgs.ColorRGBA(1.0, 0.2, 0.4, 0.5)
marker.id = marker_id
marker.pose = pss_msg.pose
marker.mesh_resource = stl_uris[obj['label']]
# Store the message
marker_array.markers.append(marker)
marker_id += 1
# Publish the object information
obj_pub.publish(objects_msg)
# Publish the markers
obj_marker_pub.publish(marker_array)
publish_rate.sleep()
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(
visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(
visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd',
10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2,
'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(
sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
#Create Environment
environment = MapEnvironment(self.print, "simple_open.dictionary",
"src/exjobb/maps/map_simple_open.png",
0.015)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
#start_point = np.array([5,0.55,0]) #spiral
start_point = np.array([0.55, 0.7, 0])
goal_coverage = 1
paths_markers = []
#Get CPP path
current = "BA*"
current_param = {
'angle_offset': 0,
'step_size': 0.5,
'visited_threshold': 0.375
}
if current == "BA*":
cpp = BAstar(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
if current == "Inward Spiral":
cpp = Spiral(self.print,
motion_planner,
coverable_point_cloud,
time_limit=TIME_LIMIT,
parameters=current_param)
self.pcd_pub.publish(
point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(
coverable_point_cloud.point_cloud(coverable_point_cloud.points,
'my_frame'))
self.traversable_pcd_pub.publish(
traversable_point_cloud.point_cloud(traversable_point_cloud.points,
'my_frame'))
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
self.print(cpp.print_stats(path))
current_max = 0
coverable_point_cloud.covered_points_idx = np.array([])
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max],
[0.1, 1.0, 1.0], "path")
self.markers_msg.markers = [msg]
current_max += 2
new_path = path[current_max - 2:current_max]
self.markers_pub.publish(self.markers_msg)
#if current_max > 2:
coverable_point_cloud.visit_path(new_path)
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd(
)
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
def __init__(self):
super().__init__('MainNode')
#Publishers:
self.markers_pub = self.create_publisher(visualization_msgs.MarkerArray, 'marker', 3000)
self.markers_path_pub = self.create_publisher(visualization_msgs.Marker, 'path_markers', 3000)
self.pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'pcd', 10)
self.coverable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'coverable_pcd', 100)
self.visited_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_pcd', 100)
self.visited_ground_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'visited_ground_pcd', 100)
self.traversable_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'traversable_pcd', 100)
self.inaccessible_pcd_pub = self.create_publisher(sensor_msgs.PointCloud2, 'inaccessible_pcd', 100)
self.path_pub = self.create_publisher(nav_msgs.Path, 'path', 10)
with open(FILE, 'rb') as cached_pcd_file:
cache_data = pickle.load(cached_pcd_file)
all_results = deepcopy(cache_data)
param = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["param"]
cpp_name = list(filter(lambda x: x["ns"] == PATH_NS, all_results))[0]["cpp"]
#Create Environment
environment = PointCloudEnvironment(self.print, "garage_terrain_assessment.dictionary", "garage.pcd", False)
point_cloud = environment.full_pcd
traversable_point_cloud = environment.traversable_pcd
coverable_point_cloud = environment.coverable_pcd
motion_planner = MotionPlanner(self.print, traversable_point_cloud)
TIME_LIMIT = 400
goal_coverage = 0.97
start_point = np.array([28.6, -6.7, -10.3]) #garage
#start_point = np.array([-53.7, 54.2, -2.7]) #bridge
#start_point = np.array([-20.7, 43, -1]) #cross
if cpp_name == "BA*":
cpp = BAstar(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Inward Spiral":
cpp = Spiral(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
if cpp_name == "Sampled BA*":
cpp = RandomBAstar3(self.print, motion_planner, coverable_point_cloud, time_limit=TIME_LIMIT, parameters = param)
path = cpp.get_cpp_path(start_point, goal_coverage=goal_coverage)
all_segments = cpp.all_segments
all_movements = cpp.all_movements
markers = cpp.points_to_mark
self.pcd_pub.publish(point_cloud.point_cloud(point_cloud.points, 'my_frame'))
self.coverable_pcd_pub.publish(coverable_point_cloud.point_cloud(coverable_point_cloud.points, 'my_frame'))
self.traversable_pcd_pub.publish(traversable_point_cloud.point_cloud(traversable_point_cloud.points, 'my_frame'))
self.markers_msg = visualization_msgs.MarkerArray()
self.last_id = 0
ANIMATION = True
if not ANIMATION:
#Only markers
for idx, marker in enumerate(markers):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.point_marker(self.last_id, stamp, marker["point"], marker["color"], "markers")
self.markers_msg.markers.append(msg)
self.last_id += 1
#Add segments
coverable_point_cloud.covered_points_idx = np.array([])
for idx, segment in enumerate(all_segments):
if len(segment.path) == 0:
#all_movements.pop(idx)
continue
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, segment.path, [0.1,1.0,1.0], "segment"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
coverable_point_cloud.visit_path(segment.path)
if ANIMATION:
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
time.sleep(1)
#Add Movements
for idx, move in enumerate(all_movements):
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(self.last_id, stamp, move.path, [1.0,0.5,0.5], "move"+str(idx))
self.markers_msg.markers.append(msg)
self.last_id += 1
visited_ground_points_pcd = coverable_point_cloud.get_covered_points_as_pcd()
self.visited_ground_pcd_pub.publish(visited_ground_points_pcd)
self.markers_pub.publish(self.markers_msg)
return
current_max = 0
while current_max < len(path):
self.markers_msg = visualization_msgs.MarkerArray()
stamp = self.get_clock().now().to_msg()
msg = ROSMessage.line_marker(0, stamp, path[0:current_max], [0.1,1.0,1.0], PATH_NS)
self.markers_msg.markers = [msg]
current_max += 10
self.markers_pub.publish(self.markers_msg)