I wasn't planning on writing another part in this series, just yet, but then I got a little carried away with some refactoring work and decided it deserved a post of its own.
I wasn't fully happy with the code in the last post. The DecomposeCurve() function simply did too much: it opened a curve, extracted the points we were interested in and then created "movement" strings for each segment connecting the points. So the function was just way too single-purpose, even if a number of the individual operations being performed could potentially have been of use elsewhere.
So I went and made a Decompose() extension method that returns a set of points from a Curve. These points can then have ToAngleStrings() called upon them to create movement instructions. Or they can have ToConnectingLines() called to create a set of Line objects we can add to the drawing for debugging. Or both. 🙂
When writing ToAngleStrings() and ToConnectingLines(), I found they both iterated through the point array in exactly the same way. So I went and created a generic IterateVertices<T> extension method that calls a function repeatedly to create an array full of objects of type T.
With these tools in the box it was a simple matter of creating a new DEC command that displays the paths the robots will take for a particular curve.
Here's the updated C# code:
using Autodesk.AutoCAD.ApplicationServices;
using Autodesk.AutoCAD.DatabaseServices;
using Autodesk.AutoCAD.EditorInput;
using Autodesk.AutoCAD.Geometry;
using Autodesk.AutoCAD.Runtime;
using System;
using System.Collections.Generic;
using System.Collections.Specialized;
using System.Diagnostics;
using System.Globalization;
using System.Linq;
using System.Net;
using System.Threading.Tasks;
using System.Web.Script.Serialization;
namespace DriveRobots
{
public static class Extensions
{
// We use this to capitalicise keywords, although this could fail for
// various cases (common first letters, etc)
public static string ToTitleCase(this string str)
{
return CultureInfo.CurrentCulture.TextInfo.ToTitleCase(str.ToLower());
}
// Simplistic curve length method (should add code to check for Rays)
public static double Length(this Curve cur)
{
double start = cur.GetDistanceAtParameter(cur.StartParam);
double end = cur.GetDistanceAtParameter(cur.EndParam);
return Math.Abs(end - start);
}
// Midpoint of a curve
public static Point3d MidPoint(this Curve c)
{
return c.StartPoint + (c.EndPoint - c.StartPoint) * 0.5;
}
// Create a Point3d array from a Point3dCollection
public static Point3d[] ToArray(this Point3dCollection pts)
{
var pta = new Point3d[pts.Count];
pts.CopyTo(pta, 0);
return pta;
}
// Decompose a curve into a set of points, most of which will be "inc"
// apart (the last one may be closer: it will be the end point of the
// Curve)
public static Point3d[] Decompose(this Curve curve, double inc = 1.0)
{
if (curve is Ray)
return null;
; double dist = 0.0;
var points = new Point3dCollection();
var curLen = curve.Length();
while (dist < curLen)
{
// Make sure we go to the end of the curve, even if the last step
// is shorter than the specified distance
var endDist = dist + inc > curLen ? curLen : dist + inc;
var start = curve.GetPointAtDist(dist);
var end = curve.GetPointAtDist(endDist);
// If the first time through, add the start point
if (dist < inc) {
points.Add(start);
}
// Always add the end point
points.Add(end);
dist += inc;
}
return points.ToArray();
}
// Call a function on adjacent points in an array, returning the results
public static T[] IterateVertices<T>(
this Point3d[] pts, Func<Point3d, Point3d, T> f
)
{
if (pts.Length < 2)
return null;
var array = new T[pts.Length - 1];
for (int i = 0; i < pts.Length - 1; i++)
{
array[i] = f(pts[i], pts[i + 1]);
}
return array;
}
// Get the angle direction of a vector as a string
public static string DirectionString(this Vector3d vec)
{
// Get an integer direction (0-359) as a string
// (these will be instructions for our robots)
var ang = vec.GetAngleTo(Vector3d.YAxis, Vector3d.ZAxis);
var deg = (ang * 180.0) / Math.PI;
return Math.Round(deg).ToString();
}
// Get the anglular directions from an array of points as strings
public static String[] ToAngleStrings(this Point3d[] pts)
{
return pts.IterateVertices<String>(
(start, end) => DirectionString(end - start)
);
}
// Get the lines connecting a sequence (array) of points
public static Line[] ToConnectingLines(this Point3d[] pts)
{
return pts.IterateVertices<Line>((start, end) => new Line(start, end));
}
}
// Encapsulate calls to our robots behind a controller
public class
RobotController : IDisposable
{
const string host = "http://localhost:8080";
const string root = host + "/api/robots";
private WebClient _wc;
private bool disposed = false;
public RobotController()
{
_wc = new WebClient();
}
// Public implementation of Dispose pattern callable by consumers
public void Dispose()
{
Dispose(true);
GC.SuppressFinalize(this);
}
// Protected implementation of Dispose pattern
protected virtual void Dispose(bool disposing)
{
if (disposed)
return;
if (disposing && _wc != null)
{
_wc.Dispose();
_wc = null;
}
disposed = true;
}
~RobotController()
{
Dispose(false);
}
// Return the connected robots
public async Task<string[]> GetRobots()
{
var json = await _wc.DownloadStringTaskAsync(root);
return new JavaScriptSerializer().Deserialize<string[]>(json);
}
// Return the names directions they can move in
public string[] GetDirections()
{
return new string[] { "Left", "Right", "Forward", "Backward" };
}
// Wake the specified robot
public async Task WakeRobot(string robot)
{
await _wc.DownloadStringTaskAsync(root + "/" + robot.ToLower());
}
// Move the specified robot in a particular direction
public async Task MoveRobot(string robot, string direction)
{
await _wc.DownloadStringTaskAsync(
root + "/" + robot.ToLower() + "/" + direction.ToLower()
);
await Task.Delay(500);
}
}
public class Commands
{
private static string _lastBot = "";
[CommandMethod("DR")]
&#
160; public async void DriveRobot()
{
var doc = Application.DocumentManager.MdiActiveDocument;
if (doc == null)
return;
var ed = doc.Editor;
var db = doc.Database;
using (var rc = new RobotController())
{
string[] names = null;
try
{
names = await rc.GetRobots();
}
catch (System.Exception ex)
{
ed.WriteMessage("\nCan't access robot web-service: {0}.", ex.Message);
return;
}
// Ask the user for the robot to control
var pko = new PromptKeywordOptions("\nRobot name");
foreach (var name in names)
{
pko.Keywords.Add(name.ToTitleCase());
}
// If a bot was selected previously, set it as the default
if (!string.IsNullOrEmpty(_lastBot))
{
pko.Keywords.Default = _lastBot;
}
var pkr = ed.GetKeywords(pko);
if (pkr.Status != PromptStatus.OK)
return;
_lastBot = pkr.StringResult;
// Start by getting the bot - this should wake it, if needed
try
{
await rc.WakeRobot(_lastBot);
}
catch (System.Exception ex)
{
ed.WriteMessage("\nCan't connect to {0}: {1}.", _lastBot, ex.Message);
return;
}
// The direction can be one of the four main directions or a number
var pio = new PromptIntegerOptions("\nDirection");
var directions = rc.GetDirections();
foreach (var direction in directions)
{
pio.Keywords.Add(direction);
}
pio.AppendKeywordsToMessage = true;
// Set the direction depending on which was chosen
var pir = ed.GetInteger(pio);
var dir = "";
if (pir.Status == PromptStatus.Keyword)
{
dir = pir.StringResult;
}
else if (pir.Status == PromptStatus.OK)
{
dir = pir.Value.ToString();
}
else return;
// Our move command
try
{
await rc.MoveRobot(_lastBot, dir);
}
catch (System.Exception ex)
{
ed.WriteMessage("\nCan't move {0}: {1}.", _lastBot, ex.Message);
}
}
}
[CommandMethod("DRC", CommandFlags.UsePickSet)]
public async void DriveRobotAlongCurve()
{
var doc = Application.DocumentManager.MdiActiveDocument;
if (doc == null)
return;
var ed = doc.Editor;
var db = doc.Database;
var psr = ed.GetSelection();
if (psr.Status != PromptStatus.OK)
return;
// Filter the ObjectIds belonging to Curves
var curveIds =
psr.Value.GetObjectIds().Where(
id => id.ObjectClass.IsDerivedFrom(RXObject.GetClass(typeof(Curve)))
).ToArray<ObjectId>();
ed.WriteMessage("\n{0} curves selected.", curveIds.Length);
if (curveIds.Length == 0)
return;
// Ask the user for the step size to move along each path
var pdo = new PromptDoubleOptions("\nStep size for paths");
pdo.AllowNegative = false;
pdo.AllowZero = false;
pdo.DefaultValue = 1.0;
pdo.UseDefaultValue = true;
var pdr = ed.GetDouble(pdo);
if (pdr.Status != PromptStatus.OK)
return;
var inc = pdr.Value;
using (var rc = new RobotController())
{
// Store our robot names in a StringCollection to make it easier to
// remove them as they get selected by the user for association with
// a path
var names = new StringCollection();
var robots2paths = new Dictionary<string, ObjectId>();
try
{
names.AddRange(await rc.GetRobots());
}
60; catch (System.Exception ex)
{
ed.WriteMessage("\nCan't access robot web-service: {0}.", ex.Message);
return;
}
using (var tr = doc.TransactionManager.StartTransaction())
{
foreach (var id in curveIds)
{
// Highlight the curve
var ent = (Entity)tr.GetObject(id, OpenMode.ForRead);
ent.Highlight();
// Ask the user for the robot to associate with this path
var pko = new PromptKeywordOptions("\nRobot to drive along path");
foreach (var name in names)
{
pko.Keywords.Add(name.ToTitleCase());
}
var pkr = ed.GetKeywords(pko);
if (pkr.Status != PromptStatus.OK)
{
ent.Unhighlight();
return;
}
// Remove the selected robot from our list and map it to the path
var robot = pkr.StringResult.ToLower();
names.Remove(robot);
robots2paths.Add(robot, id);
// Unhighlight the curve
ent.Unhighlight();
// Wake the robot associated with this path
try
{
await rc.WakeRobot(robot);
}
catch (System.Exception ex)
{
ed.WriteMessage("\nCan't connect to {0}: {1}.", robot, ex.Message);
return;
}
}
// Now the fun starts...
// Get the list of moves for each robot
var robots2moves = new Dictionary<string, String[]>();
foreach (var kv in robots2paths)
{
var curve = tr.GetObject(kv.Value, OpenMode.ForRead) as Curve;
if (curve != null)
{
var pts = curve.Decompose(inc);
var moves = pts.ToAngleStrings();
robots2moves.Add(kv.Key, moves);
}
}
// Find out which is the largest sequence of moves - use that
// for our loop
var max = robots2moves.Max(kv => kv.Value.Length);
// Do a breadth-first traversal of our various moves lists,
// so the moves get executed quasi-simultaneously
for (int i = 0; i < max; i++)
{
foreach (var kv in robots2moves)
{
if (kv.Value.Length > i)
{
await rc.MoveRobot(kv.Key, kv.Value[i]);
}
}
}
// Always commit
tr.Commit();
}
}
}
[CommandMethod("DEC")]
public static void DecomposeCurve()
{
var doc = Application.DocumentManager.MdiActiveDocument;
if (doc == null)
return;
var ed = doc.Editor;
var db = doc.Database;
var peo = new PromptEntityOptions("\nSelect curve to decompose");
peo.SetRejectMessage("\nMust be a curve.");
peo.AddAllowedClass(typeof(Curve), false);
var per = ed.GetEntity(peo);
if (per.Status != PromptStatus.OK)
return;
using (var tr = doc.TransactionManager.StartTransaction())
{
// Open the current space for us to place our geometry
var btr =
(BlockTableRecord)tr.GetObject(db.CurrentSpaceId, OpenMode.ForWrite);
// Open the curve for read
var cur = tr.GetObject(per.ObjectId, OpenMode.ForRead) as Curve;
if (cur != null) // This test should always pass, but hey
{
// Get the array of points from the curve
var pts = cur.Decompose();
if (pts != null)
{
// Get the lines connecting the points
var lns = pts.ToConnectingLines();
// Get the "move" instructions between the points
var labels = pts.ToAngleStrings();
// They should (must) be the same length
Debug.Assert(lns.Length == labels.Length);
for (int i=0; i < lns.Length; i++)
{
// We'll make each line red and add it to the drawing
var ln = lns[i];
ln.ColorIndex = 1;
btr.AppendEntity(ln);
tr.AddNewlyCreatedDBObject(ln, true);
// Create a piece of text with the move instruction halfway
// along the line, making it yellow
var txt = new DBText();
txt.TextString = labels[i];
txt.Position = ln.MidPoint();
txt.ColorIndex = 2;
btr.AppendEntity(txt);
tr.AddNewlyCreatedDBObject(txt, true);
}
}
}
tr.Commit();
}
}
}
}
Here's what happens when we call the DEC command and select the curves we saw in the last post. It shows very well the operations that were sent to our two robots to have them travel along these paths.
There's still lots of places I can envision going with this series… I still want to work out how to use the obstacle detection capabilities of the BB-8 to map out a room, for instance: we should be able to create a low-resolution, inaccurate 2D point cloud of a room. But with a tiny, cute robot doing the work… sounds like fun!
Leave a Reply to Kean Walmsley Cancel reply