def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
if self.current == self.gtg:
# 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))
elif self.current == self.avoidobstacles:
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(
0, 0, arrow_length * cos(self.avoidobstacles.heading_angle),
arrow_length * sin(self.avoidobstacles.heading_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))
# Important sensors
renderer.set_pen(0)
for v in self.wall.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
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))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
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(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.away_angle),
arrow_length*sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0,0,
arrow_length*cos(self.blending.heading_angle),
arrow_length*sin(self.blending.heading_angle))
def draw(self, renderer):
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(away_angle),
arrow_length * sin(away_angle))
# Week 3
renderer.set_pen(0)
for v in self.avoidobstacles.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
def draw(self, renderer):
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
# Week 3
renderer.set_pen(0)
for v in self.avoidobstacles.vectors:
x,y,z = v
renderer.push_state()
renderer.translate(x,y)
renderer.rotate(atan2(y,x))
renderer.draw_line(0.01,0.01,-0.01,-0.01)
renderer.draw_line(0.01,-0.01,-0.01,0.01)
renderer.pop_state()
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
if self.current == self.gtg:
# 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)
)
elif self.current == self.avoidobstacles:
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(
0,
0,
arrow_length * cos(self.avoidobstacles.heading_angle),
arrow_length * sin(self.avoidobstacles.heading_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)
)
# Important sensors
renderer.set_pen(0)
for v in self.wall.vectors:
x, y, z = v
renderer.push_state()
renderer.translate(x, y)
renderer.rotate(atan2(y, x))
renderer.draw_line(0.01, 0.01, -0.01, -0.01)
renderer.draw_line(0.01, -0.01, -0.01, 0.01)
renderer.pop_state()
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))
def draw(self, renderer):
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
# Draw direction from obstacles
renderer.set_pen(0xFF0000)
arrow_length = self.robot_size * 5
renderer.draw_arrow(0, 0,
arrow_length * cos(self.avoidobstacles.away_angle),
arrow_length * sin(self.avoidobstacles.away_angle))
# Draw direction to goal
renderer.set_pen(0x444444)
goal_angle = atan2(self.parameters.goal.y - self.pose_est.y,
self.parameters.goal.x - self.pose_est.x) \
- self.pose_est.theta
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0, 0, arrow_length * cos(goal_angle),
arrow_length * sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(away_angle),
arrow_length * sin(away_angle))
def draw(self, renderer):
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
# Draw direction from obstacles
renderer.set_pen(0xFF0000)
arrow_length = self.robot_size*5
renderer.draw_arrow(0,0,
arrow_length*cos(self.avoidobstacles.away_angle),
arrow_length*sin(self.avoidobstacles.away_angle))
# Draw direction to goal
renderer.set_pen(0x444444)
goal_angle = atan2(self.parameters.goal.y - self.pose_est.y,
self.parameters.goal.x - self.pose_est.x) \
- self.pose_est.theta
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self,renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size*5
away_angle = self.avoidobstacles.get_heading_angle(self.parameters)
goal_angle = self.gtg.get_heading_angle(self.parameters)
# Draw arrow to goal
renderer.set_pen(0x00FF00)
renderer.draw_arrow(0,0,
arrow_length*cos(goal_angle),
arrow_length*sin(goal_angle))
# Draw arrow away from obstacles
renderer.set_pen(0xFF0000)
renderer.draw_arrow(0,0,
arrow_length*cos(away_angle),
arrow_length*sin(away_angle))
def draw(self, renderer):
"""Draw controller info"""
K3Supervisor.draw(self, renderer)
renderer.set_pose(self.pose_est)
arrow_length = self.robot_size * 5
# Draw arrow to goal
renderer.set_pen(0x00FF00)
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(0xFF0000)
renderer.draw_arrow(0, 0, arrow_length * cos(self.blending.away_angle),
arrow_length * sin(self.blending.away_angle))
# Draw heading
renderer.set_pen(0x0000FF)
renderer.draw_arrow(0, 0,
arrow_length * cos(self.blending.heading_angle),
arrow_length * sin(self.blending.heading_angle))
