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