/// <summary>
/// Starts the simulation in the V-REP EDU application and begins drawing the map on a new thread.
/// </summary>
private void StartSimulation()
{
VREPWrapper.simxGetObjectHandle(_clientId, R.WheelLeft0, out _handleLeftMotor, simx_opmode.oneshot_wait);
VREPWrapper.simxGetObjectHandle(_clientId, R.WheelRight0, out _handleRightMotor, simx_opmode.oneshot_wait);
VREPWrapper.simxGetObjectHandle(_clientId, R.SICKS300, out _handleSick, simx_opmode.oneshot_wait);
VREPWrapper.simxGetObjectHandle(_clientId, R.Origo, out _handleRelative, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle left motor: " + _handleLeftMotor);
Debug.WriteLine("Handle right motor: " + _handleRightMotor);
Debug.WriteLine("Handle laser scanner: " + _handleSick);
Localization.ClientId = _clientId;
Localization.HandleSick = _handleSick;
Localization.HandleRelative = _handleRelative;
MapBuilder.RequestLaserScannerDataRefresh += RequestLaserScannerDataRefresh;
MapBuilder.CalculatePose += CalculatePose;
_mapBuilderThread = new Thread(MapBuilder.BuildLayers);
try
{
_mapBuilderThread.Start();
}
catch (Exception ex)
{
Debug.WriteLine(ex);
}
VREPWrapper.simxStartSimulation(_clientId, simx_opmode.oneshot_wait);
SimulationIsRunning = true;
}
public void Disconnect()
{
if (_connected)
{
VREPWrapper.simxFinish(_clientID);
}
}
/// <summary>
/// Stops the simulation in the V-REP EDU application and aborts the map drawing thread.
/// </summary>
private void StopSimulation()
{
VREPWrapper.simxStopSimulation(_clientId, simx_opmode.oneshot_wait);
SimulationIsRunning = false;
_mapBuilderThread?.Abort();
Localization.PoseChanged -= PoseChanged;
MapBuilder.RequestLaserScannerDataRefresh -= RequestLaserScannerDataRefresh;
MapBuilder.CalculatePose -= CalculatePose;
}
private void InitHandlers()
{
VREPWrapper.simxGetObjectHandle(_clientID, "neobotix#0", out _handleNeo, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle neobotix#0: " + _handleNeo);
VREPWrapper.simxGetObjectHandle(_clientID, "wheel_left#0", out _handleLeftMotor, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle left motor #0: " + _handleLeftMotor);
VREPWrapper.simxGetObjectHandle(_clientID, "wheel_right#0", out _handleRightMotor, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle right motor #0: " + _handleRightMotor);
VREPWrapper.simxGetObjectHandle(_clientID, "SICK_S300_fast#0", out _handleSick, simx_opmode.oneshot_wait);
}
/// <summary>
/// Connect or disconnect from V-REP
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void buttonConnect_Click(object sender, RoutedEventArgs e)
{
if (!_connected) // If not connected - try to connect
{
try
{
_clientID = VREPWrapper.simxStart("127.0.0.1", 19997, true, true, 5000, 5);
}
catch (DllNotFoundException ex)
{
MessageBox.Show("remoteApi.dll missing");
}
if (_clientID != -1) // Successfully connected to V-REP
{
Debug.WriteLine("Connected to V-REP");
buttonConnect.Background = Brushes.LightSeaGreen;
_connected = true;
stackControls.Visibility = Visibility.Visible;
buttonConnect.Content = "Disconnect";
VREPWrapper.simxGetObjectHandle(_clientID, "neobotix#0", out _handleNeo0, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle neobotix#0: " + _handleNeo0);
VREPWrapper.simxGetObjectHandle(_clientID, "neobotix#1", out _handleNeo1, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle neobotix#1: " + _handleNeo1);
VREPWrapper.simxGetObjectHandle(_clientID, "wheel_left#0", out _handleLeftMotor0, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle left motor #0: " + _handleLeftMotor0);
VREPWrapper.simxGetObjectHandle(_clientID, "wheel_right#0", out _handleRightMotor0, simx_opmode.oneshot_wait);
Debug.WriteLine("Handle right motor #0: " + _handleRightMotor0);
VREPWrapper.simxGetObjectHandle(_clientID, "SICK_S300_fast#0", out _handleSick0, simx_opmode.oneshot_wait);
if (_handleNeo0 == 0 && _handleNeo1 == 0)
{
MessageBox.Show("Connected, but no neobotix robot found");
}
}
else // Connection trial failed
{
Debug.WriteLine("Error connecting to V-REP");
MessageBox.Show("Error connecting to V-REP :(");
buttonConnect.Background = Brushes.IndianRed;
_connected = false;
stackControls.Visibility = Visibility.Hidden;
}
}
else // If connected - try to disconnect
{
VREPWrapper.simxFinish(_clientID);
buttonConnect.Background = Brushes.IndianRed;
buttonConnect.Content = "Reconnect";
_connected = false;
stackControls.Visibility = Visibility.Hidden;
Debug.WriteLine("Disconnected from V-REP");
_updatePos = false;
}
}
/// <summary>
/// Disconnects the robot.
/// </summary>
public void Disconnect()
{
if (!_connected)
{
return;
}
SetSimulationState(SimulationCommand.Stop);
Thread.Sleep(1000);
VREPWrapper.simxFinish(-1);
}
private void buttonScan_Click(object sender, RoutedEventArgs e)
{
int i;
// reading the laser scanner stream
VREPWrapper.simxReadStringStream(_clientID, "measuredDataAtThisTime0", ref _signalValuePtr, ref _signalLength, simx_opmode.streaming);
//Debug.WriteLine(String.Format("test: {0:X8} {1:D} {2:X8}", _signalValuePtr, _signalLength, _signalValuePtr+_signalLength));
float[] f = new float[685 * 3];
if (_signalLength >= f.GetLength(0))
{
_laserScannerData = new float[3, f.GetLength(0) / 3];
PointCollection laserPointCollection = new PointCollection();
Polyline laserScannerLine = new Polyline(); // polyline for laser scanner data display
laserScannerLine = new Polyline();
laserScannerLine.Stroke = System.Windows.Media.Brushes.DarkMagenta;
laserScannerLine.StrokeThickness = 2;
// todo read the latest stream (this is not the latest)
unsafe
{
float *pp = (float *)(_signalValuePtr).ToPointer();
//Debug.WriteLine("pp: " + *pp);
for (i = 0; i < f.GetLength(0); i++)
{
f[i] = (float)*pp++; // pointer to float array
}
}
i = 0;
// reshaping the 1D [3*685] data to 2D [3, 685] > x, y, z coordinates
for (i = 0; i < f.GetLength(0); i++)
{
if (!(Math.Abs((float)f[i]) < 0.000001))
{
_laserScannerData[i % 3, i / 3] = (float)f[i];
}
}
for (i = 0; i < _laserScannerData.GetLength(1); i++)
{
//Debug.WriteLine(_laserScannerData[0, i] + ";\t" + _laserScannerData[1, i] + ";\t" + _laserScannerData[2, i]);
if (!(Math.Abs((float)_laserScannerData[0, 1]) < 0.001))
{
laserPointCollection.Add(new Point(_laserScannerData[0, i] * 40 + 50, _laserScannerData[1, i] * 40 + 150));
}
}
//Debug.WriteLine("laserPointCollection count: " + laserPointCollection.Count);
laserScannerLine.Points = laserPointCollection; // adds the data to the polyline
canvScan.Children.Add(laserScannerLine); // displays the polyline on the cansvas
//Debug.Write("--end scanner--");
}
}
public void upd()
{
float[] pos = new float[3]; // The position of the #0 robot relative to the #1 robot
while (_updatePos) // todo
{
VREPWrapper.simxGetObjectPosition(_clientID, _handleNeo0, _handleNeo1, _pos, simx_opmode.oneshot_wait);
VREPWrapper.simxGetJointPosition(_clientID, _handleLeftMotor0, ref _posLeftMotor0, simx_opmode.oneshot_wait);
PositionString = String.Format("x:{0:F4} y:{1:F4} z:{2:F4}", _pos[2], _pos[1], _pos[0]);
WheelString = String.Format("wheel:{0:F3}", _posLeftMotor0);
WheelX = (Math.Cos(_posLeftMotor0) * 100) + 150;
WheelY = (Math.Sin(_posLeftMotor0) * 100) + 150;
//Debug.WriteLine("x:{0:F4} y:{1:F4}", WheelX, WheelY);
Thread.Sleep(400);
}
}
/// <summary>
/// Collects the laser scanner data from the simulator.
/// </summary>
/// <returns>Returns the collected laser scanner data.</returns>
private double[,] GetLaserScannerData()
{
double[,] laserScannerData;
// reading the laser scanner stream
simx_error s = VREPWrapper.simxReadStringStream(_clientId, R.MeasuredData0, ref _signalValuePtr, ref _signalLength, simx_opmode.streaming);
if (s != simx_error.noerror)
{
return(new double[0, 0]);
}
//Debug.WriteLine(s);
// Debug.WriteLine(String.Format("test: {0:X8} {1:D} {2:X8}", _signalValuePtr, _signalLength, _signalValuePtr+_signalLength));
float[] f = new float[685 * 3];
if (_signalLength / sizeof(float) >= f.GetLength(0))
{
//we managed to get the laserdatas from Vrep
laserScannerData = new double[3, f.GetLength(0) / 3];
// todo read the latest stream (this is not the latest)
int i;
unsafe
{
float *pp = (float *)_signalValuePtr.ToPointer();
//Debug.WriteLine("pp: " + *pp);
for (i = 0; i < f.GetLength(0); i++)
{
f[i] = *pp++; // pointer to float array
}
}
// reshaping the 1D [3*685] data to 2D [3, 685] > x, y, z coordinates
for (i = 0; i < f.GetLength(0); i++)
{
if (!(Math.Abs(f[i]) < 0.000001))
{
laserScannerData[i % 3, i / 3] = f[i];
}
}
return(laserScannerData);
}
// we couldnt get the laserdata, so we return an empty array
laserScannerData = new double[0, 0];
return(laserScannerData);
}
public void GetPose()
{
VREPWrapper.simxGetObjectPosition(ClientId, HandleSick, HandleRelative, _pos, simx_opmode.oneshot_wait);
VREPWrapper.simxGetObjectOrientation(ClientId, HandleSick, HandleRelative, _ori, simx_opmode.oneshot_wait);
//mysterious formula
if (_ori[0] < 0)
{
_ori[1] = _ori[1] * -1 + (float)Math.PI / 2;
}
else
{
_ori[1] = _ori[1] + (float)Math.PI + (float)Math.PI / 2;
}
Pose = new Pose(
(int)(_pos[0] * RobotControl.MapZoom),
(int)(_pos[1] * RobotControl.MapZoom),
180.0 * _ori[2] / Math.PI);
}
/// <summary>
/// Starts, stops or resets the V-REP EDU simulation according to the given command.
/// </summary>
/// <param name="command">The given command.</param>
public void SetSimulationState(SimulationCommand command)
{
switch (command)
{
case SimulationCommand.Stop:
StopSimulation();
break;
case SimulationCommand.Start:
StartSimulation();
break;
case SimulationCommand.Reset:
//StopSimulation();
VREPWrapper.simxStopSimulation(_clientId, simx_opmode.oneshot_wait);
Thread.Sleep(400);
//StartSimulation();
VREPWrapper.simxStartSimulation(_clientId, simx_opmode.oneshot_wait);
break;
}
}
private void Window_Closed(object sender, EventArgs e)
{
// todo
_updatePos = false;
try
{
if (_up.IsAlive)
{
_up.Abort();
}
}
catch (Exception ex)
{
//todo
//MessageBox.Show("Exception occured");
}
if (_connected)
{
VREPWrapper.simxFinish(_clientID);
}
}
public int Connect()
{
if (!_connected) // If not connected - try to connect
{
try
{
_clientID = VREPWrapper.simxStart("127.0.0.1", 19997, true, true, 5000, 5);
}
catch (DllNotFoundException ex)
{
MessageBox.Show("remoteApi.dll missing");
}
if (_clientID != -1) // Successfully connected to V-REP
{
Debug.WriteLine("Connected to V-REP");
_connected = true;
InitHandlers();
return(0);
}
else // Connection trial failed
{
Debug.WriteLine("Error connecting to V-REP");
MessageBox.Show("Error connecting to V-REP :(");
_connected = false;
return(-1);
}
}
else // If connected - try to disconnect
{
VREPWrapper.simxFinish(_clientID);
_connected = false;
Debug.WriteLine("Disconnected from V-REP");
return(-2);
}
}
/// <summary>
/// Function for connecting to the robot.
/// </summary>
/// <param name="host">Address of the robot.</param>
/// <returns>Returns error codes about the success of the connection.</returns>
public int Connect(string host)
{
// If not connected - try to connect
if (!_connected)
{
try
{
_clientId = VREPWrapper.simxStart(host, 19997, true, true, 5000, 5);
}
catch (DllNotFoundException)
{
MessageBox.Show(R.ErrorMessage_DLL);
}
// Successfully connected to V-REP
if (_clientId != -1)
{
Debug.WriteLine("Connected to V-REP");
_connected = true;
return(0);
}
Debug.WriteLine("Error connecting to V-REP");
MessageBox.Show("Error connecting to V-REP");
_connected = false;
return(-1);
}
Disconnect();
Debug.WriteLine("Disconnected from V-REP");
_connected = false;
return(-2);
}
private void buttonResetSim_Click(object sender, RoutedEventArgs e)
{
VREPWrapper.simxStopSimulation(_clientID, simx_opmode.oneshot_wait);
Thread.Sleep(400);
VREPWrapper.simxStartSimulation(_clientID, simx_opmode.oneshot_wait);
}
private void buttonLeft_Click(object sender, RoutedEventArgs e)
{
VREPWrapper.simxSetJointTargetVelocity(_clientID, _handleLeftMotor0, -10, simx_opmode.oneshot_wait);
VREPWrapper.simxSetJointTargetVelocity(_clientID, _handleRightMotor0, 10, simx_opmode.oneshot_wait);
}
public void SetWheelSpeed(double R, double L)
{
VREPWrapper.simxSetJointTargetVelocity(_clientID, _handleLeftMotor, (float)R, simx_opmode.oneshot_wait);
VREPWrapper.simxSetJointTargetVelocity(_clientID, _handleRightMotor, (float)L, simx_opmode.oneshot_wait);
}
public void ResetSimulation()
{
VREPWrapper.simxStopSimulation(_clientID, simx_opmode.oneshot_wait);
Thread.Sleep(400);
VREPWrapper.simxStartSimulation(_clientID, simx_opmode.oneshot_wait);
}
