def get_current_position_marker(self, link, offset=None, root="", scale=1, color=(0,1,0,1), idx=0):
(mesh, pose) = self.get_link_data(link)
marker = Marker()
if offset==None :
marker.pose = pose
else :
marker.pose = toMsg(fromMsg(offset)*fromMsg(pose))
marker.header.frame_id = root
marker.header.stamp = rospy.get_rostime()
marker.ns = self.robot_name
marker.mesh_resource = mesh
marker.type = Marker.MESH_RESOURCE
marker.action = Marker.MODIFY
marker.scale.x = scale
marker.scale.y = scale
marker.scale.z = scale
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
marker.text = link
marker.id = idx
marker.mesh_use_embedded_materials = True
return marker
def poseStampedToLabeledSphereMarker(cls, psdata, label, fmt="{label} %Y-%m-%d %H:%M:%S", zoffset=0.05):
"[poseStamped, meta] -> sphereMarker"
ps, meta = psdata
res = []
h = Header()
h.stamp = rospy.Time.now()
h.frame_id = ps.header.frame_id
sphere = Marker(type=Marker.SPHERE,
action=Marker.ADD,
header=h,
id=cls.marker_id)
sphere.scale.x = 0.07
sphere.scale.y = 0.07
sphere.scale.z = 0.07
sphere.pose = ps.pose
sphere.color = ColorRGBA(1.0,0,0,1.0)
sphere.ns = "db_play"
sphere.lifetime = rospy.Time(3)
cls.marker_id += 1
res.append(sphere)
text = Marker(type=Marker.TEXT_VIEW_FACING,
action=Marker.ADD,
header=h,
id=cls.marker_id)
text.scale.z = 0.1
text.pose = deepcopy(ps.pose)
text.pose.position.z += zoffset
text.color = ColorRGBA(1.0,1.0,1.0,1.0)
text.text = meta["inserted_at"].strftime(fmt).format(label=label)
text.ns = "db_play_text"
text.lifetime = rospy.Time(300)
cls.marker_id += 1
res.append(text)
return res
def publish_prob2(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
idx = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
current_probs = [0 for foo in objects]
for node in self._topo_map:
if node.name in waypoints:
for j in range(0, n_objects):
marker = Marker()
marker.header.frame_id = 'map'
marker.id = idx
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = probs[n_objects*i + j]
prob = prob/(scaling_factor)
print "AHAHHAHBHBHBHBHBHB", prob
marker.pose.position.z = marker.pose.position.z + current_probs[j]
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1*prob
current_probs[j] = current_probs[j] + prob + 0.1
marker.color.a = 1.0
marker.color.r = 1.0*prob
marker.color.g = 1.0*prob
marker.color.b = 1.0*prob
prob_msg.markers.append(marker)
idx = idx + 1
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def publish(anns, data):
wall_list = WallList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Walls
object = deserialize_msg(d.data, Wall)
wall_list.obstacles.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "wall_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('wall_marker', MarkerArray, latch=True, queue_size=1)
wall_pub = rospy.Publisher('wall_pose_list', WallList, latch=True, queue_size=1)
wall_pub.publish(wall_list)
marker_pub.publish(marker_list)
return
def publish(anns, data):
ar_mk_list = AlvarMarkers()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# AR markers
object = deserialize_msg(d.data, AlvarMarker)
ar_mk_list.markers.append(object)
# Visual markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "ar_mk_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('ar_mk_marker', MarkerArray, latch=True, queue_size=1)
ar_mk_pub = rospy.Publisher('ar_mk_pose_list', AlvarMarkers,latch=True, queue_size=1)
ar_mk_pub.publish(ar_mk_list)
marker_pub.publish(marker_list)
return
def publish(self, grasps, obj=None):
msg = MarkerArray()
self.marker_id = 0 # reset marker counter
if obj and len(obj.primitives) > 0 and len(obj.primitive_poses) > 0:
m = Marker()
m.header = obj.header
m.ns = "object"
m.id = self.marker_id
self.marker_id += 1
m.type = m.CUBE
m.action = m.ADD
m.pose = obj.primitive_poses[0]
m.scale.x = obj.primitives[0].dimensions[0]
m.scale.y = obj.primitives[0].dimensions[1]
m.scale.z = obj.primitives[0].dimensions[2]
m.color.r = 0
m.color.g = 0
m.color.b = 1
m.color.a = 0.8
msg.markers.append(m)
for grasp in grasps:
msg.markers.append(self.get_gripper_marker(grasp.grasp_pose, grasp.grasp_quality))
self.pub.publish(msg)
def pose_cb(self, pose):
pose = self.listener.transformPose(self.reference_frame,pose)
q = pose.pose.orientation
qvec = [q.x,q.y,q.z,q.w]
euler = euler_from_quaternion(qvec)
self.goal_x = pose.pose.position.x
self.goal_y = pose.pose.position.y
self.goal_theta = euler[2]
(ex,ey,etheta) = self.compute_error()
if ex < -self.dist_threshold:
self.goal_theta = norm_angle(etheta + pi)
print "New goal: %.2f %.2f %.2f" % (self.goal_x, self.goal_y, self.goal_theta)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 1.0;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def pub_at_position(self, odom_msg):
"""
Handles necessary information for displaying things at
ACCEPTED (NOVATEL) POSITION SOLUTION:
-vehicle mesh
!!!this should only be invoked when the accepted (novatel) position
is updated
"""
### G35 Mesh #############################################################
marker = Marker()
pub = self.curpos_publisher
marker.header = odom_msg.header
marker.id = 0 # enumerate subsequent markers here
marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = 1.55
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "vehicle_model"
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.0254 # artifact of sketchup export
marker.scale.y = 0.0254 # artifact of sketchup export
marker.scale.z = 0.0254 # artifact of sketchup export
marker.color.r = .05
marker.color.g = .05
marker.color.b = .05
marker.color.a = .2
marker.mesh_resource = self.veh_mesh_resource
marker.mesh_use_embedded_materials = False
pub.publish(marker)
def draw_bounding_box(self, id, box_msg, color=(1.0, 1.0, 0.0, 0.0), duration=0.0):
"""
Draws a bounding box as detectd by detect_bounding_box.
Parameters:
box_msg is a FindClusterBoundingBoxResponse msg.
color: a quadruple with alpha, r,g,b
duration: how long should the bounding box last. 0 means forever.
"""
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = box_msg.pose.header.frame_id
marker.pose = box_msg.pose.pose
marker.scale.x = box_msg.box_dims.x
marker.scale.y = box_msg.box_dims.y
marker.scale.z = box_msg.box_dims.z
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(marker)
def draw_table_rviz(self, id, table_msg, color = (1.0, 1.0, 1.0, 0.0), duration = 0.0):
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.ns = "object_detector"
marker.type = Marker.CUBE
marker.action = marker.ADD
marker.id = id
marker.header.frame_id = table_msg.pose.header.frame_id
marker.pose = table_msg.pose.pose
marker.pose.position.x = (table_msg.x_max-table_msg.x_min)/2 \
+ table_msg.x_min
marker.pose.position.y = (table_msg.y_max-table_msg.y_min)/2 \
+ table_msg.y_min
marker.scale.x = table_msg.x_max - table_msg.x_min
marker.scale.y = table_msg.y_max - table_msg.y_min
marker.scale.z = 0.01
marker.color.a = color[0]
marker.color.r = color[1]
marker.color.g = color[2]
marker.color.b = color[3]
marker.lifetime = rospy.Duration(duration)
self.box_drawer.publish(marker)
def publish_prob(self, waypoints, objects, probs):
prob_msg = MarkerArray()
i = 0
n_waypoints = len(waypoints)
n_objects = len(objects)
scaling_factor = max(probs)
for node in self._topo_map:
if node.name in waypoints:
marker = Marker()
marker.header.frame_id = 'map'
marker.id = i
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = node.pose
prob = 1
for j in range(0, n_objects):
prob = prob*probs[n_objects*i + j]
prob = prob/(scaling_factor**2)
print prob
marker.scale.x = 1*prob
marker.scale.y = 1*prob
marker.scale.z = 1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
prob_msg.markers.append(marker)
i = i + 1
self._prob_marker_pub.publish(prob_msg)
def create_object_marker(self, soma_obj, roi, soma_type, pose,markerno):
# create an interactive marker for our server
marker = Marker()
marker.header.frame_id = "map"
#int_marker.name = soma_obj+'_'+str(markerno)
#int_marker.description = soma_type + ' (' + roi +'_'+str(markerno)+ ')'
marker.pose = pose
marker.id = markerno;
# print marker.pose
marker.pose.position.z = 0.01
#marker = Marker()
marker.type = Marker.SPHERE
marker.action = 0
marker.scale.x = 0.25
marker.scale.y = 0.25
marker.scale.z = 0.25
marker.pose.position.z = (marker.scale.z / 2)
random.seed(soma_type)
val = random.random()
marker.color.r = r_func(val)
marker.color.g = g_func(val)
marker.color.b = b_func(val)
marker.color.a = 1.0
#marker.pose = pose
return marker
def create_person_label_marker(self, person, color):
h = Header()
h.frame_id = person.header.frame_id #tie marker visualization to laser it comes from
h.stamp = rospy.Time.now() # Note you need to call rospy.init_node() before this will work
#create marker:person_marker, modify a red cylinder, last indefinitely
mark = Marker()
mark.header = h
mark.ns = "person_label_marker"
## simple hack to map persons name to integer value for unique marker id
char_list = list(person.name)
char_int_list = [str(ord(x)) for x in char_list]
char_int_str = "".join(char_int_list)
char_int = int(char_int_str) & 255
print
print "str to int"
print char_int_list
print char_int
print "Char int binary) ", bin(char_int)
# mark.id = int(char_int / 10000)
mark.id = int(float(person.name)) #char_int
mark.type = Marker.TEXT_VIEW_FACING
mark.action = 0
mark.scale = Vector3(self.scale_factor * 0.5, self.scale_factor * 0.5, 1)
mark.color = color
mark.lifetime = rospy.Duration(0.5,0)
print "person name: ", person.name
mark.text = person.name
pose = Pose(Point(person.pose.position.x + 0.75, person.pose.position.y + 0.5, self.person_height + 0.75), Quaternion(0.0,0.0,1.0,cos(person.pose.position.z/2)))
mark.pose = pose
return mark
def line(self, frame_id, p, r, t=[0.0, 0.0], key=None):
"""
line: t r + p
This will be drawn for t[0] .. t[1]
"""
r = np.array(r)
p = np.array(p)
m = Marker()
m.header.frame_id = frame_id
m.ns = key or 'lines'
m.id = 0 if key else len(self.lines)
m.type = Marker.LINE_STRIP
m.action = Marker.MODIFY
m.pose = Pose(Point(0,0,0), Quaternion(0,0,0,1))
m.scale = Vector3(0.005, 0.005, 0.005)
m.color = ColorRGBA(0,0.8,0.8,1)
m.points = [Point(*(p+r*t)) for t in np.linspace(t[0], t[1], 10)]
m.colors = [m.color] * 10
key = key or element_key(m)
with self.mod_lock:
self.lines[key] = m
return key
def publish_pose(publisher, pose_stamped, r, g, b, a, marker_id):
"""Publishes a marker representing a bounding box.
Args:
publisher: A visualization_msgs/Marker publisher
pose_stamped: pose of marker
x, y, z: dimensions of bounding box
r, g, b, a: colour information for marker
marker_id: id # for marker
"""
marker = Marker()
marker.header.frame_id = pose_stamped.header.frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = 'pose'
marker.id = marker_id
marker.type = Marker.ARROW
marker.action = Marker.ADD
marker.pose = pose_stamped.pose
marker.scale.x = 0.07
marker.scale.y = 0.01
marker.scale.z = 0.01
marker.color.r = r
marker.color.g = g
marker.color.b = b
marker.color.a = a
marker.lifetime = rospy.Duration()
_publish(publisher, marker, "pose")
def _visualize_grasps(self, grasps, frame_id):
'''
Visualize all the grasps
**Args:**
**grasps:** The set of grasps to be visualized
**frame_id:** The frame id in which the grasps are defined
'''
mm = Marker()
action = mm.ADD
marray2 = MarkerArray()
for i in range(len(grasps)):
marker = Marker()
marker.header.frame_id = frame_id
marker.type = marker.ARROW
marker.action = action
marker.scale.x = 0.1
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
if i==0:
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
marker.id = i
marker.pose = grasps[i].grasp_pose
marray2.markers.append(marker)
self._grasps_pub.publish(marray2)
return True
def callback(self, state):
self._id = 0
m = self.create_arrow(state.base_pose)
m.color.r = 1.0
self._out_pub.publish(m)
m = self.create_arrow(state.bridge_pose)
m.color.g = 1.0
m.scale.x = Constants.BRIDGE_TO_STRIKE_MIN
self._out_pub.publish(m)
m = Marker()
m.header = state.bridge_pose.header
m.ns = "billiards_shot_plan"
m.id = self._id
m.action = Marker.ADD
m.type = Marker.CUBE
m.pose = state.bridge_pose.pose
m.pose.position.z += 0.055
m.scale.x = 0.005
m.scale.y = 0.05
m.scale.z = 0.11
m.color.a = 1.0
m.color.b = 1.0
self._out_pub.publish(m)
self._id += 1
def pose_cb(self, pose):
m_pose = PointStamped()
m_pose.header = pose.header
m_pose.point = pose.pose.position
m_pose = self.listener.transformPoint(self.reference_frame,m_pose)
self.goal_x = m_pose.point.x
self.goal_y = m_pose.point.y
print "New goal: %.2f %.2f" % (self.goal_x, self.goal_y)
marker = Marker()
marker.header = pose.header
marker.ns = "goal_marker"
marker.id = 10
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.5;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=-1.0
self.marker_pub.publish(marker)
def pawnCreation():
pawn = Marker()
pawn.header.frame_id = "/p_c_optical_frame"
pawn.type = pawn.CUBE
pawn.action = pawn.ADD
global homogMatrix
T = PyKDL.Frame(PyKDL.Rotation(homogMatrix[0][0], homogMatrix[0][1], homogMatrix[0][2],homogMatrix[1][0], homogMatrix[1][1], homogMatrix[1][2], homogMatrix[2][0], homogMatrix[2][1], homogMatrix[2][2]))
print homogMatrix
q = T.M.GetQuaternion()
pawn.pose = Pose(Point(newPoint[0],newPoint[1],newPoint[2]), Quaternion(q[0],q[1],q[2],q[3]))
pawn.color.a = 1.0
pawn.color.r = 1.0
pawn.color.g = 1.0
pawn.color.b = 0.0
pawn.scale.x = 0.2
pawn.scale.y = 0.4
pawn.scale.z = 0.01
if homogMatrix[0][0] != 0.0 :
pawn.scale.x = 0.2
pawn.scale.y = 0.4
pawn.scale.z = 0.01
else :
pawn.scale.x = 0.0
pawn.scale.y = 0.0
pawn.scale.z = 0.0
publisher.publish(pawn)
rospy.sleep(0.01)
def publish_shelf(publisher, pose_stamped):
"""Publishes a shelf marker at a given pose.
The pose is assumed to represent the bottom center of the shelf, with the
+x direction pointing along the depth axis of the bins and +z pointing up.
Args:
publisher: A visualization_msgs/Marker publisher
pose_stamped: A PoseStamped message with the location, orientation, and
reference frame of the shelf.
"""
marker = Marker()
marker.header.frame_id = pose_stamped.header.frame_id
marker.header.stamp = rospy.Time().now()
marker.ns = 'shelf'
marker.id = 0
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = 'package://pr2_pick_perception/models/shelf/shelf.ply'
marker.mesh_use_embedded_materials = True
marker.action = Marker.ADD
marker.pose = pose_stamped.pose
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.lifetime = rospy.Duration()
_publish(publisher, marker, "shelf")
def MakeMuneObject(self, MenuName, MenuPose):
MenuInteractiveMarker = InteractiveMarker()
MenuInteractiveMarker.name = MenuName
MenuInteractiveMarker.header.frame_id = self.frame_id
MenuInteractiveMarker.pose.position.z += self.MenuHight
MenuInteractiveMarker.scale = self.MenuScale
MenuControl = InteractiveMarkerControl()
MenuControl.interaction_mode = InteractiveMarkerControl.MENU
MenuControl.always_visible = False
MenuMarker = Marker()
MenuMarker.type = Marker.ARROW
MenuMarker.scale.x = MenuInteractiveMarker.scale * 2
MenuMarker.scale.y = MenuInteractiveMarker.scale * 0.45
MenuMarker.scale.z = MenuInteractiveMarker.scale * 0.45
MenuMarker.color.r = 0.5
MenuMarker.color.g = 0.5
MenuMarker.color.b = 0.5
MenuMarker.color.a = 1.0
MenuMarker.pose = MenuPose
MenuControl.markers.append(MenuMarker)
MenuInteractiveMarker.controls.append(MenuControl)
#print '###################MenuInteractiveMarker info:\n', MenuInteractiveMarker
self.server.insert(MenuInteractiveMarker)
rospy.loginfo('insert Menu Marker Object')
def _startPathPub(self):
path_marker_pub = rospy.Publisher('PathMarker', MarkerArray, queue_size=10)
rate = rospy.Rate(1) # 10hz
while self.run and not rospy.is_shutdown():
id = 1
m_array = MarkerArray()
for pt in self._current_pub_path:
m = Marker()
m.header.frame_id = "camera_link";
m.header.stamp = rospy.Time();
m.ns = "my_namespace";
m.id = id
m.type = Marker.SPHERE
m.action = Marker.ADD
m.pose = pt
m.scale.x = 0.05
m.scale.y = 0.05
m.scale.z = 0.05
m.color.r = 0.5
m.color.a = 1.0
m_array.markers.append(m)
id += 1
path_marker_pub.publish(m_array)
rate.sleep()
def publish(anns, data):
table_list = TableList()
marker_list = MarkerArray()
marker_id = 1
for a, d in zip(anns, data):
# Tables
object = pickle.loads(d.data)
table_list.tables.append(object)
# Markers
marker = Marker()
marker.id = marker_id
marker.header = a.pose.header
marker.type = a.shape
marker.ns = "table_obstacles"
marker.action = Marker.ADD
marker.lifetime = rospy.Duration.from_sec(0)
marker.pose = copy.deepcopy(a.pose.pose.pose)
marker.scale = a.size
marker.color = a.color
marker_list.markers.append(marker)
marker_id = marker_id + 1
marker_pub = rospy.Publisher('table_marker', MarkerArray, latch=True, queue_size=1)
table_pub = rospy.Publisher('table_pose_list', TableList, latch=True, queue_size=1)
table_pub.publish(table_list)
marker_pub.publish(marker_list)
return
def create_loading_station_markers(self, pose_percept=None, approach_pose=None, id=0):
if pose_percept:
station = Marker()
station.header.frame_id = pose_percept.header.frame_id
station.header.stamp = rospy.Time.now()
station.pose = copy.deepcopy(pose_percept.pose.pose)
station.ns = 'loading_stations'
station.id = id
station.color.r = 0.4
station.color.g = 0.4
station.color.b = 0.8
station.color.a = 1.0
station.type = Marker.CUBE
station.scale.x = 0.02
station.scale.y = 0.2
station.scale.z = 0.2
if approach_pose:
approach = copy.deepcopy(station)
approach.ns = 'loading_approach'
approach.type = Marker.ARROW
approach.pose = copy.deepcopy(approach_pose.pose)
approach.scale = Point(0.2, 0.1, 0.1)
else:
approach = self.create_delete_marker('loading_approach', id)
return station, approach
return self.create_delete_marker('loading_stations', id), self.create_delete_marker('loading_approach', id)
def make_kin_tree_from_link(ps,linkname, ns='default_ns',valuedict=None):
markers = []
U = get_pr2_urdf()
link = U.links[linkname]
if link.visual and link.visual.geometry and isinstance(link.visual.geometry,urdf.Mesh):
rospy.logdebug("%s is a mesh. drawing it.", linkname)
marker = Marker(type = Marker.MESH_RESOURCE, action = Marker.ADD)
marker.ns = ns
marker.header = ps.header
linkFromGeom = conversions.trans_rot_to_hmat(link.visual.origin.position, transformations.quaternion_from_euler(*link.visual.origin.rotation))
origFromLink = conversions.pose_to_hmat(ps.pose)
origFromGeom = np.dot(origFromLink, linkFromGeom)
marker.pose = conversions.hmat_to_pose(origFromGeom)
marker.scale = gm.Vector3(1,1,1)
marker.color = stdm.ColorRGBA(1,1,0,.5)
marker.id = 0
marker.lifetime = rospy.Duration()
marker.mesh_resource = str(link.visual.geometry.filename)
marker.mesh_use_embedded_materials = False
markers.append(marker)
else:
rospy.logdebug("%s is not a mesh", linkname)
if U.child_map.has_key(linkname):
for (cjoint,clink) in U.child_map[linkname]:
markers.extend(make_kin_tree_from_joint(ps,cjoint,ns=ns,valuedict=valuedict))
return markers
def draw_marker(
self,
ps,
id=None,
type=Marker.CUBE,
ns="default_ns",
rgba=(0, 1, 0, 1),
scale=(0.03, 0.03, 0.03),
text="",
duration=0,
):
"""
If markers aren't showing up, it's probably because the id's are being overwritten. Make sure to set
the ids and namespace.
"""
if id is None:
id = self.get_unused_id()
marker = Marker(type=type, action=Marker.ADD)
marker.ns = ns
marker.header = ps.header
marker.pose = ps.pose
marker.scale = gm.Vector3(*scale)
marker.color = stdm.ColorRGBA(*rgba)
marker.id = id
marker.text = text
marker.lifetime = rospy.Duration(duration)
self.pub.publish(marker)
self.ids.add(id)
return MarkerHandle(marker, self.pub)
def create_banner_markers(self, pose_percept=None, approach_pose=None, id=0):
if pose_percept:
banner = Marker()
banner.header.frame_id = pose_percept.header.frame_id
banner.header.stamp = rospy.Time.now()
banner.pose = copy.deepcopy(pose_percept.pose.pose)
banner.ns = 'number_banners'
banner.id = id
i = id + 4
banner.color.r = i/9*0.4
banner.color.g = i%9/3*0.4+0.1
banner.color.b = i%3*0.4+0.2
banner.color.a = 1.0
#banner.type = Marker.CUBE
banner.type = Marker.ARROW
banner.scale.x = 0.4
#banner.scale.x = 0.02
banner.scale.y = 0.2
banner.scale.z = 0.2
label = copy.deepcopy(banner)
label.type = Marker.TEXT_VIEW_FACING
label.text = '{} ({:3.1f})'.format(id, pose_percept.info.support)
label.ns = 'banner_label'
label.pose.position.z += 0.4
label.scale.z = 0.4
if approach_pose:
approach = copy.deepcopy(banner)
approach.ns = 'banner_approach'
approach.type = Marker.ARROW
approach.pose = copy.deepcopy(approach_pose.pose)
approach.scale = Point(0.2, 0.1, 0.1)
else:
approach = self.create_delete_marker('banner_approach', id)
return banner, label, approach
return self.create_delete_marker('number_banners', id), self.create_delete_marker('banner_label', id), self.create_delete_marker('banner_approach', id)
def callbackGeneratedActions(self,actions):
zero_orientation = Quaternion(0, 0, 0, 1)
zero_position = Point(0, 0, 0)
zero_pose = Pose(zero_position, zero_orientation)
counter = 0
marker_array = MarkerArray()
for pose in actions.actions:
marker = Marker()
marker.header.stamp = rospy.get_rostime()
marker.header.frame_id = "world"
marker.ns = "model_visualizer_generated_actions"
marker.id = counter
counter = counter + 1
marker.action = Marker.ADD
marker.scale = Vector3(0.02,0.02,0.02)
marker.color.a = 1
marker.color.b = random.random()
marker.color.r = random.random()
marker.color.g = random.random()
marker.type = Marker.ARROW
marker.pose = zero_pose
marker.points.append( Point(0,0,0) )
marker.points.append( Point(pose.position.x, pose.position.y, pose.position.z) )
marker_array.markers.append(marker)
self.pub_array.publish(marker_array)
def on_enter(self, userdata):
ma = MarkerArray()
self._path = MoveBaseActionPath()
for i in range(len(userdata.path.poses)):
marker = Marker(type=Marker.ARROW)
marker.header = userdata.path.header
marker.pose = userdata.path.poses[i].pose
marker.scale.x = 0.2
marker.scale.y = 0.02
marker.scale.z = 0.02
marker.color.b = 1.0
marker.color.r = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.g = 0.9 - 0.7 * i / len(userdata.path.poses)
marker.color.a = 0.8 - 0.5 * i / len(userdata.path.poses)
marker.id = i
ma.markers.append(marker)
self._failed = False
self._path.goal.target_path.poses = userdata.path.poses
self._path.goal.target_path.header.frame_id = "map"
self._pub.publish(self._pathTopic, self._path)
self._pub.publish(self._marker_topic, ma)
self._reached = True
def callback(data):
m = Marker()
m.header.frame_id = data.header.frame_id
# m.header.stamp = rospy.get_time()
m.ns = 'ncvrl'
m.id = 0
m.type = 2
# m.pose.position.x = 0
# m.pose.position.y = 0
# m.pose.position.z = 0
# m.pose.orientation.x = 0
# m.pose.orientation.y = 0
# m.pose.orientation.z = 0
# m.pose.orientation.w = 1.0
m.pose = data.pose
m.scale.x = 0.2
m.scale.y = 0.2
m.scale.z = 0.2
m.color.a = 0.5
m.color.r = 0.0
m.color.g = 1.0
m.color.b = 0.0
pub.publish(m);
def get_3d_feature(self, y1_bboxes, pointcloud_upper, pointcloud_lower):
start = time.time()
#rospy.loginfo('Processing Pointcloud with FPointNet')
# Assumed that pointclouds have 64 bit floats!
pc_upper = ros_numpy.numpify(pointcloud_upper).astype({
'names': ['x', 'y', 'z', 'intensity', 'ring'],
'formats': ['f4', 'f4', 'f4', 'f4', 'f4'],
'offsets': [0, 4, 8, 16, 20],
'itemsize':
32
})
pc_lower = ros_numpy.numpify(pointcloud_lower).astype({
'names': ['x', 'y', 'z', 'intensity', 'ring'],
'formats': ['f4', 'f4', 'f4', 'f4', 'f4'],
'offsets': [0, 4, 8, 16, 20],
'itemsize':
32
})
pc_upper = torch.from_numpy(
pc_upper.view(np.float32).reshape(pc_upper.shape +
(-1, )))[:, [0, 1, 2, 4]]
pc_lower = torch.from_numpy(
pc_lower.view(np.float32).reshape(pc_lower.shape +
(-1, )))[:, [0, 1, 2, 4]]
# move onto gpu if available
try:
pc_upper = pc_upper.cuda()
pc_lower = pc_lower.cuda()
except:
pass
# translate and rotate into camera frame using calib object
# in message pointcloud has x pointing forward, y pointing to the left and z pointing upward
# need to transform this such that x is pointing to the right, y pointing downwards, z pointing forward
# also done inside calib
pc_upper = self.calib.move_lidar_to_camera_frame(pc_upper, upper=True)
pc_lower = self.calib.move_lidar_to_camera_frame(pc_lower, upper=False)
pc = torch.cat([pc_upper, pc_lower], dim=0)
pc[:, 3] = 1
# pc = pc.cpu().numpy()
# self.publish_pointcloud_from_array(pc, self.pc_transform_pub, header = pointcloud_upper.header)
# idx = torch.randperm(pc.shape[0]).cuda()
# pc = pc[idx]
detections_2d = []
frame_det_ids = []
count = 0
for y1_bbox in y1_bboxes.bounding_boxes:
if y1_bbox.Class == 'person':
xmin = y1_bbox.xmin
xmax = y1_bbox.xmax
ymin = y1_bbox.ymin
ymax = y1_bbox.ymax
probability = y1_bbox.probability
frame_det_ids.append(count)
count += 1
detections_2d.append(
[xmin, ymin, xmax, ymax, probability, -1, -1])
features_3d = detection3d_with_feature_array()
features_3d.header.stamp = y1_bboxes.header.stamp
features_3d.header.frame_id = 'occam'
boxes_3d_markers = MarkerArray()
if not detections_2d:
self.marker_box_pub.publish(boxes_3d_markers)
self.feature_3d_pub.publish(features_3d)
return
boxes_3d, valid_3d, rot_angles, _, depth_features, frustums = \
generate_detections_3d(self.depth_model, detections_2d, pc,
self.calib, (3, 480, 3760), omni=True,
peds=True)
depth_features = convert_depth_features(depth_features, valid_3d)
for box, feature, i in zip(boxes_3d, depth_features, frame_det_ids):
#frustum = frustums[i]
#frustum[:, [0,2]] = np.squeeze(np.matmul(
# np.array([[np.cos(rot_angles[i]), np.sin(rot_angles[i])],
# [-np.sin(rot_angles[i]), np.cos(rot_angles[i])]]),
# np.expand_dims(frustum[:, [0,2]], 2)), 2)
# frustum[:, 3] = np.amax(logits[i], axis = 1)
#self.publish_pointcloud_from_array(frustum, self.pc_pub, header = pointcloud_upper.header)
det_msg = detection3d_with_feature()
det_msg.header.frame_id = 'occam'
det_msg.header.stamp = features_3d.header.stamp
det_msg.valid = True if valid_3d[i] != -1 else False
det_msg.frame_det_id = i
if det_msg.valid:
det_msg.x = box[0]
det_msg.y = box[1]
det_msg.z = box[2]
det_msg.l = box[3]
det_msg.h = box[4]
det_msg.w = box[5]
det_msg.theta = box[6]
det_msg.feature = feature
features_3d.detection3d_with_features.append(det_msg)
pose_msg = Pose()
marker_msg = Marker()
marker_msg.header.stamp = pointcloud_lower.header.stamp
marker_msg.header.frame_id = 'occam'
marker_msg.action = 0
marker_msg.id = i
marker_msg.lifetime = rospy.Duration(0.2)
marker_msg.type = 1
marker_msg.scale = Vector3(box[3], box[4], box[5])
pose_msg.position.x = det_msg.x
pose_msg.position.y = det_msg.y - det_msg.h / 2
pose_msg.position.z = det_msg.z
marker_msg.pose = pose_msg
marker_msg.color = ColorRGBA(g=1, a=0.5)
boxes_3d_markers.markers.append(marker_msg)
else:
det_msg.y = -1
det_msg.x = -1
det_msg.z = -1
det_msg.l = -1
det_msg.w = -1
det_msg.h = -1
det_msg.theta = -1
det_msg.feature = [-1]
features_3d.detection3d_with_features.append(det_msg)
self.marker_box_pub.publish(boxes_3d_markers)
self.feature_3d_pub.publish(features_3d)
def visualize_waypoints(current_robot_pose, current_camera_pose,
marker_id, last_robot_pose=None):
"""
Publishes marker to visualize robot path
"""
waypoints = rospy.get_param("/scene_exploration_sm/waypoints")
publisher = rospy.Publisher(waypoints, Marker, queue_size=10)
points = []
rospy.logdebug("last_robot_pose is: " + str(last_robot_pose))
if last_robot_pose is not None:
points.append(Point(last_robot_pose.position.x,
last_robot_pose.position.y,
0))
points.append(Point(current_robot_pose.position.x,
current_robot_pose.position.y,
0))
if current_camera_pose is not None:
z_pose = current_camera_pose.position.z
else:
z_pose = 1.35
center = Point(current_robot_pose.position.x,
current_robot_pose.position.y,
z_pose / 2.0)
pose_marker = Marker()
pose_marker.header.stamp = rospy.Time.now()
pose_marker.header.frame_id = '/map'
pose_marker.ns = 'waypoints_cyl'
pose_marker.type = Marker.CYLINDER
pose_marker.id = marker_id + 2
pose_marker.action = Marker.ADD
pose_marker.scale = Vector3(0.05, 0.05, z_pose)
pose_marker.color = ColorRGBA(0, 0, 1, 1)
pose_marker.lifetime = rospy.Duration()
pose_marker.pose.position = center
# Only on the first try we have to wait for the publisher,
# next times we know the last pose and this won't be executed
if last_robot_pose is None:
rospy.loginfo("Sleeping till waypoint publisher is ready")
rospy.sleep(1)
publisher.publish(pose_marker)
marker = Marker()
marker.header.stamp = rospy.Time.now()
marker.header.frame_id = '/map'
marker.ns = 'waypoints_lines'
marker.type = Marker.LINE_LIST
marker.id = marker_id
marker.action = Marker.ADD
marker.scale = Vector3(0.02, 0.1, 0.1)
marker.color = ColorRGBA(0, 1, 1, 1)
marker.lifetime = rospy.Duration()
marker.points = points
publisher.publish(marker)
if current_camera_pose is not None:
arrow = Marker()
arrow.header.stamp = rospy.Time.now()
arrow.header.frame_id = '/map'
arrow.ns = 'waypoints_arrows'
arrow.pose = current_camera_pose
arrow.type = Marker.ARROW
arrow.id = marker_id + 1
arrow.action = Marker.ADD
arrow.scale = Vector3(0.5, 0.02, 0.02)
arrow.color = ColorRGBA(1, 1, 0, 1)
arrow.lifetime = rospy.Duration()
publisher.publish(arrow)
def callback(data):
xAxis = (1, 0, 0)
zAxis = (0, 0, 1)
errorFlag = 0
# joint 1
a, alpha, d, theta = params['i1']
a, alpha, d, theta = float(a), float(alpha), float(d), float(theta)
d = rospy.get_param("d1", d)
if (data.position[0] < -d) or (data.position[0] > 0):
errorFlag = 1
Tx = translation_matrix((a, 0, 0))
Rx = rotation_matrix(alpha, xAxis)
Tz = translation_matrix((0, 0, data.position[0] + d))
Rz = rotation_matrix(theta, zAxis)
T_1 = concatenate_matrices(Tx, Rx, Tz, Rz)
# joint 2
a, alpha, d, theta = params['i2']
a, alpha, d, theta = float(a), float(alpha), float(d), float(theta)
d = rospy.get_param("d2", d)
if (data.position[1] < -d) or (data.position[1] > 0):
errorFlag = 2
Tx = translation_matrix((a, 0, 0))
Rx = rotation_matrix(alpha, xAxis)
Tz = translation_matrix((0, 0, data.position[1] + d))
Rz = rotation_matrix(theta, zAxis)
T_2 = concatenate_matrices(Tx, Rx, Tz, Rz)
# joint 3
a, alpha, d, theta = params['i3']
a, alpha, d, theta = float(a), float(alpha), float(d), float(theta)
d = rospy.get_param("d3", d)
if (data.position[2] < -d) or (data.position[2] > 0):
errorFlag = 3
Tx = translation_matrix((a, 0, 0))
Rx = rotation_matrix(alpha, xAxis)
Tz = translation_matrix((0, 0, data.position[2] + d))
Rz = rotation_matrix(theta, zAxis)
T_3 = concatenate_matrices(Tx, Rx, Tz, Rz)
# final touches
final_matrix = concatenate_matrices(T_1, T_2, T_3)
x, y, z = translation_from_matrix(final_matrix)
qx, qy, qz, qw = quaternion_from_matrix(final_matrix)
poseStamped = PoseStamped()
poseStamped.header.frame_id = 'base_link'
poseStamped.header.stamp = rospy.Time.now()
poseStamped.pose.position.x = x
poseStamped.pose.position.y = y
poseStamped.pose.position.z = z
poseStamped.pose.orientation.x = qx
poseStamped.pose.orientation.y = qy
poseStamped.pose.orientation.z = qz
poseStamped.pose.orientation.w = qw
marker = Marker()
marker.header = poseStamped.header
marker.type = marker.CUBE
marker.action = marker.ADD
marker.pose = poseStamped.pose
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.a = 1
marker.color.r = 0
marker.color.g = 1
marker.color.b = 0
if errorFlag == 0:
pubP.publish(poseStamped)
pubM.publish(marker)
else:
print('Error: joint{} out of bounds'.format(errorFlag))
def publish(self, timestamp):
pose = PoseStamped()
pose.header.frame_id = self.target_frame
pose.header.stamp = timestamp
pose.pose.position.x = self.X[0, 0]
pose.pose.position.y = self.X[1, 0]
pose.pose.position.z = 0.0
Q = quaternion_from_euler(0, 0, self.X[2, 0])
pose.pose.orientation.x = Q[0]
pose.pose.orientation.y = Q[1]
pose.pose.orientation.z = Q[2]
pose.pose.orientation.w = Q[3]
self.pose_pub.publish(pose)
ma = MarkerArray()
marker = Marker()
marker.header = pose.header
marker.ns = "kf_uncertainty"
marker.id = 5000
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.pose.position.z = -0.1
print 'diag(P): ' + str(numpy.diag(self.P))
try:
marker.scale.x = 3 * sqrt(self.P[0, 0])
marker.scale.y = 3 * sqrt(self.P[1, 1])
except:
print 'self.P\n' + repr(self.P)
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 1.0
ma.markers.append(marker)
for id in self.idx.iterkeys():
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
l = self.idx[id]
marker.pose.position.x = self.X[l, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = -0.1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.scale.x = 0.2
#3*sqrt(self.P[l,l])
marker.scale.y = 0.2
#3*sqrt(self.P[l+1,l+1]);
marker.scale.z = 0.1
marker.color.a = 1.0
marker.color.r = 0.25
marker.color.g = 0
marker.color.b = 0.25
marker.lifetime.secs = 3.0
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = self.target_frame
marker.ns = "landmark_kf"
marker.id = 1000 + id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = self.X[l + 0, 0]
marker.pose.position.y = self.X[l + 1, 0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 1.0
marker.lifetime.secs = 3.0
ma.markers.append(marker)
self.marker_pub.publish(ma)
def send_goal(self, x, y, theta):
#current navigation goal coordinates
curr_nav_x = x
curr_nav_y = y
curr_nav_theta = theta
self.goal_received = False
# Creates a goal to send to the action server.
pose = geometry_msgs.msg.Pose()
pose.position.x = x
pose.position.y = y
q = quaternion_from_euler(0, 0, theta)
pose.orientation = geometry_msgs.msg.Quaternion(*q)
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
#Creates an rviz marker for the goal
robot_marker = Marker()
robot_marker.header.frame_id = 'map'
robot_marker.header.stamp = rospy.Time.now()
robot_marker.action = Marker.ADD
robot_marker.scale.x = 0.2
robot_marker.scale.y = 0.2
robot_marker.scale.z = 0.2
robot_marker.pose = pose
robot_marker.color.a = 1.0
robot_marker.color.b = 0.0
robot_marker.type = Marker.CUBE
robot_marker.color.r = 1.0
robot_marker.color.g = 0.0
# Sends the goal to the action server.
rospy.loginfo('Sending goal to action server: %s', goal)
self.move_base_client.send_goal(goal,
feedback_cb=self.feedback_callback)
# state_result will give the FINAL STATE. Will be 1 when Active, and 2 if NO ERROR, 3 If Any Warning, and 3 if ERROR
state_result = self.move_base_client.get_state()
rate = rospy.Rate(10)
rospy.loginfo("state_result: " + str(state_result))
# We create some constants with the corresponing vaules from the SimpleGoalState class
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
while state_result < DONE:
rospy.loginfo("Checking for alien while performing navigation....")
#check if a goal is being published and it is not the same as the current one, then cancel navigation and start new one
if self.goal_received == True and curr_nav_x != x and curr_nav_y != y:
self.move_base_client.cancel_goal()
print "Cancelling goal..."
self.send_goal(x, y, theta)
rospy.loginfo(
"Sending goal to main algorithm -- x:%f, y:%f, theta:%f",
x, y, theta)
#publish rviz marker for goal
self.pub_marker.publish(robot_marker)
rate.sleep()
state_result = self.move_base_client.get_state()
rospy.loginfo("state_result: " + str(state_result))
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == ERROR:
rospy.logerr("Something went wrong in the Server Side")
if state_result == WARN:
rospy.logwarn("There is a warning in the Server Side")
else:
# Waits for the server to finish performing the action.
print 'Waiting for result...'
self.move_base_client.wait_for_result()
rospy.loginfo("Goal Reached! Success!")
self.marker_goal = False
return self.move_base_client.get_result()
time.sleep(10)
send_goal(0, 0, 0)
def publish(self, target_frame, timestamp):
pose = PoseStamped()
pose.header.frame_id = target_frame
pose.header.stamp = timestamp
pose.pose.position.x = self.X[0,0]
pose.pose.position.y = self.X[1,0]
pose.pose.position.z = 0.0
Q = tf.transformations.quaternion_from_euler(0, 0, self.X[2,0])
pose.pose.orientation.x = Q[0]
pose.pose.orientation.y = Q[1]
pose.pose.orientation.z = Q[2]
pose.pose.orientation.w = Q[3]
self.pose_pub.publish(pose)
ma = MarkerArray()
marker = Marker()
marker.header = pose.header
marker.ns = "kf_uncertainty"
marker.id = 5000
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
marker.pose = pose.pose
marker.pose.position.z = -0.1
marker.scale.x = 3*sqrt(self.P[0,0])
marker.scale.y = 3*sqrt(self.P[1,1]);
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = 0.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
ma.markers.append(marker)
for id in self.idx.iterkeys():
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = id
marker.type = Marker.CYLINDER
marker.action = Marker.ADD
l = self.idx[id]
marker.pose.position.x = self.X[l,0]
marker.pose.position.y = self.X[l+1,0]
marker.pose.position.z = -0.1
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
#default assumes x and y on diagonal for covariance,
marker.scale.x = 3*sqrt(self.P[l,l])
marker.scale.y = 3*sqrt(self.P[l+1,l+1]);
marker.scale.z = 0.1;
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 0.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
marker = Marker()
marker.header.stamp = timestamp
marker.header.frame_id = target_frame
marker.ns = "landmark_kf"
marker.id = 1000+id
marker.type = Marker.TEXT_VIEW_FACING
marker.action = Marker.ADD
marker.pose.position.x = self.X[l+0,0]
marker.pose.position.y = self.X[l+1,0]
marker.pose.position.z = 1.0
marker.pose.orientation.x = 0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 1
marker.pose.orientation.w = 0
marker.text = str(id)
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 0.2
marker.color.a = 1.0;
marker.color.r = 1.0;
marker.color.g = 1.0;
marker.color.b = 1.0;
marker.lifetime.secs=3.0;
ma.markers.append(marker)
self.marker_pub.publish(ma)
def visualize(model):
rospy.init_node('symb_model')
rospy.sleep(0.5)
used_links = []
for idx in range(1, 8):
used_links.append('right_arm_{}_link'.format(idx))
used_links.append('left_arm_{}_link'.format(idx))
used_links.append('head_pan_link')
used_links.append('head_tilt_link_dummy')
used_links.append('head_kinect_rgb_optical_frame')
q_list = [
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1.5, 0, 1.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[1.5, 0, -1, 0, -1, 0, -0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[-1.5, 0, -1, 0, -1, 0, -0.5, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
[
0, 0.2, -1, 0.2, -0.1, 0.5, -0.4, 0.2, 0.5, 0.3, -0.4, -0.1, -0.6,
1, 0.2, -0.5, 1.2
],
[
1.5, 0.2, -1, 0.2, -0.1, 0.5, -0.4, 0.2, 0.5, 0.3, -0.4, -0.1,
-0.6, 1, 0.2, -0.5, 1.2
],
[0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
]
pub_marker = rospy.Publisher('symb_model_vis',
MarkerArray,
queue_size=1000)
q_prev = q_list[0]
dq_prev = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
for trj_idx in range(len(q_list) - 1):
if rospy.is_shutdown():
break
q1 = q_list[trj_idx]
q2 = q_list[trj_idx + 1]
for t in np.linspace(0, 1, 100, endpoint=False):
q = []
for q1v, q2v in zip(q1, q2):
q.append(q1v * (1.0 - t) + q2v * t)
dq = []
for q_idx in range(len(q)):
dq.append(q[q_idx] - q_prev[q_idx])
ddq = []
for q_idx in range(len(dq)):
ddq.append(dq[q_idx] - dq_prev[q_idx])
m = MarkerArray()
m_id = 0
for link_name in used_links:
#func_R, func_P = model.getFkFunc(link_name)
func_T = model.getFkFunc(link_name)
func_w, func_e, func_A = model.getFdFunc(link_name)
#sP = func_P(*q)
#sR = func_R(*q)
sT = func_T(*q)
A = func_A(*(q + dq + ddq))
marker = Marker()
marker.header.frame_id = 'torso_base'
marker.header.stamp = rospy.Time.now()
marker.ns = 'symb_model_vis'
marker.id = m_id
marker.type = Marker.CUBE
marker.action = Marker.ADD
#T = PyKDL.Frame(PyKDL.Rotation(sR[0,0], sR[0,1], sR[0,2], sR[1,0], sR[1,1], sR[1,2],
# sR[2,0], sR[2,1], sR[2,2]), PyKDL.Vector(sP[0], sP[1], sP[2]))
T = PyKDL.Frame(
PyKDL.Rotation(sT[0, 0], sT[0, 1], sT[0, 2], sT[1, 0],
sT[1, 1], sT[1, 2], sT[2, 0], sT[2, 1],
sT[2, 2]),
PyKDL.Vector(sT[0, 3], sT[1, 3], sT[2, 3]))
point = T.p
qt = T.M.GetQuaternion()
marker.pose = Pose(Point(point.x(), point.y(), point.z()),
Quaternion(qt[0], qt[1], qt[2], qt[3]))
scale = 0.02
marker.scale = Vector3(scale, scale, scale)
marker.color = ColorRGBA(1, 0, 0, 1)
m.markers.append(marker)
m_id = m_id + 1
marker = Marker()
marker.header.frame_id = 'torso_base'
marker.header.stamp = rospy.Time.now()
marker.ns = 'symb_model_vis'
marker.id = m_id
marker.type = Marker.ARROW
marker.action = Marker.ADD
scale = 0.02
marker.scale = Vector3(scale, 2.0 * scale, 0)
marker.pose = Pose(Point(point.x(), point.y(), point.z()),
Quaternion(qt[0], qt[1], qt[2], qt[3]))
marker.color = ColorRGBA(0, 1, 0, 1)
marker.points.append(Point(0, 0, 0))
A_scale = 1000.0
#print link_name, A_scale*A[0],A_scale*A[1],A_scale*A[2]
marker.points.append(
Point(A_scale * A[0], A_scale * A[1], A_scale * A[2]))
m.markers.append(marker)
m_id = m_id + 1
pub_marker.publish(m)
q_prev = q
dq_prev = dq
rospy.sleep(0.01)
def generate_target():
"""
Main function. Publishes the target at a constant frame rate.
"""
# Create the simulated object
object = SimulatedObject()
# Init the node
rospy.init_node('generate_target', anonymous=True)
# Get ROS params
x_value = rospy.get_param("~x_value", default=1.5)
radius = rospy.get_param("~radius", default=0.1)
publish_rate = rospy.get_param("~publish_rate", default=30)
object.x = x_value
# Define publishers
vector_pub = rospy.Publisher('/target', Target, queue_size=5)
marker_pub = rospy.Publisher('/target_marker', Marker, queue_size=5)
# Publish the target at a constant ratetarget
rate = rospy.Rate(publish_rate)
while not rospy.is_shutdown():
# publish the location of the target as a Vector3Stamped
pose_msg = PoseStamped()
pose_msg.header.stamp = rospy.Time.now()
pose_msg.header.frame_id = object.frame
pose_msg.pose.position.x = object.x
pose_msg.pose.position.y = object.center_y + np.sin(
object.angle) * object.radius
pose_msg.pose.position.z = object.center_z + np.cos(
object.angle) * object.radius
pose_msg.pose.orientation.w = 1.0
target_msg = Target()
target_msg.pose = pose_msg
target_msg.radius = radius
vector_pub.publish(target_msg)
# publish a marker to visualize the target in RViz
marker_msg = Marker()
marker_msg.header = pose_msg.header
marker_msg.action = Marker.ADD
marker_msg.color.a = 0.5
marker_msg.color.r = 1.0
marker_msg.lifetime = rospy.Duration(1.0)
marker_msg.id = 0
marker_msg.ns = "target"
marker_msg.type = Marker.SPHERE
marker_msg.pose = pose_msg.pose
marker_msg.scale.x = 2.0 * radius
marker_msg.scale.y = 2.0 * radius
marker_msg.scale.z = 2.0 * radius
marker_pub.publish(marker_msg)
# update the simulated object state
object.step()
# sleep to keep the desired publishing rate
rate.sleep()
def _visualize_waypoints(self, switched):
if not switched and not self._first_draw:
return
if self._first_draw:
for wp in range(1, self.nwp):
mk = Marker()
mk.header.seq += 1
mk.header.frame_id = "map"
mk.header.stamp = rospy.Time.now()
mk.ns = "waypoints"
mk.id = wp
mk.type = Marker.CYLINDER
D = np.sqrt(self.controller.R2)
mk.scale.x = D
mk.scale.y = D
mk.scale.z = 2. # height [m]
mk.action = Marker.ADD
mk.pose = geometry_msgs.msg.Pose()
mk.pose.position.x = self.wp[wp, 0]
mk.pose.position.y = self.wp[wp, 1]
mk.pose.orientation.w = 1
mk.lifetime = rospy.Duration()
mk.color.a = .3
mk.color.r = 0.
mk.color.g = 0.
mk.color.b = 0.
if wp == self.cwp:
mk.color.g = 1.
else:
mk.color.r = 1.
self._wps_publisher.publish(mk)
else:
for wp in [self.cwp - 1, self.cwp]:
mk = Marker()
mk.header.seq += 1
mk.header.frame_id = "map"
mk.header.stamp = rospy.Time.now()
mk.ns = "waypoints"
mk.id = wp
mk.type = Marker.CYLINDER
D = np.sqrt(self.controller.R2) * 2
mk.scale.x = D
mk.scale.y = D
mk.scale.z = 2. # height [m]
mk.action = Marker.ADD
mk.pose = geometry_msgs.msg.Pose()
mk.pose.position.x = self.wp[wp, 0]
mk.pose.position.y = self.wp[wp, 1]
mk.pose.orientation.w = 1
mk.lifetime = rospy.Duration()
mk.color.a = .3
mk.color.r = 0.
mk.color.g = 0.
mk.color.b = 0.
if wp == self.cwp:
mk.color.g = 1.
else:
mk.color.r = 1.
self._wps_publisher.publish(mk)
self._first_draw = True
m3.header.frame_id = "map"
m3.ns = "demo"
m3.id = 3
m3.type = Marker.SPHERE
m3.action = Marker.ADD
m3.scale.x = 0.1
m3.scale.y = 0.1
m3.scale.z = 0.1
m3.color.a = 1
m3.color.r = 1
m3.color.g = 1
m3.color.b = 0
'''
while not rospy.is_shutdown():
m1.pose = to1.get_pose().pose
m1.header.stamp = rospy.Time.now()
pub.publish(m1)
m2.pose = to1.get_query_point_pose("marker_point").pose
m2.header.stamp = rospy.Time.now()
pub.publish(m2)
m3.pose = to1.get_query_point_pose("test_point").pose
m3.header.stamp = rospy.Time.now()
pub.publish(m3)
m4.pose = to2.get_query_point_pose("paper_center").pose
pub.publish(m4)
def super_obs(self):
self.no_obs()
pose_stamped = geometry_msgs.msg.PoseStamped()
pose_stamped.header.frame_id = "world_link"
pose_stamped.header.stamp = rospy.Time(0)
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
self.scene.add_box("obs1", pose_stamped,(0.1,0.1,0.8))
self.scene.add_box("box1", pose_stamped,(0.05,0.05,0.75))
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
self.scene.add_box("obs2", pose_stamped,(0.1,0.5,0.1))
self.scene.add_box("box2", pose_stamped,(0.05,0.45,0.05))
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, -0.25, 0.4)) )
self.scene.add_box("obs3", pose_stamped,(0.1,0.1,0.8))
self.scene.add_box("box3", pose_stamped,(0.05,0.05,0.75))
pose_stamped.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.3)) )
self.scene.add_box("obs4", pose_stamped,(0.1,0.5,0.1))
self.scene.add_box("box4", pose_stamped,(0.05,0.45,0.05))
obs = MarkerArray()
obj1 = Marker()
obj1.header.frame_id = "world_link"
obj1.header.stamp = rospy.Time(0)
obj1.ns = 'obstacle'
obj1.id = 1
obj1.type = Marker.CUBE
obj1.action = Marker.ADD
obj1.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.25, 0.4)) )
obj1.scale.x = 0.1
obj1.scale.y = 0.1
obj1.scale.z = 0.8
obj1.color.r = 0.0
obj1.color.g = 1.0
obj1.color.b = 0.0
obj1.color.a = 1.0
obs.markers.append(obj1)
obj2 = Marker()
obj2.header.frame_id = "world_link"
obj2.header.stamp = rospy.Time(0)
obj2.ns = 'obstacle'
obj2.id = 2
obj2.type = Marker.CUBE
obj2.action = Marker.ADD
obj2.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.8)) )
obj2.scale.x = 0.1
obj2.scale.y = 0.5
obj2.scale.z = 0.1
obj2.color.r = 0.0
obj2.color.g = 1.0
obj2.color.b = 0.0
obj2.color.a = 1.0
obs.markers.append(obj2)
obj3 = Marker()
obj3.header.frame_id = "world_link"
obj3.header.stamp = rospy.Time(0)
obj3.ns = 'obstacle'
obj3.id = 3
obj3.type = Marker.CUBE
obj3.action = Marker.ADD
obj3.pose = convert_to_message( tf.transformations.translation_matrix((0.5, -0.25, 0.4)) )
obj3.scale.x = 0.1
obj3.scale.y = 0.1
obj3.scale.z = 0.8
obj3.color.r = 0.0
obj3.color.g = 1.0
obj3.color.b = 0.0
obj3.color.a = 1.0
obs.markers.append(obj3)
obj4 = Marker()
obj4.header.frame_id = "world_link"
obj4.header.stamp = rospy.Time(0)
obj4.ns = 'obstacle'
obj4.id = 4
obj4.type = Marker.CUBE
obj4.action = Marker.ADD
obj4.pose = convert_to_message( tf.transformations.translation_matrix((0.5, 0.0, 0.3)) )
obj4.scale.x = 0.1
obj4.scale.y = 0.5
obj4.scale.z = 0.1
obj4.color.r = 0.0
obj4.color.g = 1.0
obj4.color.b = 0.0
obj4.color.a = 1.0
obs.markers.append(obj4)
self.marker_pub.publish(obs)
def draw_curves(msg):
global curve_array, right_curve, left_curve
r_lim, l_lim = msg.data[0], msg.data[2]
curve_array.markers = []
right_curve.points = []
for point in curve_coords[curve_coords[:, 0] <= -r_lim]:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = -point[3]
curve_pt.z = 0
right_curve.points.append(curve_pt)
curve_array.markers.append(right_curve)
left_curve.points = []
for point in curve_coords[curve_coords[:, 0] <= l_lim]:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = point[3]
curve_pt.z = 0
left_curve.points.append(curve_pt)
curve_array.markers.append(left_curve)
ref_pose = Pose()
ref_pose.position.x, ref_pose.position.y, ref_pose.position.z = 0.0, 0.0, 0.0
ref_quat = quaternion_from_euler(0.0, 0.0, 0.0)
ref_pose.orientation.x = ref_quat[0]
ref_pose.orientation.y = ref_quat[1]
ref_pose.orientation.z = ref_quat[2]
ref_pose.orientation.w = ref_quat[3]
right_out = Marker()
right_out.header.frame_id = "front_axle_center"
right_out.ns = "curves"
right_out.id = 2
right_out.type = 4
right_out.action = 0
right_out.pose = ref_pose
right_out.scale.x = 0.01
right_out.color.a = 1.0
right_out.color.r = 0.0
right_out.color.g = 0.0
right_out.color.b = 1.0
right_out.lifetime.secs = 0.1
right_out.points = []
curve_right_out = curve_coords_out[curve_coords[:, 0] < -r_lim]
for point in curve_right_out[curve_right_out[:, 0] > 0.0]:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = -point[3]
curve_pt.z = 0
right_out.points.append(curve_pt)
curve_array.markers.append(right_out)
right_in = Marker()
right_in.header.frame_id = "front_axle_center"
right_in.ns = "curves"
right_in.id = 3
right_in.type = 4
right_in.action = 0
right_in.pose = ref_pose
right_in.scale.x = 0.01
right_in.color.a = 1.0
right_in.color.r = 0.0
right_in.color.g = 0.0
right_in.color.b = 1.0
right_in.lifetime.secs = 0.1
right_in.points = []
curve_right_in = curve_coords_in[curve_coords[:, 0] < -r_lim]
for point in curve_right_in:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = -point[3]
curve_pt.z = 0
right_in.points.append(curve_pt)
curve_array.markers.append(right_in)
left_out = Marker()
left_out.header.frame_id = "front_axle_center"
left_out.ns = "curves"
left_out.id = 4
left_out.type = 4
left_out.action = 0
left_out.pose = ref_pose
left_out.scale.x = 0.01
left_out.color.a = 1.0
left_out.color.r = 0.0
left_out.color.g = 0.0
left_out.color.b = 1.0
left_out.lifetime.secs = 0.1
left_out.points = []
curve_left_out = curve_coords_out[curve_coords[:, 0] < l_lim]
for point in curve_left_out[curve_left_out[:, 0] > 0.0]:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = point[3]
curve_pt.z = 0
left_out.points.append(curve_pt)
curve_array.markers.append(left_out)
left_in = Marker()
left_in.header.frame_id = "front_axle_center"
left_in.ns = "curves"
left_in.id = 5
left_in.type = 4
left_in.action = 0
left_in.pose = ref_pose
left_in.scale.x = 0.01
left_in.color.a = 1.0
left_in.color.r = 0.0
left_in.color.g = 0.0
left_in.color.b = 1.0
left_in.lifetime.secs = 0.1
left_in.points = []
curve_left_in = curve_coords_in[curve_coords[:, 0] < l_lim]
for point in curve_left_in:
curve_pt = Point()
curve_pt.x = point[2]
curve_pt.y = point[3]
curve_pt.z = 0
left_in.points.append(curve_pt)
curve_array.markers.append(left_in)
curve_markers_pub.publish(curve_array)
rospy.init_node('markerstuff')
pub = rospy.Publisher("colormap", Marker, queue_size=10)
mk = Marker()
mk.header.seq = 0
mk.header.frame_id = 'map'
mk.header.stamp = rospy.Time.now()
mk.ns = 'colormap'
mk.id = 0
mk.type = Marker.MESH_RESOURCE
mk.mesh_resource = "package://asv_simulator/meshes/polyHazardsCorrect.stl"
mk.action = Marker.ADD
mk.mesh_use_embedded_materials = True
mk.pose = Pose()
mk.pose.position.x = 0.0
mk.pose.position.y = 0.
mk.pose.position.z = 0.
q = euler2quat(0., 0., 0.)
mk.pose.orientation.x = q[0]
mk.pose.orientation.y = q[1]
mk.pose.orientation.z = q[2]
mk.pose.orientation.w = q[3]
mk.scale.x = 1000.
mk.scale.y = 1000.
mk.scale.z = 1000.
def send_goal(self, x, y, theta):
#current navigation goal coordinates
curr_nav_x = x
curr_nav_y = y
curr_nav_theta = theta
self.goal_received = False
# Creates a goal to send to the action server.
pose = geometry_msgs.msg.Pose()
pose.position.x = x
pose.position.y = y
q = quaternion_from_euler(0, 0, theta)
pose.orientation = geometry_msgs.msg.Quaternion(*q)
goal = MoveBaseGoal()
goal.target_pose.pose = pose
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
#Creates an rviz marker for the goal
robot_marker = Marker()
robot_marker.header.frame_id = 'map'
robot_marker.header.stamp = rospy.Time.now()
robot_marker.action = Marker.ADD
robot_marker.scale.x = 0.9
robot_marker.scale.y = 0.05
robot_marker.scale.z = 0.1
robot_marker.pose = pose
robot_marker.color.a = 1.0
robot_marker.color.b = 0.0
robot_marker.type = Marker.ARROW
robot_marker.color.r = 1.0
robot_marker.color.g = 0.0
# Sends the goal to the action server.
rospy.loginfo('Sending goal to action server: %s', goal)
self.move_base_client.send_goal(goal,
feedback_cb=self.feedback_callback)
# state_result will give the FINAL STATE. Will be 1 when Active, and 2 if NO ERROR, 3 If Any Warning, and 3 if ERROR
state_result = self.move_base_client.get_state()
rate = rospy.Rate(10)
rospy.loginfo("state_result: " + str(state_result))
# We create some constants with the corresponing vaules from the SimpleGoalState class
PENDING = 0
ACTIVE = 1
DONE = 2
WARN = 3
ERROR = 4
while state_result < DONE:
rospy.loginfo("Checking for alien while performing navigation....")
#check if a goal is being published and it is not the same as the current one, then cancel navigation and start new one
if self.goal_received == True and curr_nav_x != x and curr_nav_y != y:
self.move_base_client.cancel_goal()
print "Cancelling goal..."
self.marker_goal = False
self.send_goal(x, y, theta)
rospy.loginfo(
"Sending goal to main algorithm -- x:%f, y:%f, theta:%f",
x, y, theta)
#publish rviz marker for goal
self.pub_marker.publish(robot_marker)
rate.sleep()
state_result = self.move_base_client.get_state()
rospy.loginfo("state_result: " + str(state_result))
rospy.loginfo("[Result] State: " + str(state_result))
if state_result == ERROR:
rospy.logerr("Something went wrong in the Server Side")
if state_result == WARN:
rospy.logwarn("There is a warning in the Server Side")
else:
# Waits for the server to finish performing the action.
print 'Waiting for result...'
self.move_base_client.wait_for_result()
rospy.loginfo("Goal Reached! Success!")
move = Twist()
distance_travelled = 0.0
while not rospy.is_shutdown():
initial_x = 0.0
initial_y = 0.0
a = "Dock"
b = "undock"
c = "continue"
print "What do you want to do?"
print "A) Dock"
print "B) Undock"
print "C) Set new navigation goal"
result = input()
self.alien_detected = False
if result == "dock" or result == "Dock" or result == "A" or result == "a":
initial_x = self.x_curr
initial_y = self.y_curr
move.linear.x = 0.3
while self.alien_detected == False:
print self.alien_detected
print "one"
self.publisher.publish(move)
rate.sleep()
print "two"
move.linear.x = 0
distance_travelled = sqrt(
pow(self.x_curr - initial_x, 2) +
pow(self.y_curr - initial_y, 2))
while not rospy.is_shutdown():
connections = self.publisher.get_num_connections()
if connections > 0:
self.publisher.publish(move)
break
else:
rate.sleep()
elif result == "undock" or result == "Undock" or result == "B" or result == "b":
initial_theta = self.theta_curr
move.angular.z = 1.0
difference = 0.0
while difference < pi:
print "enter"
if initial_theta > 0 and self.theta_curr <= 0:
self.theta_curr = self.theta_curr + 2 * pi
difference = fabs(self.theta_curr - initial_theta)
print initial_theta * 180 / pi
print self.theta_curr * 180 / pi
print difference * 180 / pi
if difference > 2.97:
move.angular.z = 0.03
self.publisher.publish(move)
rate.sleep()
move.angular.z = 0.0
while not rospy.is_shutdown():
connections = self.publisher.get_num_connections()
if connections > 0:
self.publisher.publish(move)
break
else:
rate.sleep()
move.linear.x = 0.3
initial_x = self.x_curr
initial_y = self.y_curr
distance_back = 0.0
while distance_back < distance_travelled:
print distance_back
print distance_travelled
distance_back = sqrt(
pow(self.x_curr - initial_x, 2) +
pow(self.y_curr - initial_y, 2))
self.publisher.publish(move)
rate.sleep()
move.linear.x = 0
while not rospy.is_shutdown():
connections = self.publisher.get_num_connections()
if connections > 0:
self.publisher.publish(move)
break
else:
rate.sleep()
elif result == "continue" or result == "Continue" or result == "C" or result == "c":
break
else:
print "Wrong command"
return self.move_base_client.get_result()
def detect(self):
obj_info = []
# make sure the robot's head is pointing in the right direction
if not self.point_head('center'): return None
# segment point cloud and get point clusters corresponding to objects on the floor
seg_res = self.segment_objects()
if seg_res is None: return None
for idx, cluster in enumerate(seg_res.clusters):
bbox_response = self.find_bounding_box_srv(cluster)
if bbox_response.error_code != FindClusterBoundingBoxResponse.SUCCESS:
rospy.logwarn('unable to find for cluster %d bounding box' %
idx)
continue
# transform bounding box pose to another reference frame (arm base by default)
bbox_response.pose.header.stamp = rospy.Time(0)
bbox_pose_stamped = self.listener.transformPose(
self.reference_frame, bbox_response.pose)
bbox_pose = bbox_pose_stamped.pose
bbox_dims = bbox_response.box_dims
# create a bounding box visualization marker that's valid for 10 seconds
marker = Marker()
marker.header.frame_id = self.reference_frame
marker.header.stamp = rospy.Time.now()
marker.ns = 'floor_object_bboxes'
marker.id = idx
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.pose = bbox_pose
marker.scale = bbox_dims
marker.color = ColorRGBA(1.0, 0.0, 0.0, 0.5)
marker.lifetime = rospy.Duration(10)
# calculate 3D distance to cluster from the reference frame origin
# if the object is behind the arm make sure we know about it
dist = math.sqrt(bbox_pose.position.x**2 +
bbox_pose.position.y**2 + bbox_pose.position.z**2)
dist = math.copysign(dist, bbox_pose.position.x)
# calculate angle to object
ang = math.atan2(bbox_pose.position.y, bbox_pose.position.x)
# calculate object bounding box volume
vol = bbox_dims.x * bbox_dims.y * bbox_dims.z
rospy.loginfo(
'[%d] dist = %.3f; ang = %.3f; vol = %f; pos = %s; bbox_dims = %s'
% (idx, dist, math.degrees(ang), vol, str(
bbox_pose.position).replace(
'\n', ' '), str(bbox_dims).replace('\n', ' ')))
info = (idx, dist, ang, vol, bbox_response)
if self.within_limits(info):
obj_info.append(info)
# change color to green for objects reachable from robot's current position
marker.color.r = 0.0
marker.color.g = 1.0
self.marker_pub.publish(marker)
else:
rospy.loginfo(
'cluster %d outside of specified limits, skipping' % idx)
rospy.loginfo('there are %d clusters after filtering' % len(obj_info))
obj_info_sorted = sorted(obj_info, key=itemgetter(1))
for idx, inf in enumerate(obj_info_sorted):
dist = inf[1]
ang = inf[2]
rospy.loginfo('[%d] dist = %.3f; ang = %.3f' %
(idx, dist, math.degrees(ang)))
if not obj_info_sorted: return None
else:
return {
'segmentation_result': seg_res,
'cluster_information': obj_info_sorted
}
def get_pose(self, e, c1, c2, depth_image, bgr_image):
point1X = c1[0][0][0]
#print(point1X)
point1Y = c1[0][0][1]
#print(point1Y)
point2X = c1[int(len(c1) / 4)][0][0]
point2Y = c1[int(len(c1) / 4)][0][1]
point3X = c1[int(len(c1) / 4) * 2][0][0]
#print(point3X)
point3Y = c1[int(len(c1) / 4) * 2][0][1]
#print(point3Y)
point4X = c1[int(len(c1) / 4) * 3][0][0]
point4Y = c1[int(len(c1) / 4) * 3][0][1]
min1 = 1000000000
closest1 = c1[0]
min2 = 1000000000
closest2 = c1[0]
min3 = 1000000000
closest3 = c1[0]
min4 = 1000000000
closest4 = c1[0]
for i in range(0, len(c2)) :
dist1 = np.sqrt((c2[i][0][0]-point1X) ** 2 + (c2[i][0][1] - point1Y) ** 2)
if dist1 < min1 :
min1 = dist1
closest1 = c2[i][0]
dist2 = np.sqrt((c2[i][0][0]-point2X) ** 2 + (c2[i][0][1] - point2Y) ** 2)
if dist2 < min2 :
min2 = dist2
closest2 = c2[i][0]
dist3 = np.sqrt((c2[i][0][0]-point3X) ** 2 + (c2[i][0][1] - point3Y) ** 2)
if dist3 < min3 :
min3 = dist3
closest3 = c2[i][0]
dist4 = np.sqrt((c2[i][0][0]-point4X) ** 2 + (c2[i][0][1] - point4Y) ** 2)
if dist4 < min4 :
min4 = dist4
closest4 = c2[i][0]
por1X = (point1X + closest1[0])/2
por1Y = (point1Y + closest1[1])/2
por2X = (point2X + closest2[0])/2
por2Y = (point2Y + closest2[1])/2
por3X = (point3X + closest3[0])/2
por3Y = (point3Y + closest3[1])/2
por4X = (point4X + closest4[0])/2
por4Y = (point4Y + closest4[1])/2
#avgRingDepth = (depth_image[por1Y, por1X]/4 + depth_image[por2Y, por2X]/4 + depth_image[por3Y, por3X]/4 + depth_image[por4Y, por4X]/4)
avgRingDepth = depth_image[int(e[0][1]), int(e[0][0])]
avgRed = (bgr_image[por1Y, por1X, 2]/4 + bgr_image[por2Y, por2X, 2]/4 + bgr_image[por3Y, por3X, 2]/4 + bgr_image[por4Y, por4X, 2]/4)
avgBlue = (bgr_image[por1Y, por1X, 0]/4 + bgr_image[por2Y, por2X, 0]/4 + bgr_image[por3Y, por3X, 0]/4 + bgr_image[por4Y, por4X, 0]/4)
avgGreen = (bgr_image[por1Y, por1X, 1]/4 + bgr_image[por2Y, por2X, 1]/4 + bgr_image[por3Y, por3X, 1]/4 + bgr_image[por4Y, por4X, 1]/4)
#print("distance, BGR", avgRingDepth, avgBlue, avgGreen, avgRed);
# Calculate the position of the detected ellipse
# CENTER
k_f = 525 # kinect focal length in pixels
elipse_x = self.dims[1] / 2 - e[0][0]
elipse_y = self.dims[0] / 2 - e[0][1]
angle_to_center = np.arctan2(elipse_x, k_f)
# Get the angles in the base_link relative coordinate system
(x_center, y_center) = (avgRingDepth/1000.0 * np.cos(angle_to_center), avgRingDepth/1000.0
* np.sin(angle_to_center))
#print("x,y,center", x_center, y_center)
size = (e[1][0]+e[1][1])/2
center = (e[0][1], e[0][0])
#print("image shape", bgr_image.shape)
x1 = int(center[0] )
x2 = int(center[0] )
x_min = x1 if x1>0 else 0
x_max = x2 if x2<bgr_image.shape[0] else bgr_image.shape[0]
#print("x_min, x_max", x_min, x_max)
y1 = int(center[1] - size / 2)
y2 = int(center[1] + size / 2)
y_min = y1 if y1 > 0 else 0
y_max = y2 if y2 < bgr_image.shape[1] else bgr_image.shape[1]
#print("y_min, y_max", y_min, y_max)
# Draw a diagonal blue line with thickness of 5 px
#cv2.line(bgr_image,(y_min,x_min),(y_max,x_max),(255,0,0),5)
#cv2.imwrite('debug.jpeg', bgr_image)
# Convert the depth image to a Numpy array since most cv2 functions
# require Numpy arrays.
#depth_array = np.array(depth_image, dtype=np.float32)
# Normalize the depth image to fall between 0 (black) and 1 (white)
#cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
# At this point you can display the result properly:
# If you write it as it si, the result will be a image with only 0 to 1 values.
# To actually store in a this a image like the one we are showing its needed
# to reescale the otuput to 255 gray scale.
#cv2.imwrite('debug_depth.png', depth_array * 255)
# POINT 1
elipse_x = self.dims[1] / 2 - y_min #morde sta zamenjana x in y
elipse_y = self.dims[0] / 2 - x_min
angle_to_point1 = np.arctan2(elipse_x, k_f)
(x_1, y_1) = (depth_image[x_min, y_min]/1000.0 * np.cos(angle_to_point1), depth_image[x_min, y_min]/1000.0
* np.sin(angle_to_point1))
# POINT 2
elipse_x = self.dims[1] / 2 - y_max #morde sta zamenjana x in y
elipse_y = self.dims[0] / 2 - x_max
angle_to_point2 = np.arctan2(elipse_x, k_f)
(x_2, y_2) = (depth_image[x_max, y_max]/1000.0 * np.cos(angle_to_point2), depth_image[x_max, y_max]/1000.0
* np.sin(angle_to_point2))
#print("koti", angle_to_center, angle_to_point1, angle_to_point2)
#print("x1, y1, x2, y2 pred transformom", x_1, y_1, x_2, y_2)
#print("avgDepth, depth_levo, depth_desno", avgRingDepth, depth_image[x_min, y_min], depth_image[x_max, y_max])
point_s = PointStamped()
point_s.point.x = -y_center
point_s.point.y = 0
point_s.point.z = x_center
point_s.header.frame_id = 'camera_rgb_optical_frame'
point_s.header.stamp = rospy.Time(0)
# Get the point in the "map" coordinate system
point_world = self.tf_buf.transform(point_s, 'map')
#print("center", point_world.point.x, point_world.point.y)
# Create a Pose object with the same position
pose = Pose()
pose.position.x = point_world.point.x
pose.position.y = point_world.point.y
pose.position.z = point_world.point.z
# POINT 1
point_s1 = PointStamped()
point_s1.point.x = -y_1
point_s1.point.y = 0
point_s1.point.z = x_1
point_s1.header.frame_id = 'camera_rgb_optical_frame'
point_s1.header.stamp = rospy.Time(0)
# Get the point in the "map" coordinate system
point_world1 = self.tf_buf.transform(point_s1, 'map')
x_1 = point_world1.point.x
y_1 = point_world1.point.y
# POINT 2
point_s2 = PointStamped()
point_s2.point.x = -y_2
point_s2.point.y = 0
point_s2.point.z = x_2
point_s2.header.frame_id = 'camera_rgb_optical_frame'
point_s2.header.stamp = rospy.Time(0)
# Get the point in the "map" coordinate system
point_world2 = self.tf_buf.transform(point_s2, 'map')
x_2 = point_world2.point.x
y_2 = point_world2.point.y
# Marker for circle
# Create a marker used for visualization
self.marker_num += 1
marker = Marker()
marker.header.stamp = point_world.header.stamp
marker.header.frame_id = point_world.header.frame_id
marker.pose = pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(10)
marker.id = self.marker_num
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(0, 1, 0, 1)
self.markers_pub.publish(marker)
# CALCULATING NORMAL
# {x,y} -> {y,-x}
#print("x1, y1, x2, y2", x_1, y_1, x_2, y_2)
#xy = [x_1-x_2, y_1-y_2]
#print(xy)
normala = [y_1-y_2, -(x_1-x_2)]
#print("normala", normala)
normala[0] = normala[0] / ((normala[0]**2+normala[1]**2)**(1/2))
normala[1] = normala[1] / ((normala[0]**2+normala[1]**2)**(1/2))
#print("normalizirana normala", normala)
pose3 = Pose()
pose3.position.x = point_world.point.x + normala[0]
pose3.position.y = point_world.point.y + normala[1]
pose3.position.z = point_world.point.z
# Marker for point perpendicular
self.marker_num += 1
marker = Marker()
marker.header.stamp = point_world.header.stamp
marker.header.frame_id = point_world.header.frame_id
marker.pose = pose3
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(10)
marker.id = self.marker_num
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 1, 1, 1)
self.normals_pub.publish(marker)
return(avgRingDepth/1000)
def DealObstacleModel(msg):
local_msg = ModelStates()
local_msg = msg
target_id_list = []
target_index_list = []
target_model_pose_list = []
target_model_twist_list = []
target_index = 0
for item in local_msg.name:
for index in range(1, 13):
if (item == "agent" + str(index)):
target_id_list.append(index)
target_index_list.append(target_index)
target_model_pose_list.append(local_msg.pose[target_index])
target_model_twist_list.append(local_msg.twist[target_index])
break
target_index += 1
# print(item)
b_time = time.time()
obs_array = ObjectArray()
obs_array.header.frame_id = 'world'
obs_array.header.stamp = rospy.Time.now()
obs_array.header.seq = 1
for index in range(0, 12):
ob = Object()
#world pose
pose = geometry_msgs.msg.Pose()
twist = geometry_msgs.msg.Twist()
pose = target_model_pose_list[index]
twist = target_model_twist_list[index]
ob.world_pose.header.stamp = rospy.Time.now()
ob.world_pose.header.frame_id = 'world'
ob.world_pose.point.x = pose.position.x
ob.world_pose.point.y = pose.position.y
ob.world_pose.point.z = pose.position.z
(roll, pitch, yaw) = euler_from_quaternion([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
# print("yaw: ", yaw)
ob.heading = yaw
ob.velocity = numpy.sqrt(
pow(twist.linear.x, 2) + pow(twist.linear.y, 2))
ob.id = target_id_list[index]
#velo pose
#cam pose
#others
obs_array.list.append(ob)
#pub marker for visualize
ob_array = PoseArray()
marker_array = MarkerArray()
info_marker_array = MarkerArray()
ob_array.header.frame_id = 'world'
ob_array.header.stamp = rospy.Time.now()
marker_id = 0
for index in range(0, 12):
#pose
temp_pose = geometry_msgs.msg.Pose()
temp_marker = Marker()
pose = geometry_msgs.msg.Pose()
twist = geometry_msgs.msg.Twist()
pose = target_model_pose_list[index]
twist = target_model_twist_list[index]
temp_pose.position.x = pose.position.x
temp_pose.position.y = pose.position.y
angle = math.atan2(twist.linear.y, twist.linear.x)
q = quaternion_from_euler(0, 0, angle)
temp_pose.orientation.x = q[0]
temp_pose.orientation.y = q[1]
temp_pose.orientation.z = q[2]
temp_pose.orientation.w = q[3]
ob_array.poses.append(temp_pose)
#info marker:velocity info array
temp_info_marker = Marker()
temp_info_marker.header.stamp = rospy.Time.now()
temp_info_marker.action = temp_info_marker.ADD
temp_info_marker.ns = 'basic_shape'
temp_info_marker.type = temp_info_marker.TEXT_VIEW_FACING
temp_info_marker.header.frame_id = 'world'
temp_info_marker.id = marker_id
# temp_info_marker.pose = temp_pose
temp_info_marker.pose.position.x = temp_pose.position.x
temp_info_marker.pose.position.y = temp_pose.position.y
temp_info_marker.pose.position.z = temp_pose.position.z + 1
# temp_info_marker.pose.position.z += 0.4 * 1.5
temp_info_marker.pose.orientation.x = 0
temp_info_marker.pose.orientation.y = 0
temp_info_marker.pose.orientation.z = 0
temp_info_marker.pose.orientation.w = 1
# temp_info_marker.scale.x = 2.0
# temp_info_marker.scale.y = 2.0
temp_info_marker.scale.z = 0.6
temp_info_marker.color.a = 1.0
temp_info_marker.color.r = 1
temp_info_marker.color.g = 0
temp_info_marker.color.b = 1
car_x = map2base_link_ts.transform.translation.x
car_y = map2base_link_ts.transform.translation.y
car_z = map2base_link_ts.transform.translation.z
dis2car = math.sqrt(
pow(car_x - temp_pose.position.x, 2) +
pow(car_y - temp_pose.position.y, 2))
dis2car = (float)((int)(100 * dis2car)) / 100.0 - 0.6
temp_info_marker.text = "dis:" + str(dis2car) + "m."
info_marker_array.markers.append(temp_info_marker)
# if dis2car <= 0.0:
# temp_info_marker.text = "Collision"
# else:
# temp_info_marker.text = "dis:" + str(dis2car) + "m."
#marker
temp_marker.ns = 'basic_shape'
temp_marker.header.stamp = rospy.Time.now()
temp_marker.header.frame_id = 'world'
temp_marker.type = temp_marker.SPHERE
temp_marker.pose = temp_pose
temp_marker.action = temp_marker.ADD
temp_marker.scale.x = 0.4
temp_marker.scale.y = 0.4
temp_marker.scale.z = 0.4
temp_marker.id = marker_id
marker_id = marker_id + 1
temp_marker.color.r = 0
temp_marker.color.g = 1
temp_marker.color.b = 0
temp_marker.color.a = 1
marker_array.markers.append(temp_marker)
ob_pub.publish(ob_array)
marker_pub.publish(marker_array)
info_marker_pub.publish(info_marker_array)
obstacle_pub.publish(obs_array)
def detections_callback(self, detection):
global counter
global maxDistance #the maximal distance between two points required for them to be considered the same detection
global alreadyDetected
global n
global lastAdded
global detected
global numFaces
u = detection.x + detection.width / 2
v = detection.y + detection.height / 2
camera_info = None
best_time = 100
for ci in self.camera_infos:
time = abs(ci.header.stamp.to_sec() -
detection.header.stamp.to_sec())
if time < best_time:
camera_info = ci
best_time = time
if not camera_info or best_time > 1:
print("Best time is higher than 1")
return
camera_model = PinholeCameraModel()
camera_model.fromCameraInfo(camera_info)
point = Point(
((u - camera_model.cx()) - camera_model.Tx()) / camera_model.fx(),
((v - camera_model.cy()) - camera_model.Ty()) / camera_model.fy(),
1)
localization = self.localize(detection.header, point,
self.region_scope)
transformedPoint = point
if not localization:
return
now = detection.header.stamp
self.tf.waitForTransform("camera_rgb_optical_frame", "map", now,
rospy.Duration(5.0))
m = geometry_msgs.msg.PoseStamped()
m.header.frame_id = "camera_rgb_optical_frame"
m.pose = localization.pose
m.header.stamp = detection.header.stamp
transformedPoint = self.tf.transformPose("map", m)
if (localization.pose.position.x != 0.0):
#print("Transformation: ", transformedPoint)
beenDetected = False
if counter == 0:
lastAdded = transformedPoint
else:
lastAdded = transformedPoint
for p in detected:
if self.distance(p, transformedPoint) <= maxDistance:
beenDetected = True
break
if (beenDetected == False):
counter += 1
print("-----------------Good to go------------------------")
detected.append(lastAdded)
self.pub_face.publish(transformedPoint)
marker = Marker()
marker.header.stamp = detection.header.stamp
marker.header.frame_id = detection.header.frame_id
marker.pose = localization.pose
marker.type = Marker.CUBE
marker.action = Marker.ADD
marker.frame_locked = False
marker.lifetime = rospy.Duration.from_sec(1)
marker.id = self.marker_id_counter
marker.scale = Vector3(0.1, 0.1, 0.1)
marker.color = ColorRGBA(1, 0, 0, 1)
self.markers.markers.append(marker)
self.marker_id_counter += 1
#self.soundhandle.say("I detected a face.", blocking = False)
print("Number of detected faces: ", len(detected))
lastAdded = transformedPoint
def makeGripperMarker(angle=0.541, color=None, scale=1.0):
"""
Creates an InteractiveMarkerControl with the PR2 gripper shape.
Parameters:
angle: the aperture angle of the gripper (default=0.541)
color: (r,g,b,a) tuple or None (default) if using the material colors
scale: the scale of the gripper, default is 1.0
Returns:
The new gripper InteractiveMarkerControl
"""
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, 0.01, 0.)
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
control = InteractiveMarkerControl()
mesh = Marker()
mesh.type = Marker.MESH_RESOURCE
mesh.scale.x = scale
mesh.scale.y = scale
mesh.scale.z = scale
if color is not None:
mesh.color.r = color[0]
mesh.color.g = color[1]
mesh.color.b = color[2]
mesh.color.a = color[3]
mesh.mesh_use_embedded_materials = False
else:
mesh.mesh_use_embedded_materials = True
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/gripper_palm.dae"
mesh.pose.orientation.w = 1
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
T1 = euclid.Matrix4()
T2 = euclid.Matrix4()
T1.translate(0.07691, -0.01, 0.)
T1.rotate_axis(numpy.pi, euclid.Vector3(1,0,0))
T1.rotate_axis(angle, euclid.Vector3(0,0,1))
T2.translate(0.09137, 0.00495, 0.)
T1.rotate_axis(-angle, euclid.Vector3(0,0,1))
T_proximal = T1.copy()
T_distal = T1 * T2
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger.dae"
mesh.pose = matrix4ToPose(T_proximal)
control.markers.append(copy.deepcopy(mesh))
mesh.mesh_resource = "package://pr2_description/meshes/gripper_v0/l_finger_tip.dae"
mesh.pose = matrix4ToPose(T_distal)
control.markers.append(copy.deepcopy(mesh))
control.interaction_mode = InteractiveMarkerControl.BUTTON
return control
def test():
rospy.init_node('intersect_plane_test')
marker_pub = rospy.Publisher('table_marker', Marker)
pose_pub = rospy.Publisher('pose', PoseStamped)
int_pub = rospy.Publisher('intersected_points', PointCloud2)
tfl = tf.TransformListener()
# Test table
table = Table()
table.pose.header.frame_id = 'base_link'
table.pose.pose.orientation.x, table.pose.pose.orientation.y, table.pose.pose.orientation.z, table.pose.pose.orientation.w = (
0, 0, 0, 1)
table.x_min = -0.5
table.x_max = 0.5
table.y_min = -0.5
table.y_max = 0.5
# A marker for that table
marker = Marker()
marker.header.frame_id = table.pose.header.frame_id
marker.id = 1
marker.type = Marker.LINE_STRIP
marker.action = 0
marker.pose = table.pose.pose
marker.scale.x, marker.scale.y, marker.scale.z = (0.005, 0.005, 0.005)
marker.color.r, marker.color.g, marker.color.b, marker.color.a = (0.0, 1.0,
1.0, 1.0)
marker.frame_locked = False
marker.ns = 'table'
marker.points = [
Point(table.x_min, table.y_min, table.pose.pose.position.z),
Point(table.x_min, table.y_max, table.pose.pose.position.z),
Point(table.x_max, table.y_max, table.pose.pose.position.z),
Point(table.x_max, table.y_min, table.pose.pose.position.z),
Point(table.x_min, table.y_min, table.pose.pose.position.z),
]
marker.colors = []
marker.text = ''
# marker.mesh_resource = ''
marker.mesh_use_embedded_materials = False
marker.header.stamp = rospy.Time.now()
# A test pose
pose = PoseStamped()
pose.header = table.pose.header
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z = (0, 0,
0.5)
pose.pose.orientation.x, pose.pose.orientation.y, pose.pose.orientation.z, pose.pose.orientation.w = quaternion_from_euler(
0, -pi / 5, pi / 5)
intersection = cast_ray(pose, table, tfl)
cloud = xyz_array_to_pointcloud2(
np.array([[
intersection.point.x, intersection.point.y, intersection.point.z
]]))
cloud.header = pose.header
while not rospy.is_shutdown():
marker_pub.publish(marker)
pose_pub.publish(pose)
int_pub.publish(cloud)
rospy.loginfo('published')
rospy.sleep(0.1)
def callback(data):
boxes = data.boxes
marker_array = MarkerArray()
markers = []
i = 0
for box in boxes:
new_marker = Marker()
new_marker.header = data.header
new_marker.ns = str(i)
new_marker.id = box.id
new_marker.type = 5 #line list
new_marker.action = 0
marker_pose = Pose()
marker_pose.position.x = 0
marker_pose.position.y = 0
marker_pose.position.z = 0
marker_pose.orientation.x = 0
marker_pose.orientation.y = 0
marker_pose.orientation.z = 0
marker_pose.orientation.w = 1
new_marker.pose = marker_pose
scale_vec = Vector3()
scale_vec.x = 0.1
scale_vec.y = 0.1
scale_vec.z = 0.1
new_marker.scale = scale_vec
line_color = ColorRGBA()
if(str(box.text) == "pedestrian"):
line_color.r = 0.0
line_color.g = 0.0
line_color.b = 1.0
line_color.a = 1.0
elif(str(box.text) == "cyclist"):
line_color.r = 0.0
line_color.g = 1.0
line_color.b = 0.0
line_color.a = 1.0
elif(str(box.text) == "vehicle"):
line_color.r = 1.0
line_color.g = 1.0
line_color.b = 0.0
line_color.a = 1.0
else:
line_color.r = 1.0
line_color.g = 1.0
line_color.b = 1.0
line_color.a = 1.0
new_marker.color = line_color
new_marker.lifetime = rospy.Duration.from_sec(0.1)
new_marker.frame_locked = False
new_marker.text = str(box.text)
x1 = box.p1.x
y1 = box.p1.y
z1 = box.p1.z
x2 = box.p2.x
y2 = box.p2.y
z2 = box.p2.z
for i in range(16):
new_point = Point()
if(i == 0):
new_point.x = x1
new_point.y = y1
new_point.z = z1
elif(i == 1):
new_point.x = x1
new_point.y = y1
new_point.z = z2
elif(i == 2):
new_point.x = x2
new_point.y = y1
new_point.z = z2
elif(i == 3):
new_point.x = x2
new_point.y = y1
new_point.z = z1
elif(i == 4):
new_point.x = x2
new_point.y = y2
new_point.z = z1
elif(i == 5):
new_point.x = x2
new_point.y = y2
new_point.z = z2
elif(i == 6):
new_point.x = x1
new_point.y = y2
new_point.z = z2
elif(i == 7):
new_point.x = x1
new_point.y = y2
new_point.z = z1
elif(i == 8):
new_point.x = x1
new_point.y = y1
new_point.z = z1
elif(i == 9):
new_point.x = x2
new_point.y = y1
new_point.z = z1
elif(i == 10):
new_point.x = x2
new_point.y = y1
new_point.z = z2
elif(i == 11):
new_point.x = x2
new_point.y = y2
new_point.z = z2
elif(i == 12):
new_point.x = x2
new_point.y = y2
new_point.z = z1
elif(i == 13):
new_point.x = x1
new_point.y = y2
new_point.z = z1
elif(i == 14):
new_point.x = x1
new_point.y = y2
new_point.z = z2
elif(i == 15):
new_point.x = x1
new_point.y = y1
new_point.z = z2
else:
rospy.loginfo("Visualizer Fail!")
new_marker.points.append(new_point)
markers.append(new_marker)
marker_array.markers = markers
marker_pub.publish(marker_array)
def callback(data):
global lock
global pa
global ma
global to_store
global load_success
global n
global file_name_from_button
if lock == False:
print('testing')
pa.header.stamp = rospy.Time(0)
pa.header.seq = n
n = n + 1
#pose_in = Pose()
#o = data.pose.orientation
#(o1x, o1y, o1z) = euler_from_quaternion([o.x, o.y, o.z, o.w])
print('testing')
pa.poses.append(data.pose)
print('testing')
if len(pa.poses) > to_store:
pa.poses = pa.poses[1:]
global publisher
publisher.publish(pa)
animals = ['Cat', 'Camel', 'Rabbit', 'Donkey', 'Croc', 'Pig', '-']
marker_in = Marker()
marker_in.header.frame_id = "/map"
marker_in.ns = ""
marker_in.id = 0
marker_in.type = marker_in.TEXT_VIEW_FACING
marker_in.action = marker_in.ADD
marker_in.pose = data.pose
marker_in.text = ""
marker_in.scale.x = 1.0
marker_in.scale.y = 1.0
marker_in.scale.z = 0.750
marker_in.color.r = 1.0
marker_in.color.g = 1.0
marker_in.color.b = 1.0
marker_in.color.a = 1.0
ma.markers.append(marker_in)
if len(ma.markers) > to_store:
ma.markers = ma.markers[1:]
for idx in range(len(ma.markers)):
ma.markers[idx].text = animals[idx]
ma.markers[idx].id = idx
global publisher_text
publisher_text.publish(ma)
global loaded
if loaded == True:
poster_list = []
poster_list.append(['x', 'y', 'angle'])
for idx in range(len(pa.poses)):
pose = pa.poses[idx]
po = pose.orientation
(ox, oy, oz) = euler_from_quaternion([po.x, po.y, po.z, po.w])
poster_list.append([pose.position.x, pose.position.y, oz])
with open("RewardData/" + file_name_from_button, 'w') as rloc:
print("saving to: " + file_name_from_button)
wr = csv.writer(rloc, dialect='excel')
wr.writerows(poster_list)
rloc.close()
def _plot(self, publisher, model, data, previous_ids=()):
"""Create markers for each object of the model and publish it as MarkerArray (also delete unused previously created markers)"""
from rospy import get_rostime
from std_msgs.msg import Header, ColorRGBA
from geometry_msgs.msg import Point
from visualization_msgs.msg import MarkerArray, Marker
self.seq += 1
header = Header(frame_id='map', seq=self.seq,
stamp=get_rostime()) # Common header for every marker
marker_array = MarkerArray()
for obj in model.geometryObjects:
obj_id = model.getGeometryId(obj.name)
# Prepare marker
marker = Marker()
marker.id = obj_id
marker.header = header
marker.action = Marker.ADD # same as Marker.MODIFY
marker.pose = SE3ToROSPose(data.oMg[obj_id])
marker.color = ColorRGBA(*obj.meshColor)
if obj.meshTexturePath != "":
warnings.warn(
"Textures are not supported in RVizVisualizer (for " +
obj.name + ")",
category=UserWarning,
stacklevel=2)
# Create geometry
geom = obj.geometry
if WITH_HPP_FCL_BINDINGS and isinstance(geom, hppfcl.ShapeBase):
# append a primitive geometry
if isinstance(geom, hppfcl.Cylinder):
d, l = 2 * geom.radius, 2 * geom.halfLength
marker.type = Marker.CYLINDER
marker.scale = Point(d, d, l)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Box):
size = npToTuple(2. * geom.halfSide)
marker.type = Marker.CUBE
marker.scale = Point(*size)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Sphere):
d = 2 * geom.radius
marker.type = Marker.SPHERE
marker.scale = Point(d, d, d)
marker_array.markers.append(marker)
elif isinstance(geom, hppfcl.Capsule):
d, l = 2 * geom.radius, 2 * geom.halfLength
marker_array.markers.extend(
create_capsule_markers(marker, data.oMg[obj_id], d, l))
else:
msg = "Unsupported geometry type for %s (%s)" % (
obj.name, type(geom))
warnings.warn(msg, category=UserWarning, stacklevel=2)
continue
else:
# append a mesh
marker.type = Marker.MESH_RESOURCE # Custom mesh
marker.scale = Point(*npToTuple(obj.meshScale))
marker.mesh_resource = 'file://' + obj.meshPath
marker_array.markers.append(marker)
# Remove unused markers
new_ids = [marker.id for marker in marker_array.markers]
for old_id in previous_ids:
if not old_id in new_ids:
marker_remove = Marker()
marker_remove.header = header
marker_remove.id = old_id
marker_remove.action = Marker.DELETE
marker_array.markers.append(marker_remove)
# Publish markers
publisher.publish(marker_array)
# Return list of markers id
return new_ids
def find_affordance(self, data):
aff_list = []
markerArray = MarkerArray()
no = 0
any_screw_on_lid = False
subtract_flag = False
for part_ in data.part_array:
partname = part_.part_id[:-1]
if partname == self.affordance[0]:
part = part_
if partname == 'platters_clamp':
subtract_flag = True
subtract_part = part_
part_mask = self.bridge.imgmsg_to_cv2(
part.part_outline.part_mask, part.part_outline.part_mask.encoding)
img_w, img_h = part_mask.shape[0], part_mask.shape[1]
width_scaled = 256
height_scaled = 256
part_mask_scaled = self.image_resize(part_mask, width_scaled,
height_scaled)
if self.affordance[0] == 'magnet':
suck_points = self.findSuckPoints(part_mask_scaled, width_scaled,
height_scaled, 20, 20)
elif self.affordance[0] == 'lid' or self.affordance[0] == 'pcb':
suck_points = self.findSuckPoints(part_mask_scaled, width_scaled,
height_scaled, 40, 30)
elif self.affordance[0] == 'platter':
if subtract_flag:
subtract_part_mask = self.bridge.imgmsg_to_cv2(
subtract_part.part_outline.part_mask,
subtract_part.part_outline.part_mask.encoding)
subtract_part_mask_scaled = self.image_resize(
subtract_part_mask, 256, 256)
part_mask_scaled = part_mask_scaled - subtract_part_mask_scaled
suck_points = self.findSuckPoints(part_mask_scaled, width_scaled,
height_scaled, 40, 30)
else:
return aff_list
if len(suck_points) == 0:
return aff_list
suck_points_converted = ConvertPixel2WorldRequest()
for i in range(len(suck_points)):
suck_point = geometry_msgs.msg.Point()
suck_point.y = suck_points[i, 0] * img_w / 256.0 #todo
suck_point.x = suck_points[i, 1] * img_h / 256.0
suck_point.z = 0
suck_points_converted.pixels.append(suck_point)
point_inverted = np.array(suck_points[i, ::-1])[1:]
cv2.circle(affordanceWrapper.affordance_vis_image,
tuple(point_inverted.astype(np.int16)), 3,
(0, 255, 255), -1)
resp = self.pixel_world_srv(suck_points_converted)
aff = Affordance()
aff.object_name = part.part_id
aff.effect_name = 'suckable'
aff.affordance_name = 'suckability'
markerArray = MarkerArray()
for i in range(len(suck_points)):
marker = Marker()
ap = ActionParameters()
ap.confidence = suck_points[i, 2]
suck_point = AscPair()
suck_point.key = 'start_pose'
suck_point.value_type = 2
suck_point.value_pose.position.x = resp.points[i].x
suck_point.value_pose.position.y = resp.points[i].y
suck_point.value_pose.position.z = resp.points[i].z
suck_point.value_pose.orientation.x = 0
suck_point.value_pose.orientation.y = -0.707
suck_point.value_pose.orientation.z = 0
suck_point.value_pose.orientation.w = 0.707
ap.parameters.append(suck_point)
h = std_msgs.msg.Header()
h.stamp = rospy.Time.now()
h.frame_id = resp.header.frame_id
marker.header = h
marker.ns = "suck_point_" + part.part_id
marker.id = i
marker.type = 3
marker.action = 0
marker.scale.x = 0.01
marker.scale.y = 0.001
marker.scale.z = 0.002
marker.lifetime = rospy.Duration(150)
marker.color.r = 1.0
marker.color.g = 1.0
marker.color.b = 0.0
marker.color.a = 1.0
marker.pose = suck_point.value_pose
markerArray.markers.append(marker)
aff.action_parameters_array.append(ap)
aff_list.append(aff)
rate = rospy.Rate(10)
for _ in range(5):
self.suck_rviz.publish(markerArray)
rate.sleep()
return aff_list
def run_dope_node(params, freq=5):
'''Starts ROS node to listen to image topic, run DOPE, and publish DOPE results'''
global g_img
global g_draw
pubs = {}
models = {}
pnp_solvers = {}
pub_dimension = {}
draw_colors = {}
class_ids = {}
model_transforms = {}
dimensions = {}
mesh_scales = {}
meshes = {}
# Initialize parameters
matrix_camera = np.zeros((3,3))
matrix_camera[0,0] = params["camera_settings"]['fx']
matrix_camera[1,1] = params["camera_settings"]['fy']
matrix_camera[0,2] = params["camera_settings"]['cx']
matrix_camera[1,2] = params["camera_settings"]['cy']
matrix_camera[2,2] = 1
dist_coeffs = np.zeros((4,1))
if "dist_coeffs" in params["camera_settings"]:
dist_coeffs = np.array(params["camera_settings"]['dist_coeffs'])
config_detect = lambda: None
config_detect.mask_edges = 1
config_detect.mask_faces = 1
config_detect.vertex = 1
config_detect.threshold = 0.5
config_detect.softmax = 1000
config_detect.thresh_angle = params['thresh_angle']
config_detect.thresh_map = params['thresh_map']
config_detect.sigma = params['sigma']
config_detect.thresh_points = params["thresh_points"]
# For each object to detect, load network model, create PNP solver, and start ROS publishers
for model in params['weights']:
models[model] =\
ModelData(
model,
g_path2package + "/weights/" + params['weights'][model]
)
models[model].load_net_model()
mesh_scales[model] = 1.0
model_transforms[model] = np.array([0.0, 0.0, 0.0, 1.0], dtype='float64')
draw_colors[model] = \
tuple(params["draw_colors"][model])
class_ids[model] = \
(params["class_ids"][model])
dimensions[model] = tuple(params["dimensions"][model])
pnp_solvers[model] = \
CuboidPNPSolver(
model,
matrix_camera,
Cuboid3d(params['dimensions'][model]),
dist_coeffs=dist_coeffs
)
pubs[model] = \
rospy.Publisher(
'{}/pose_{}'.format(params['topic_publishing'], model),
PoseStamped,
queue_size=10
)
pub_dimension[model] = \
rospy.Publisher(
'{}/dimension_{}'.format(params['topic_publishing'], model),
String,
queue_size=10
)
# Start ROS publisher
pub_rgb_dope_points = \
rospy.Publisher(
params['topic_publishing']+"/rgb_points",
ImageSensor_msg,
queue_size=10
)
pub_detections = \
rospy.Publisher(
'detected_objects',
Detection3DArray,
queue_size=10
)
pub_markers = \
rospy.Publisher(
'markers',
MarkerArray,
queue_size=10
)
# Starts ROS listener
rospy.Subscriber(
topic_cam,
ImageSensor_msg,
__image_callback
)
# Initialize ROS node
rospy.init_node('dope_vis', anonymous=True)
rate = rospy.Rate(freq)
print ("Running DOPE... (Listening to camera topic: '{}')".format(topic_cam))
print ("Ctrl-C to stop")
while not rospy.is_shutdown():
if g_img is not None:
# Copy and draw image
img_copy = g_img.copy()
im = Image.fromarray(img_copy)
g_draw = ImageDraw.Draw(im)
detection_array = Detection3DArray()
detection_array.header = image_msg.header
for m in models:
# Detect object
results = ObjectDetector.detect_object_in_image(
models[m].net,
pnp_solvers[m],
g_img,
config_detect
)
# Publish pose and overlay cube on image
for i_r, result in enumerate(results):
if result["location"] is None:
continue
loc = result["location"]
ori = result["quaternion"]
# transform orientation
transformed_ori = tf.transformations.quaternion_multiply(ori, model_transforms[m])
# rotate bbox dimensions if necessary
# (this only works properly if model_transform is in 90 degree angles)
dims = rotate_vector(vector=dimensions[m], quaternion=model_transforms[m])
dims = np.absolute(dims)
dims = tuple(dims)
msg = PoseStamped()
msg.header.frame_id = params["frame_id"]
msg.header.stamp = rospy.Time.now()
CONVERT_SCALE_CM_TO_METERS = 100
msg.pose.position.x = loc[0] / CONVERT_SCALE_CM_TO_METERS
msg.pose.position.y = loc[1] / CONVERT_SCALE_CM_TO_METERS
msg.pose.position.z = loc[2] / CONVERT_SCALE_CM_TO_METERS
msg.pose.orientation.x = ori[0]
msg.pose.orientation.y = ori[1]
msg.pose.orientation.z = ori[2]
msg.pose.orientation.w = ori[3]
# Publish
pubs[m].publish(msg)
pub_dimension[m].publish(str(params['dimensions'][m]))
# Add to Detection3DArray
detection = Detection3D()
hypothesis = ObjectHypothesisWithPose()
hypothesis.id = class_ids[result["name"]]
hypothesis.score = result["score"]
hypothesis.pose.pose = msg.pose
detection.results.append(hypothesis)
detection.bbox.center = msg.pose
detection.bbox.size.x = dims[0] / CONVERT_SCALE_CM_TO_METERS
detection.bbox.size.y = dims[1] / CONVERT_SCALE_CM_TO_METERS
detection.bbox.size.z = dims[2] / CONVERT_SCALE_CM_TO_METERS
detection_array.detections.append(detection)
# Draw the cube
if None not in result['projected_points']:
points2d = []
for pair in result['projected_points']:
points2d.append(tuple(pair))
DrawCube(points2d, draw_colors[m])
# Publish the image with results overlaid
pub_rgb_dope_points.publish(
CvBridge().cv2_to_imgmsg(
np.array(im)[...,::-1],
"bgr8"
)
)
# Delete all existing markers
markers = MarkerArray()
marker = Marker()
marker.action = Marker.DELETEALL
markers.markers.append(marker)
pub_markers.publish(markers)
# Object markers
class_id_to_name = {class_id: name for name, class_id in class_ids.iteritems()}
markers = MarkerArray()
for i, det in enumerate(detection_array.detections):
name = class_id_to_name[det.results[0].id]
color = draw_colors[name]
# cube marker
marker = Marker()
marker.header = detection_array.header
marker.action = Marker.ADD
marker.pose = det.bbox.center
marker.color.r = color[0] / 255.0
marker.color.g = color[1] / 255.0
marker.color.b = color[2] / 255.0
marker.color.a = 0.4
marker.ns = "bboxes"
marker.id = i
marker.type = Marker.CUBE
marker.scale = det.bbox.size
markers.markers.append(marker)
pub_markers.publish(markers)
pub_detections.publish(detection_array)
rate.sleep()
obj_id += 1
marker.type = 2
scale = Vector3()
scale.x = 0.1
scale.y = 0.1
scale.z = 0.1
marker.scale = scale
pose = Pose()
pose.position.x = j_obj["position"]["x"]
pose.position.y = j_obj["position"]["y"]
pose.position.z = 0
marker.pose = pose
header = Header()
header.frame_id = "map"
marker.header = header
color = ColorRGBA()
color.r = 0
color.g = 0
color.b = 255
color.a = 1
marker.color = color
markers.append(marker)
marker_array.markers = markers
pub.publish(marker_array)
def rviz_map(self):
# ground plane
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 150.0
box_marker.scale.y = 150.0
box_marker.scale.z = 0.1
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(30.0, 50.0, 0.1)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_plane"
self.map_marker.markers.append(box_marker)
#
####################### basebox #########################
#
# basebox 1
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 0.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_1"
self.map_marker.markers.append(box_marker)
# basebox 2
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 0.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_2"
self.map_marker.markers.append(box_marker)
# basebox 3
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 100.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_3"
self.map_marker.markers.append(box_marker)
# basebox 4
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 100.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_4"
self.map_marker.markers.append(box_marker)
# basebox 5
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 50.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_5"
self.map_marker.markers.append(box_marker)
# basebox 6
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 6.0
box_marker.scale.y = 6.0
box_marker.scale.z = 8.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 50.0, 4.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_basebox_6"
self.map_marker.markers.append(box_marker)
#
####################### Column #########################
#
# column 1
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 0.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_1"
self.map_marker.markers.append(box_marker)
# column 2
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 0.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_2"
self.map_marker.markers.append(box_marker)
# column 3
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 100.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_3"
self.map_marker.markers.append(box_marker)
# column 4
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 100.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_4"
self.map_marker.markers.append(box_marker)
# column 5
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 50.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_5"
self.map_marker.markers.append(box_marker)
# column 6
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 1.0
box_marker.scale.y = 1.0
box_marker.scale.z = 20.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 50.0, 10.0)
box_att = Quaternion(0, 0, 0, 1)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_column_6"
self.map_marker.markers.append(box_marker)
#
####################### Roof #########################
#
# roof 1
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 100.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 50.0, 20.0)
box_att = Quaternion(0.7071, 0, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_1"
self.map_marker.markers.append(box_marker)
# roof 2
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 100.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 50.0, 20.0)
box_att = Quaternion(0.7071, 0, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_2"
self.map_marker.markers.append(box_marker)
# roof 3
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 40.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(20.0, 100.0, 20.0)
box_att = Quaternion(0, 0.7071, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_3"
self.map_marker.markers.append(box_marker)
# roof 4
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 40.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(20.0, 0.0, 20.0)
box_att = Quaternion(0, 0.7071, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_4"
self.map_marker.markers.append(box_marker)
# roof 5
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 100.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(0.0, 50.0, 0.1)
box_att = Quaternion(0.7071, 0, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_5"
self.map_marker.markers.append(box_marker)
# roof 6
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 100.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(40.0, 50.0, 0.1)
box_att = Quaternion(0.7071, 0, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_6"
self.map_marker.markers.append(box_marker)
# roof 7
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 40.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(20.0, 100.0, 0.1)
box_att = Quaternion(0, 0.7071, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_7"
self.map_marker.markers.append(box_marker)
# roof 8
box_marker = Marker()
box_marker.type = Marker.CUBE
box_marker.header = Header(frame_id='map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 40.0
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(20.0, 0.0, 0.1)
box_att = Quaternion(0, 0.7071, 0, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "mbzirc_roof_8"
self.map_marker.markers.append(box_marker)
# tunnel 1
'''
box_marker = Marker()
box_marker.type = Marker.MESH_RESOURCE
box_marker.mesh_resource = "package://plot_data/mesh/AIGC_PIPE.dae"
box_marker.header = Header(frame_id='scout/map')
box_marker.scale.x = 0.1
box_marker.scale.y = 0.1
box_marker.scale.z = 0.1
box_marker.color.r = 1.0
box_marker.color.g = 1.0
box_marker.color.b = 1.0
box_marker.color.a = self.transparents
box_pos = Point(25.75, -5.0, 1.55)
box_att = Quaternion(0, 0, 0.7071, 0.7071)
box_marker.pose = Pose(box_pos, box_att)
box_marker.ns = "tunnel_1"
self.map_marker.markers.append(box_marker)
'''
'''
def init_controls(self):
self.int_marker.controls = []
# only add controls if they're enabled
imc = InteractiveMarkerControl()
imc.name = "rotate_x"
imc.always_visible = True
imc.orientation = Quaternion(1, 0, 0, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "translate_x"
imc.always_visible = True
imc.orientation = Quaternion(1, 0, 0, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "rotate_y"
imc.always_visible = True
imc.orientation = Quaternion(0, 1, 0, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "translate_y"
imc.always_visible = True
imc.orientation = Quaternion(0, 1, 0, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "rotate_z"
imc.always_visible = True
imc.orientation = Quaternion(0, 0, 1, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
self.int_marker.controls.append(imc)
imc = InteractiveMarkerControl()
imc.name = "translate_z"
imc.always_visible = True
imc.orientation = Quaternion(0, 0, 1, 1)
imc.orientation_mode = InteractiveMarkerControl.INHERIT
imc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.int_marker.controls.append(imc)
# add visual marker for the center
m = Marker()
m.lifetime = rospy.Duration(1.0)
m.ns = self.object_class
m.id = self.object_id
m.type = Marker.MESH_RESOURCE
m.mesh_resource = self.mesh_resource
m.mesh_use_embedded_materials = False
m.scale.x = m.scale.y = m.scale.z = 1.0
m.color.r = 1.0
m.color.g = m.color.b = 0.2
m.color.a = 0.3
# this works for orientation, but not position
#m.pose = copy.copy(self.pose)
m.pose = Pose()
m.pose.orientation.w = 1.0
imc = InteractiveMarkerControl()
imc.name = "mesh"
imc.always_visible = True
imc.orientation_mode = InteractiveMarkerControl.VIEW_FACING
imc.interaction_mode = InteractiveMarkerControl.MOVE_PLANE
imc.independent_marker_orientation = True
imc.markers.append(m)
self.int_marker.controls.append(imc)
def publish_markers(self, detection_array):
# Delete all existing markers
markers = MarkerArray()
marker = Marker()
marker.action = Marker.DELETEALL
markers.markers.append(marker)
self.pub_markers.publish(markers)
# Object markers
class_id_to_name = {class_id: name for name, class_id in self.class_ids.items()}
markers = MarkerArray()
for i, det in enumerate(detection_array.detections):
name = class_id_to_name[det.results[0].id]
color = self.draw_colors[name]
# cube marker
marker = Marker()
marker.header = detection_array.header
marker.action = Marker.ADD
marker.pose = det.bbox.center
marker.color.r = color[0] / 255.0
marker.color.g = color[1] / 255.0
marker.color.b = color[2] / 255.0
marker.color.a = 0.4
marker.ns = "bboxes"
marker.id = i
marker.type = Marker.CUBE
marker.scale = det.bbox.size
markers.markers.append(marker)
# text marker
marker = Marker()
marker.header = detection_array.header
marker.action = Marker.ADD
marker.pose = det.bbox.center
marker.color.r = color[0] / 255.0
marker.color.g = color[1] / 255.0
marker.color.b = color[2] / 255.0
marker.color.a = 1.0
marker.id = i
marker.ns = "texts"
marker.type = Marker.TEXT_VIEW_FACING
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.text = '{} ({:.2f})'.format(name, det.results[0].score)
markers.markers.append(marker)
# mesh marker
try:
marker = Marker()
marker.header = detection_array.header
marker.action = Marker.ADD
marker.pose = det.bbox.center
marker.color.r = color[0] / 255.0
marker.color.g = color[1] / 255.0
marker.color.b = color[2] / 255.0
marker.color.a = 0.7
marker.ns = "meshes"
marker.id = i
marker.type = Marker.MESH_RESOURCE
marker.scale.x = self.mesh_scales[name]
marker.scale.y = self.mesh_scales[name]
marker.scale.z = self.mesh_scales[name]
marker.mesh_resource = self.meshes[name]
markers.markers.append(marker)
except KeyError:
# user didn't specify self.meshes[name], so don't publish marker
pass
self.pub_markers.publish(markers)
colorPeople.b=0.0
colorPeople.a=1.0
markerPeople.color = colorPeople
markerPeople.lifetime.secs=1
markerPeople.lifetime.nsecs=0
markerArrayPeople = MarkerArray()
# if (counter % 500)==0:
# if people_inside:
# people_inside=False
# else:
people_inside=True
if people_inside:
j.poses.append(pose1)
# j.poses.append(pose2)
markerPeople.pose=pose1
markerArrayPeople.markers.append(markerPeople)
#print(j.header.seq)
publisher.publish(j)
#publisher_marker.publish(markerPeople)
publisher_marray.publish(markerArrayPeople)
counter = counter + 1
rate.sleep()
rospy.spin()
