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()
# setup buffers
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'])
# stripe parameters and build array
gap = 0.3
sd = 0 # sd = 0, stripes in x direction sd = 1 stripes in y direction
x1 = -0.0
x2 = 0.7
y1 = 0.0
y2 = -0.7
psi = 0 * math.pi
goal_array = kgstripes.stripes(sd, gap, x1, x2, y1, y2, psi)
# goal_array = np.array([[0, 0, 0], [0, -0.1, 0], [0.8, -0.1, 2],
# [0.8, 0.1, -0.5], [0, 0.1, 0], [0, 0, 0]])
# this runs when new slam output data is published and it publishes on the twist topic
def callback(data, paramf):
global dtv, dxv, dyv, tp, xp, yp, qp, ed
global flag_first, flag_goal_met, flag_end, n_safe, n_goals
global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array
pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10)
twist = Twist()
q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]