def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self, renderer)
# Make sure to have all headings:
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
# Ensure the headings are calculated
# Draw arrow to goal
if self.current == self.gtg:
goal_angle = self.gtg.get_heading_angle(self.parameters)
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# Draw arrow for PathFollowing
elif self.current == self.path:
goal_angle = self.path.get_heading_angle(self.parameters)
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# elif self.current == self.blending:
# goal_angle = self.blending.get_heading_angle(self.parameters)
# renderer.set_pen(0x0000FF)
# renderer.draw_arrow(0,0,
# arrow_length*cos(goal_angle),
# arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
elif self.current == self.avoidobstacles:
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
renderer.set_pen(0xCC3311)
renderer.draw_arrow(0, 0, arrow_length * cos(away_angle),
arrow_length * sin(away_angle))
elif self.current == self.wall:
# Draw vector to wall:
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0, 0, self.wall.to_wall_vector[0],
self.wall.to_wall_vector[1])
# Draw
renderer.set_pen(0xFF00FF)
renderer.push_state()
renderer.translate(self.wall.to_wall_vector[0],
self.wall.to_wall_vector[1])
renderer.draw_arrow(0, 0, self.wall.along_wall_vector[0],
self.wall.along_wall_vector[1])
renderer.pop_state()
# Draw heading (who knows, it might not be along_wall)
renderer.set_pen(0xFF00FF)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.wall.heading_angle),
arrow_length * sin(self.wall.heading_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self, renderer)
renderer.set_pose(Pose(self.pose_est.x, self.pose_est.y))
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(self.gtg.heading_angle),
arrow_length * sin(self.gtg.heading_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
renderer.set_pose(Pose(self.pose_est.x,self.pose_est.y))
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(self.gtg.heading_angle),
arrow_length*sin(self.gtg.heading_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
# Make sure to have all headings:
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
# Ensure the headings are calculated
# Draw arrow to goal
if self.current == self.gtg:
goal_angle = self.gtg.get_heading_angle(self.parameters)
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
elif self.current == self.avoidobstacles:
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
renderer.set_pen(0xCC3311)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
elif self.current == self.wall:
# Draw vector to wall:
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0,0,
self.wall.to_wall_vector[0],
self.wall.to_wall_vector[1])
# Draw
renderer.set_pen(0xFF00FF)
renderer.push_state()
renderer.translate(self.wall.to_wall_vector[0], self.wall.to_wall_vector[1])
renderer.draw_arrow(0,0,
self.wall.along_wall_vector[0],
self.wall.along_wall_vector[1])
renderer.pop_state()
# Draw heading (who knows, it might not be along_wall)
renderer.set_pen(0xFF00FF)
renderer.draw_arrow(0,0,
arrow_length*cos(self.wall.heading_angle),
arrow_length*sin(self.wall.heading_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
# Ensure the headings are calculated
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
if self.current == self.gtg:
renderer.set_pen(0x00FF00,0.01)
else:
renderer.set_pen(0xA000FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
if self.current == self.avoidobstacles:
renderer.set_pen(0xFF0000,0.01)
else:
renderer.set_pen(0xA0FF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
if "blending" in self.__dict__:
blend_angle = self.blending.get_heading_angle(self.parameters)
# Draw the blending
if self.current == self.blending:
renderer.set_pen(0xFF, 0.01)
else:
renderer.set_pen(0xA00000FF)
renderer.draw_arrow(0,0,
arrow_length*cos(blend_angle),
arrow_length*sin(blend_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0xCC00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.goal_angle),
arrow_length * sin(self.blending.goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xCCFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.away_angle),
arrow_length * sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0xFF, 0.02)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.blending.heading_angle),
arrow_length * sin(self.blending.heading_angle))
def draw_foreground(self, renderer):
"""Draw controller info"""
QuickBotSupervisor.draw_foreground(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0xCC00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.goal_angle),
arrow_length*sin(self.blending.goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xCCFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.away_angle),
arrow_length*sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0xFF,0.02)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.heading_angle),
arrow_length*sin(self.blending.heading_angle))
