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()