def drawPath(self, boundary, markerFrame): # Lines will be updateable (size, location), but will not be interactive themselves int_marker = InteractiveMarker() int_marker.header.frame_id = markerFrame int_marker.name = "pathLines" int_marker.description = "" int_marker.pose = Pose() int_marker.pose.orientation.w = 1.0 line_marker = Marker() line_marker.type = Marker.LINE_STRIP line_marker.scale.x = 0.10 line_marker.scale.y = 0.10 line_marker.scale.z = 0.10 line_marker.color.r = 0.5 line_marker.color.g = 0.5 line_marker.color.b = 0.0 line_marker.color.a = 0.5 line_marker.frame_locked = True #Stripes algorithm takes arguements: orientation, line gap, x1, x2, y1, y2, yaw offset of robot) #To ensure the robot is "inside" the coverage area, the boundary is inset by the robot radius insetBoundary = [0,0,0,0] for cidx, corners in enumerate(boundary): if abs(corners) >= self.robotRadius: if round(cidx/2) == 1.0: factor = -1 else: factor = 1 insetBoundary[cidx] = boundary[cidx] + factor * self.robotRadius x1 = insetBoundary[0] x2 = insetBoundary[2] y1 = insetBoundary[1] y2 = insetBoundary[3] self.path = kgstripes.stripes(0, self.pathGap, x1, x2, y1, y2, 0) for position in self.path: line_marker.points.append(Point(*position)) line_control = InteractiveMarkerControl() line_control.always_visible = True line_control.markers.append( line_marker ) # add the control to the interactive marker int_marker.controls.append( line_control ) # As the marker is only for show, it has NONE interactive control null_control = InteractiveMarkerControl() null_control.name = "none" null_control.interaction_mode = InteractiveMarkerControl.NONE # Keep the lines orientated in the orientation of the main marker (i.e. int_marker pose, there for in the x-y plane, despite the user moving the scene in rviz) null_control.orientation_mode = InteractiveMarkerControl.FIXED # add the control to the interactive marker int_marker.controls.append(null_control); #self.server.insert(int_marker, lineFeedback) self.server.insert(int_marker) self.server.applyChanges()
psides = 0 # execute back and forth motion between two goals back_and_forth = False single_goal = True counter = 0 # stripe parameters gap = 0.2 sd = 0 # sd = 0, stripes in x direction sd = 1 stripes in y direction x1 = -0.0 x2 = 2.0 y1 = 0.0 y2 = 0.6 psi = 0 * math.pi goal_array = kgstripes.stripes(sd, gap, x1, x2, y1, y2, psi) # Make a blank goal array goal_array = np.array([]) # control parameters param = dict(vel=0.1, psivel=0.2, goal_tol=0.05, goal_tol_psi=0.1, nv=4, t_ramp=5) dtv = coll.deque([1e-5, 1e-5], maxlen=param['nv']) dxv = coll.deque([1e-5, 1e-5], maxlen=param['nv']) dyv = coll.deque([1e-5, 1e-5], maxlen=param['nv']) dpsiv = coll.deque([1e-5, 1e-5], maxlen=param['nv']) def poseParse(PoseMsg): # Convert geometry_msgs/Pose structures into [x,y,z] orientation = tft.euler_from_quaternion([PoseMsg.orientation.x, PoseMsg.orientation.y, PoseMsg.orientation.z, PoseMsg.orientation.w]) pose = np.array([PoseMsg.position.x, PoseMsg.position.y, orientation[2]])