static void Main(string[] args)
{
RosBridgeUtility.RosBridgeConfig conf1 = new RosBridgeUtility.RosBridgeConfig();
conf1.readConfig("XMLFile1.xml");
RosBridgeUtility.RosBridgeLogic logic = new RosBridgeUtility.RosBridgeLogic();
logic.AddStoppedListener(logic_MonitoringStopped);
String fos = "[10, 10]";
Console.WriteLine(Array.ConvertAll(fos.Replace("[", "").Replace("]", "").Split(','), Double.Parse));
logic.Initialize(conf1.URI);
logic.Connect();
logic.InitializeCollection();
/*
* Dictionary<String, double> lin = new Dictionary<string, double>()
* {
* {"x",1.5},
* {"y",0.0},
* {"z",0.0},
* };
* Dictionary<String, double> ang = new Dictionary<string, double>()
* {
* {"x",0.0},
* {"y",0.0},
* {"z",3.14},
* };
* Object[] vals = { lin, ang };
*/
//logic.PublishMessage("/turtle1/command_velocity", keys, vals);
//logic.PublishMessage("/turtle1/cmd_vel", keys, vals);
Object[] lin = { 2.0, 0.0, 0.0 };
Object[] ang = { 0.0, 0.0, 3.14 };
foreach (var item in conf1.getPublicationList())
{
logic.PublishTwistMsg(item, lin, ang);
}
logic.StartCollections(conf1.getTopicList());
Object[] linNeo = { 2.1, 2.1 };
Object[] driveActive = { true, true };
Object[] quickStop = { false, false };
Object[] disableBrake = { true, true };
//logic.PublishNeobotixCommandMsg("/DriveCommands", linNeo, driveActive, quickStop, disableBrake);
System.Threading.Thread.Sleep(1000);
logic.RemoveCollections(conf1.getTopicList());
var x = logic.StopCollection();
foreach (var attr in conf1.ProjectedAttributes())
{
Console.Out.WriteLine(attr.Item2);
foreach (var item in logic.getResponseAttribute(attr.Item1, attr.Item2))
{
Console.WriteLine(item);
//Console.Out.WriteLine(((List<Double>)item)[0]);
//Console.Out.WriteLine("{0}: {1} {2}", attr.Item1, ((List<Double>)item)[0], ((List<Double>)item)[1]);
}
}
}
public MainWindow()
{
InitializeComponent();
//bridgeLogic.currentVelocityState = new RosBridgeUtility.VelocityState();
stackControls.Visibility = System.Windows.Visibility.Hidden;
this.DataContext = _responseObj;
this.bridgeLogic = new RosBridgeUtility.RosBridgeLogic();
this.connectionState = (int)ConnectionState.Disconnected;
this.subscriptionState = (int)SubscriptionState.Unsubscribed;
bridgeLogic.setSubject(this);
this.bridgeConfig = new RosBridgeUtility.RosBridgeConfig();
bridgeConfig.readConfig("XMLFile1.xml");
Console.WriteLine("Ipaddress: {0}", bridgeConfig.ipaddress);
bridgeLogic.currentTarget = bridgeConfig.target;
bridgeLogic.initVelocityThreshold(bridgeConfig.min_vel, bridgeConfig.max_vel,
bridgeConfig.inc_vel, bridgeConfig.init_vel);
bridgeLogic.initAngularVelocityThreshold(bridgeConfig.min_ang, bridgeConfig.max_ang,
bridgeConfig.inc_ang, bridgeConfig.init_ang);
txtLgth.Text = bridgeLogic.current_velocity.ToString();
txtIP.Text = bridgeConfig.ipaddress;
txtPort.Text = bridgeConfig.port.ToString();
txtTheta.Text = bridgeLogic.current_angVelocity.ToString();
bridgeLogic.currentVelocityState.inc_vel = bridgeConfig.inc_vel;
bridgeLogic.currentVelocityState.max_vel = bridgeConfig.max_vel;
bridgeLogic.currentVelocityState.min_vel = bridgeConfig.min_vel;
bridgeLogic.currentVelocityState.current_vel = bridgeConfig.init_vel;
bridgeLogic.currentVelocityState.currentTheta = bridgeConfig.init_ang;
try
{
showState = bridgeConfig.showState;
laserScan = bridgeConfig.laserFieldTopic;
odometry = bridgeConfig.odometryTopic;
scaleFactor = bridgeConfig.vis_scaleFactor;
wcon = new WebController(bridgeConfig, bridgeLogic);
}
catch (NullReferenceException e)
{
Console.WriteLine("Error during read: {0}", e.Data);
}
wcon.updateVelocityState(bridgeLogic.currentVelocityState);
var u1 = new Uri(System.IO.Path.Combine(Environment.CurrentDirectory, "test1.jpg"));
BitmapImage img1 = new BitmapImage(u1);
kinect_screen.Source = img1;
BitmapSource source = kinect_screen.Source as BitmapSource;
//NeobotixStateServer = new WebSocketServer("ws://localhost:9091");
wcon.updateRGBState(ref source, img1.PixelHeight, img1.PixelWidth);
source = kinect_depth_screen.Source as BitmapSource;
//NeobotixStateServer = new WebSocketServer("ws://localhost:9091");
//wcon.updateDepthState(ref source, img1.PixelHeight, img1.PixelWidth);
// Kinect refresher timer
}
public WebController(RosBridgeUtility.RosBridgeConfig conf, RosBridgeUtility.RosBridgeLogic parent)
{
this.conf = conf;
this.parent = parent;
// Initialize webserver
NeobotixStateServer = new HttpServer(4649);
NeobotixStateServer.RootPath = "../../Public";
Console.WriteLine(NeobotixStateServer.RootPath);
LastMsgs = new RosBridgeUtility.ServerStorage();
LastRGB = new RosBridgeUtility.ImageStorage();
LastDepth = new RosBridgeUtility.ImageStorage();
NeobotixStateServer.OnGet += (sender, e) =>
{
var req = e.Request;
Console.WriteLine(e.Request.QueryString);
var res = e.Response;
var path = req.RawUrl;
if (path == "/")
{
path += "index.html";
}
path = NeobotixStateServer.RootPath + path;
//var content = NeobotixStateServer.GetFile(path);
Console.WriteLine(path);
Console.WriteLine(File.Exists(path));
var content = System.IO.File.ReadAllBytes(path);
if (content == null)
{
res.StatusCode = (int)HttpStatusCode.NotFound;
return;
}
if (path.EndsWith(".html"))
{
res.ContentType = "text/html";
res.ContentEncoding = Encoding.UTF8;
}
res.OutputStream.Write(content, 0, content.Length);
};
NeobotixStateServer.OnPut += NeobotixStateServer_OnPut;
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.OdometryBehavior>(("/neobot_odom"),
() => new RosBridgeUtility.OdometryBehavior(LastMsgs));
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.StateBehavior>(("/server_state"),
() => new RosBridgeUtility.StateBehavior(LastMsgs));
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.TeleopKeyBehavior>(("/neobot_teleop"),
() => new RosBridgeUtility.TeleopKeyBehavior(LastMsgs, this));
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.LaserScanBehavior>(("/neobot_laser"),
() => new RosBridgeUtility.LaserScanBehavior(LastMsgs));
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.ImageBehavior>(("/kinect_image"),
() => new RosBridgeUtility.ImageBehavior(LastRGB));
NeobotixStateServer.AddWebSocketService <RosBridgeUtility.ImageBehavior>(("/kinect_depth"),
() => new RosBridgeUtility.ImageBehavior(LastDepth));
}
