|
|
@@ -10,6 +10,7 @@ using System;
|
|
|
using System.Collections.Concurrent;
|
|
|
using System.Collections.Generic;
|
|
|
using System.Linq;
|
|
|
+using System.Text;
|
|
|
using VehicleControlSystem.ControlLayer.Serial.BatteryTabos;
|
|
|
using VehicleControlSystem.ControlLayer.Serial.DataModel;
|
|
|
using VehicleControlSystem.Managers;
|
|
|
@@ -40,7 +41,9 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
|
|
|
ThreadCancel threadCancel = new ThreadCancel();
|
|
|
|
|
|
- BlockingCollection<ZmqMessage> sendQueue = new BlockingCollection<ZmqMessage>(new ConcurrentQueue<ZmqMessage>());
|
|
|
+ BlockingCollection<ZmqMessage> sendQueue = new BlockingCollection<ZmqMessage>( new ConcurrentQueue<ZmqMessage>() );
|
|
|
+
|
|
|
+ string endServer = "tcp://127.0.0.1:5567";
|
|
|
|
|
|
private bool isReqConnected = false;
|
|
|
public bool IsReqConnected
|
|
|
@@ -54,7 +57,7 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
public eSteeringState RequestSteering
|
|
|
{
|
|
|
get { return requestSteering; }
|
|
|
- set { SetField( ref this.requestSteering, value ); }
|
|
|
+ set { SetField( ref this.requestSteering, value, false ); }
|
|
|
}
|
|
|
|
|
|
private bool isDriveMoving = false;
|
|
|
@@ -173,16 +176,15 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
|
|
|
req = new RequestSocket();
|
|
|
this.monitor = new NetMQMonitor( req, "inproc://rep.inproc", SocketEvents.Disconnected | SocketEvents.Connected );
|
|
|
- this.monitor.Connected += ( s, a ) =>
|
|
|
- {
|
|
|
- this.IsReqConnected = true;
|
|
|
+ this.monitor.Connected += ( s, a ) =>
|
|
|
+ {
|
|
|
+ this.IsReqConnected = true;
|
|
|
CurrentPointNo = GetCurrentPointNo();
|
|
|
SetCurrentSteeringState( this.steering.GetSteeringState() );
|
|
|
};
|
|
|
|
|
|
this.monitor.Disconnected += ( s, a ) => { this.IsReqConnected = false; };
|
|
|
this.monitor.StartAsync();
|
|
|
-
|
|
|
req.Connect( "tcp://127.0.0.1:5567" );
|
|
|
|
|
|
this.poller = new NetMQPoller { this.rep };
|
|
|
@@ -227,7 +229,7 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
catch ( Exception ex )
|
|
|
{
|
|
|
logger.E( ex );
|
|
|
- }
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
public void Dispose()
|
|
|
@@ -248,9 +250,9 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
}
|
|
|
|
|
|
object lockPulish = new object();
|
|
|
- public void Publish(string key, string val)
|
|
|
+ public void Publish( string key, string val )
|
|
|
{
|
|
|
- lock(lockPulish)
|
|
|
+ lock ( lockPulish )
|
|
|
this.pub.SendMoreFrame( key ).SendFrame( val );
|
|
|
}
|
|
|
|
|
|
@@ -437,22 +439,6 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- void Th_SubPoller()
|
|
|
- {
|
|
|
- while ( !this.threadCancel.Canceled )
|
|
|
- {
|
|
|
- LockUtils.Wait( 1000 );
|
|
|
- //this.pub.SendMoreFrame( "1000" ).SendFrame( "Test" );
|
|
|
- LockUtils.Wait( 100 );
|
|
|
-
|
|
|
- NetMQMessage msg = new NetMQMessage();
|
|
|
- if ( this.sub.TryReceiveMultipartMessage( TimeSpan.FromSeconds( 1 ), ref msg ) )
|
|
|
- {
|
|
|
- var m = msg;
|
|
|
- }
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
object lockObj = new object();
|
|
|
bool Request( string topic, string value, out string response )
|
|
|
{
|
|
|
@@ -585,34 +571,139 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
+ #region RequestSocket
|
|
|
+ List<string> GetRequest( string index )
|
|
|
+ {
|
|
|
+ var rll = RequestDrive( "getm", index );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ return rll;
|
|
|
+
|
|
|
+ rll.RemoveAt( 0 );
|
|
|
+
|
|
|
+ var ret = rll[0].Split( '/' );
|
|
|
+
|
|
|
+ List<string> retll = new List<string>();
|
|
|
+ foreach ( var item in ret )
|
|
|
+ {
|
|
|
+ if ( item.Equals( "ok" ) || item.Equals( "nok" ) || item.Equals( index ) )
|
|
|
+ continue;
|
|
|
+
|
|
|
+ retll.Add( item );
|
|
|
+ }
|
|
|
+
|
|
|
+ return retll;
|
|
|
+ }
|
|
|
+
|
|
|
+ List<string> RequestDrive( string key, string msg )
|
|
|
+ {
|
|
|
+ logger.D( $"[Request] - {key} / {msg}" );
|
|
|
+
|
|
|
+ var client = this.CreateRequestSocket( endServer );
|
|
|
+ client.SendMoreFrame( key ).SendFrame( msg );
|
|
|
+
|
|
|
+ var ll = new List<string>();
|
|
|
+
|
|
|
+ //if ( !client.Poll( TimeSpan.FromMilliseconds( 2000 ) ) )
|
|
|
+ // isSuccess = false;
|
|
|
+
|
|
|
+ if ( !client.TryReceiveMultipartStrings( TimeSpan.FromSeconds( 1 ), ref ll ) )
|
|
|
+ logger.D( $"[Reply Timeout] - {key} / {msg}" );
|
|
|
+
|
|
|
+ this.TerminateRequestSocket( client );
|
|
|
+
|
|
|
+ return ll;
|
|
|
+ }
|
|
|
+
|
|
|
+ RequestSocket CreateRequestSocket( string endServer )
|
|
|
+ {
|
|
|
+ var guid = Guid.NewGuid();
|
|
|
+ var client = new RequestSocket
|
|
|
+ {
|
|
|
+ Options =
|
|
|
+ {
|
|
|
+ Linger = TimeSpan.Zero,
|
|
|
+ Identity = Encoding.Unicode.GetBytes(guid.ToString())
|
|
|
+ }
|
|
|
+ };
|
|
|
+
|
|
|
+ client.Connect( endServer );
|
|
|
+ //client.ReceiveReady += Client_ReceiveReady;
|
|
|
+
|
|
|
+ return client;
|
|
|
+ }
|
|
|
+
|
|
|
+ void TerminateRequestSocket( RequestSocket client )
|
|
|
+ {
|
|
|
+ client.Disconnect( this.endServer );
|
|
|
+ client.Close();
|
|
|
+ }
|
|
|
+
|
|
|
+ private void Client_ReceiveReady( object sender, NetMQSocketEventArgs e )
|
|
|
+ {
|
|
|
+ var rsp = e.Socket.ReceiveMultipartStrings();
|
|
|
+
|
|
|
+ logger.D( $"[Reply] - {rsp[0]} / {rsp[1]}" );
|
|
|
+
|
|
|
+ switch ( rsp[0] )
|
|
|
+ {
|
|
|
+ case "3009":
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ #endregion
|
|
|
+
|
|
|
#region Get Request Method
|
|
|
public eOperatationMode GetOperationMode()
|
|
|
{
|
|
|
- List<string> rll;
|
|
|
- if ( !this.GetRequst( "111", out rll ) )
|
|
|
+ var rll = GetRequest( "111" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
{
|
|
|
logger.E( $"[Get Operation Mode] - Response Time Out" );
|
|
|
return eOperatationMode.InitialMode;
|
|
|
}
|
|
|
|
|
|
- if ( rll[0].Equals( "1" ) )
|
|
|
+ if ( rll[0].Equals( "4" ) )
|
|
|
return eOperatationMode.AutoMode;
|
|
|
else
|
|
|
return eOperatationMode.ManualMode;
|
|
|
+
|
|
|
+
|
|
|
+ //List<string> rll;
|
|
|
+ //if ( !this.GetRequst( "111", out rll ) )
|
|
|
+ //{
|
|
|
+ // logger.E( $"[Get Operation Mode] - Response Time Out" );
|
|
|
+ // return eOperatationMode.InitialMode;
|
|
|
+ //}
|
|
|
+
|
|
|
+ //if ( rll[0].Equals( "1" ) )
|
|
|
+ // return eOperatationMode.AutoMode;
|
|
|
+ //else
|
|
|
+ // return eOperatationMode.ManualMode;
|
|
|
}
|
|
|
|
|
|
public int GetCurrentPointNo()
|
|
|
{
|
|
|
int ret = 0;
|
|
|
- List<string> rll;
|
|
|
- if ( this.GetRequst( "3009", out rll ) )
|
|
|
- {
|
|
|
- int.TryParse( rll[0], out ret );
|
|
|
- }
|
|
|
- else
|
|
|
- logger.E( $"[Get Current Point] - Response Time Out" );
|
|
|
+
|
|
|
+ var rll = this.GetRequest( "3009" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ return 0;
|
|
|
+
|
|
|
+ if ( !int.TryParse( rll[0], out ret ) )
|
|
|
+ logger.E( $"[Get Current Point] - Try Parse Error" );
|
|
|
|
|
|
return ret;
|
|
|
+
|
|
|
+ //if ( this.GetRequst( "3009", out rll ) )
|
|
|
+ //{
|
|
|
+ // int.TryParse( rll[0], out ret );
|
|
|
+ //}
|
|
|
+ //else
|
|
|
+ // logger.E( $"[Get Current Point] - Response Time Out" );
|
|
|
+
|
|
|
+ //return ret;
|
|
|
}
|
|
|
#endregion
|
|
|
|
|
|
@@ -625,33 +716,55 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
else
|
|
|
cmd = "2";
|
|
|
|
|
|
- if ( !SetRequest( "3031", cmd ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"3031/{cmd}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
{
|
|
|
logger.E( $"[Set Start Move] - Response Time Out" );
|
|
|
return 38;
|
|
|
}
|
|
|
|
|
|
+ //if ( !SetRequest( "3031", cmd ) )
|
|
|
+ //{
|
|
|
+ // logger.E( $"[Set Start Move] - Response Time Out" );
|
|
|
+ // return 38;
|
|
|
+ //}
|
|
|
+
|
|
|
return ConstInt.EXECUTE_SUCCESS;
|
|
|
}
|
|
|
+
|
|
|
internal int SetTargetPoint( string point )
|
|
|
{
|
|
|
- if ( !SetRequest( "3033", point ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"3033/{point}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
{
|
|
|
logger.E( $"[Set TargetPoint] - Response Time Out" );
|
|
|
return 38;
|
|
|
}
|
|
|
|
|
|
+ //if ( !SetRequest( "3033", point ) )
|
|
|
+ //{
|
|
|
+ // logger.E( $"[Set TargetPoint] - Response Time Out" );
|
|
|
+ // return 38;
|
|
|
+ //}
|
|
|
+
|
|
|
return ConstInt.EXECUTE_SUCCESS;
|
|
|
}
|
|
|
|
|
|
- internal int SetTargetPointArray(string point )
|
|
|
+ internal int SetTargetPointArray( string point )
|
|
|
{
|
|
|
- if ( !SetRequest( "3034", point ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"3034/{point}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
{
|
|
|
logger.E( $"[Set TargetPoint] - Response Time Out" );
|
|
|
return 38;
|
|
|
}
|
|
|
|
|
|
+ //if ( !SetRequest( "3034", point ) )
|
|
|
+ //{
|
|
|
+ // logger.E( $"[Set TargetPoint] - Response Time Out" );
|
|
|
+ // return 38;
|
|
|
+ //}
|
|
|
+
|
|
|
return ConstInt.EXECUTE_SUCCESS;
|
|
|
}
|
|
|
|
|
|
@@ -677,9 +790,13 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
- if ( !this.SetRequest( "5001", val ) )
|
|
|
+
|
|
|
+ var rll = this.RequestDrive( "setm", $"5001/{val}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
logger.E( $"[Set Obstruction] - Response Time Out" );
|
|
|
|
|
|
+ //if ( !this.SetRequest( "5001", val ) )
|
|
|
+ // logger.E( $"[Set Obstruction] - Response Time Out" );
|
|
|
}
|
|
|
|
|
|
internal void SetOperationState( eOperatationMode mode )
|
|
|
@@ -688,14 +805,18 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
string val = string.Empty;
|
|
|
|
|
|
if ( mode == eOperatationMode.AutoMode )
|
|
|
- val = "1";
|
|
|
+ val = "2";
|
|
|
else if ( mode == eOperatationMode.ManualMode )
|
|
|
- val = "0";
|
|
|
+ val = "3";
|
|
|
else
|
|
|
- { }
|
|
|
+ return;
|
|
|
|
|
|
- if ( !SetRequest( "112", val ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"112/{val}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
logger.E( $"[Set Operation State] - Response Time Out" );
|
|
|
+
|
|
|
+ //if ( !SetRequest( "112", val ) )
|
|
|
+ // logger.E( $"[Set Operation State] - Response Time Out" );
|
|
|
}
|
|
|
|
|
|
internal void SetMCState( bool isOn )
|
|
|
@@ -708,8 +829,12 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
else
|
|
|
val = "0";
|
|
|
|
|
|
- if ( !this.SetRequest( "5000", val ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"5000/{val}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
logger.E( $"[Set MCState] - Response Time Out" );
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "5000", val ) )
|
|
|
+ // logger.E( $"[Set MCState] - Response Time Out" );
|
|
|
}
|
|
|
|
|
|
internal void SetCurrentSteeringState( eSteeringState state )
|
|
|
@@ -731,11 +856,150 @@ namespace VehicleControlSystem.ControlLayer.MQ
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
- if ( !this.SetRequest( "3060", val ) )
|
|
|
+ var rll = this.RequestDrive( "setm", $"3060/{val}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
logger.E( $"[Set Current Steering] - Response Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "3060", val ) )
|
|
|
+ // logger.E( $"[Set Current Steering] - Response Time Out" );
|
|
|
|
|
|
logger.D( $"[Steering_Req] - 3060/{val}" );
|
|
|
}
|
|
|
+
|
|
|
+ internal void SetSafetyBumper()
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "setm", $"6105/1" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "6105", "1" ) )
|
|
|
+ // logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
|
|
|
+
|
|
|
+ logger.D( $"[Set Safety Bumper] - Bumper is Pushed" );
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// forward 1, backward -1
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="isForward"></param>
|
|
|
+ internal void SetJogMove(bool isForward = true )
|
|
|
+ {
|
|
|
+ string v = string.Empty;
|
|
|
+ if ( isForward )
|
|
|
+ v = "1";
|
|
|
+ else
|
|
|
+ v = "-1";
|
|
|
+
|
|
|
+ var rll = this.RequestDrive( "setm", $"4091/1" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Jog Move] - Jog Move Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ #endregion
|
|
|
+
|
|
|
+ #region Get/Set Parameter
|
|
|
+ internal double GetJogSpeed()
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "getm", "4053" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Jog Speed] - Jog Speed Time Out" );
|
|
|
+ return 0d;
|
|
|
+ }
|
|
|
+
|
|
|
+ double ret = 0d;
|
|
|
+ double.TryParse( rll[0], out ret );
|
|
|
+
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ /// <summary>
|
|
|
+ /// 전진 후진 동시에 같은 값으로 설정.
|
|
|
+ /// </summary>
|
|
|
+ /// <param name="speed"></param>
|
|
|
+ internal void SetJogSpeed( double speed )
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "setm", $"4053/{speed}/{speed}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "6105", "1" ) )
|
|
|
+ // logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
|
|
|
+
|
|
|
+ logger.D( $"[Set Jog Speed] - Set Jog Speed {speed}" );
|
|
|
+ }
|
|
|
+
|
|
|
+ internal double GetDriveLinerSpeed()
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "getm", "3016" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
|
|
|
+ return 0d;
|
|
|
+ }
|
|
|
+
|
|
|
+ double ret = 0d;
|
|
|
+ double.TryParse( rll[0], out ret );
|
|
|
+
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+ internal double GetDriveCurveSpeed()
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "getm", "3016" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
|
|
|
+ return 0d;
|
|
|
+ }
|
|
|
+
|
|
|
+ double ret = 0d;
|
|
|
+ double.TryParse( rll[1], out ret );
|
|
|
+
|
|
|
+ return ret;
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void SetDriveSpeed( string liner, string curve )
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "setm", $"3016/{liner}/{curve}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "6105", "1" ) )
|
|
|
+ // logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
|
|
|
+
|
|
|
+ logger.D( $"[Set Drive Speed] - Set Drive Speed Liner{liner}, Curve{curve}" );
|
|
|
+ }
|
|
|
+
|
|
|
+ internal void SetDriveSpeedProfile( string accel, string decel, string creep, string creepDistance )
|
|
|
+ {
|
|
|
+ var rll = this.RequestDrive( "setm", $"3013/{accel}/{decel}/{creep}/{creepDistance}" );
|
|
|
+ if ( rll.Count == 0 )
|
|
|
+ {
|
|
|
+ logger.E( $"[Set Drive Speed] - Drive Speed Time Out" );
|
|
|
+ return;
|
|
|
+ }
|
|
|
+
|
|
|
+ //if ( !this.SetRequest( "6105", "1" ) )
|
|
|
+ // logger.E( $"[Set Safety Bumper] - Bumper is Pushed Time Out" );
|
|
|
+
|
|
|
+ logger.D( $"[Set Drive Speed] - Set Drive Speed Accel {accel}, Decel {decel}, Creep{creep}/{creepDistance}" );
|
|
|
+ }
|
|
|
+
|
|
|
#endregion
|
|
|
}
|
|
|
}
|