public Robot(RobotRenderer r, Color color, Color colorBloated, int id)
{
renderer = r;
pathRenderer = new PathFromRobotRenderer(r, color, .1f);
lidarRendererRear = new LidarRenderer("rear", 0, id, new SensorPose(-.36, 0, 0, Math.PI, 0, 0, 0));
lidarRendererFront = new LidarRenderer("front", 1, id, new SensorPose(0, 0, .20, 0, 0, 0, 0));
polyRenderer = new PolygonRenderer(color);
bloatedPolygonRenderer = new PolygonRenderer(colorBloated);
}
public void AddNewRobot(int robotID, Renderer r)
{
Color color;
try
{
color = colors[robotID - 1];
}
catch (IndexOutOfRangeException e)
{
Random rand = new Random();
color = Color.FromArgb((int)(rand.NextDouble() * 255), (int)(rand.NextDouble() * 255), (int)(rand.NextDouble() * 255));
}
Color colorBloated;
try
{
colorBloated = colorsBloated[robotID - 1];
}
catch (IndexOutOfRangeException e)
{
Random rand = new Random();
colorBloated = Color.FromArgb((int)(rand.NextDouble() * 255), (int)(rand.NextDouble() * 255), (int)(rand.NextDouble() * 255));
}
if (robotID != 5)
{
RobotRenderer rr = new RobotRenderer("Robot " + robotID, color);
//robots[robotID] = new Robot(rr, color, robotID);
robots[robotID] = new Robot(rr, color, colorBloated, robotID);
r.AddRenderable(rr);
r.AddRenderable(robots[robotID].PathRenderer);
r.AddRenderable(robots[robotID].LidarRendererFront);
r.AddRenderable(robots[robotID].LidarRendererRear);
r.AddRenderable(robots[robotID].PolygonRenderer);
r.AddRenderable(robots[robotID].BloatedPolygonRenderer);
r.UpdateRobotPose(robotID, new Magic.Common.RobotPose());
}
}
/// <summary>
/// Draw the waypoints & path (circles & lines)
/// </summary>
/// <param name="r"></param>
#region path drawing code
public void Draw(Renderer r)
{
//if only one waypoint, draw the waypoint
if (newPath.WaypointList.Count == 1 && shouldDraw)
{
WaypointRenderer wpFirst = newPath.WaypointList[0];
PointF drawPoint = new PointF((float)wpFirst.X, (float)wpFirst.Y);
GLUtility.DrawCircle(new GLPen(wpFirst.Color, 1.0f), drawPoint, 0.2f);
if (!shouldMove && tempActive)
{
GLUtility.DrawLine(new GLPen(Color.LawnGreen, 1.0f), drawPoint, r.ScreenToWorld(mousePoint));
}
}
// if more than one waypoint, draw waypoints and connect subsequent waypoints with a line
else if (newPath.WaypointList.Count > 1 && shouldDraw)
{
WaypointRenderer wpLast = newPath.WaypointList.First();
foreach (WaypointRenderer wp in newPath.WaypointList)
{
PointF pathPoint = new PointF((float)wp.X, (float)wp.Y);
if (shouldMove && wp.Equals(movePointWP))
{
GLUtility.DrawCircle(new GLPen(Color.Red, 1.0f), pathPoint, 0.2f);
}
else
{
GLUtility.DrawCircle(new GLPen(wp.Color, 1.0f), pathPoint, 0.2f);
}
if (!wp.Equals(newPath.WaypointList.First()))
{
PointF dummyLast = new PointF((float)wpLast.X, (float)wpLast.Y);
GLUtility.DrawLine(new GLPen(newPath.Color, 1.0f), dummyLast, pathPoint);
}
wpLast = wp;
}
// if we are not moving a waypoint, draw a light green line between last waypoint and current mouse location
if (!shouldMove && tempActive)
{
PointF dummyLast = new PointF((float)wpLast.X, (float)wpLast.Y);
GLUtility.DrawLine(new GLPen(Color.LawnGreen, 1.0f), dummyLast, r.ScreenToWorld(mousePoint));
}
}
if (newPath.WaypointList.Count > 0)
{
foreach (ISelectable selectable in r.Selectables)
{
PointF dummyFirst = new PointF();
RobotRenderer robot = selectable as RobotRenderer;
if (robot == null)
{
continue;
}
else if (robot.IsSelected)
{
Polygon p = robot.GetBoundingPolygon();
if (p.points.Count > 0)
{
dummyFirst = new PointF((float)newPath.WaypointList[0].X, (float)newPath.WaypointList[0].Y);
GLUtility.DrawLine(new GLPen(Color.Green, 1.0f), p.Center.ToPointF(), dummyFirst);
}
}
}
}
}
public void OnMouseDown(Renderer r, MouseEventArgs e)
{
PointF worldPoint = r.ScreenToWorld(e.Location);
if (e.Button == MouseButtons.Right)
{
//if (GestureExpData != null)
// GestureExpData(this, new GestureExpPointToolEventArgs("right down click, at " + e.X + ", " + e.Y + " (screen), " + worldPoint.X + ", " + worldPoint.Y + " (world)"));
}
else if (e.Button.Equals(MouseButtons.Left))
{
//PointF worldPoint = r.ScreenToWorld(e.Location);
if (modeNewPOI == true)
{
if (POItype == "PICTURE")
{
if (r.Selectables.Count() > 0)
{
foreach (ISelectable sel in r.Selectables)
{
RobotRenderer rr = sel as RobotRenderer;
if (rr != null && rr.IsSelected)
{
r.AddRenderable(new FakeObjectRenderer("important", rr.GetBoundingPolygon().Center.ToPointF(), "picture"));
if (GestureExpData != null)
{
GestureExpData(this, new GestureExpHRIEventArgs("Tool:PointTool|left click down|take picture|Robot " + rr.GetName().Substring(rr.GetName().Length - 1) + "|" + e.X + "|" + e.Y + "|(screen)|" + worldPoint.X + "|" + worldPoint.Y + "|(world)"));
}
}
}
}
}
else
{
NotepointRenderer newPoint = new NotepointRenderer();
newPoint.Name = POItype;
if (POItype == "SQUARE")
{
newPoint.Color = Color.DarkMagenta;
}
else if (POItype == "TRIANGLE")
{
newPoint.Color = Color.DarkGreen;
}
else if (POItype == "SPIRAL")
{
newPoint.Color = Color.DarkRed;
}
newPoint.X = worldPoint.X;
newPoint.Y = worldPoint.Y;
newPoint.Z = 0;
poiList.Add(newPoint);
r.AddRenderable(newPoint);
if (GestureExpData != null)
{
GestureExpData(this, new GestureExpHRIEventArgs("Tool:PointTool|left click down|create|" + newPoint.Name + "|" + e.X + "|" + e.Y + "|(screen)|" + worldPoint.X + "|" + worldPoint.Y + "|(world)"));
}
}
}
else if (modeDeletePOI == true)
{
if (poiList.Count > 0)
{
List <NotepointRenderer> pointsToRemove = new List <NotepointRenderer>();
foreach (NotepointRenderer np in poiList)
{
if ((worldPoint.X - np.X > -.25) && (worldPoint.Y - np.Y > -.25) &&
(worldPoint.X - np.X < .25) && (worldPoint.Y - np.Y < .25))
{
pointsToRemove.Add(np);
r.RemoveRenderable(np);
if (GestureExpData != null)
{
GestureExpData(this, new GestureExpHRIEventArgs("Tool:PointTool|left click down|remove|" + np.Name + "|" + e.X + "|" + e.Y + "|(screen)|" + worldPoint.X + "|" + worldPoint.Y + "|(world)"));
}
}
}
if (pointsToRemove.Count > 0)
{
foreach (NotepointRenderer n in pointsToRemove)
{
poiList.Remove(n);
}
myToolManager.SelectTool.TempReactivate();
myToolManager.PathTool.TempReactivate();
modeDeletePOI = false;
}
}
}
else if (modeMovePOI == true)
{
if (poiList.Count > 0)
{
foreach (NotepointRenderer np in poiList)
{
if ((worldPoint.X - np.X > -.25) && (worldPoint.Y - np.Y > -.25) &&
(worldPoint.X - np.X < .25) && (worldPoint.Y - np.Y < .25))
{
shouldMove = true;
pointToMove = np;
if (GestureExpData != null)
{
GestureExpData(this, new GestureExpHRIEventArgs("Tool:PointTool|left click down|move|" + np.Name + "|" + e.X + "|" + e.Y + "|(screen)|" + worldPoint.X + "|" + worldPoint.Y + "|(world)"));
}
break;
}
}
}
}
}
}
