CHOU
[MSRDS] Pioneer3DX 로봇과 실제 연결해서 작동해 보기 본문
예전에 테스트 목적으로 급하게 짜집기 한거라 조잡하기는 하지만 네이버 MSRS 카페를 보니까 Pioneer 로봇을 실제로 MSRDS와 같이 동작하는걸 궁금해 하시는 분이 많아서 간단한 설명과 함께 소스를 첨부합니다. 간단하게 TCP/IP 통신을 이용해서 Pioneer3DX 모터 서비스가 작동 되는 걸 해보겠습니다. 이것만 제대로 이해하면 나머지 서비스를 구현하는건 어렵지 않습니다.
1. 우선 포털 사이트에서 TCP/IP 코드를 구합니다. 뭐 워낙에 많아서 특별히 어디에 있다고는 언급하지 않겠습니다^^.
2. 저는 MSRDS R2 Sample 폴더에 있는 DifferentialDrive 예제를 이용해서 Pioneer3DX를 움직여 보겠습니다.
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using System;
using System.Collections.Generic;
using System.Xml;
using diffdrive = Microsoft.Robotics.Services.Drive.Proxy;
using W3C.Soap;
using simtypes = Microsoft.Robotics.Simulation;
using simengine = Microsoft.Robotics.Simulation.Engine;
using physics = Microsoft.Robotics.Simulation.Physics;
using System.ComponentModel;
using Microsoft.Dss.Core.DsspHttp;
//socket
using System.Collections;
using System.IO;
using System.Net;
using System.Net.Sockets;
using System.Threading;
namespace Microsoft.Robotics.Services.Simulation.Drive
{
/// <summary>
/// Provides access to a simulated differential drive service.\n(Uses the Generic Differential Drive contract.
/// </summary>
[DisplayName("(User) Simulated Generic Differential Drive")]
[AlternateContract(diffdrive.Contract.Identifier)]
[DssCategory(simtypes.PublishedCategories.SimulationService)]
[Contract(Contract.Identifier)]
[DssServiceDescription("http://msdn.microsoft.com/library/cc998469.aspx")]
public class SimulatedDifferentialDriveService : Microsoft.Dss.ServiceModel.DsspServiceBase.DsspServiceBase
{
/// <summary>
/// tcp ip variable declaration
/// </summary>
private TcpListener ServerTCPListner;
private static long ConnectedSockedCount = 0;
private Hashtable HTSocketHolder = new Hashtable();
private Hashtable HTThreadHolder = new Hashtable();
private IPAddress ServerIPAddress;
private int MaximumConnected = 5000;
private Socket TCPListnerAcceptSocket;
private int PORT;
//koko made^^;
#region Simulation Variables
simengine.SimulationEnginePort _simEngine;
simengine.DifferentialDriveEntity _entity;
simengine.SimulationEnginePort _notificationTarget;
#endregion
[Partner("SubMgr", Contract = submgr.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.CreateAlways)]
submgr.SubscriptionManagerPort _subMgrPort = new submgr.SubscriptionManagerPort();
string _subMgrUri = string.Empty;
[InitialStatePartner(Optional = true)]
private diffdrive.DriveDifferentialTwoWheelState _state = new diffdrive.DriveDifferentialTwoWheelState();
[ServicePort("/SimulatedDifferentialDrive", AllowMultipleInstances=true)]
private diffdrive.DriveOperations _mainPort = new diffdrive.DriveOperations();
/// <summary>
/// SimulatedDifferentialDriveService constructor that takes a PortSet to
/// notify when the service is created
/// </summary>
/// <param name="creationPort"></param>
public SimulatedDifferentialDriveService(DsspServiceCreationPort creationPort) :
base(creationPort)
{
}
/// <summary>
/// Start initializes service state and listens for drop messages
/// </summary>
protected override void Start()
{
// Find our simulation entity that represents the "hardware" or real-world service.
// To hook up with simulation entities we do the following steps
// 1) have a manifest or some other service create us, specifying a partner named SimulationEntity
// 2) in the simulation service (us) issue a subscribe to the simulation engine looking for
// an instance of that simulation entity. We use the Entity.State.Name for the match so it must be
// exactly the same. See SimulationTutorial2 for the creation process
// 3) Listen for a notification telling us the entity is available
// 4) cache reference to entity and communicate with it issuing low level commands.
_simEngine = simengine.SimulationEngine.GlobalInstancePort;
_notificationTarget = new simengine.SimulationEnginePort();
if (_state == null)
CreateDefaultState();
// enabled by default
_state.IsEnabled = true;
// PartnerType.Service is the entity instance name.
_simEngine.Subscribe(ServiceInfo.PartnerList, _notificationTarget);
// dont start listening to DSSP operations, other than drop, until notification of entity
Activate(new Interleave(
new TeardownReceiverGroup
(
Arbiter.Receive<simengine.InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
Arbiter.Receive<DsspDefaultDrop>(false, _mainPort, DefaultDropHandler)
),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup()
));
SocketServerStart();
}
/// <summary>
/// TCP IP Socket programming part.. this function send to data
/// </summary>
///
void SocketServerStart()
{
PORT = Int32.Parse("60000");
ServerIPAddress = IPAddress.Parse("192.168.0.163");
ServerTCPListner = new TcpListener(ServerIPAddress, PORT);
ServerTCPListner.Start();
System.Console.WriteLine("OK!! Server is Start");
Thread TCPServerThread = new Thread(new ThreadStart(WaitingFromClientConnect));
HTThreadHolder.Add(ConnectedSockedCount, TCPServerThread);
TCPServerThread.Start();
}
void WaitingFromClientConnect()
{
while (true)
{
TCPListnerAcceptSocket = ServerTCPListner.AcceptSocket();
if (ConnectedSockedCount < 5000)
{
Interlocked.Increment(ref ConnectedSockedCount);
}
else
{
ConnectedSockedCount = 1;
}
if (HTSocketHolder.Count < MaximumConnected)
{
while (HTSocketHolder.Contains(ConnectedSockedCount))
{
Interlocked.Increment(ref ConnectedSockedCount);
}
Thread myServerThread = new Thread(new ThreadStart(ReadingServerSocket));
lock (this)
{
HTSocketHolder.Add(ConnectedSockedCount, TCPListnerAcceptSocket);
HTThreadHolder.Add(ConnectedSockedCount, myServerThread);
}
myServerThread.Start();
}
}
}
void ReadingServerSocket()
{
long RealConnectedSocket = ConnectedSockedCount;
Socket ReadServerSocket = (Socket)HTSocketHolder[RealConnectedSocket];
while (true)
{
if (ReadServerSocket.Connected)
{
Byte[] ReceiveByte = new Byte[1024];
//try
//{
int nVlaue = ReadServerSocket.Receive(ReceiveByte, ReceiveByte.Length, 0);
if (nVlaue > 0)
{
string ReceiveData = null;
ReceiveData = System.Text.Encoding.Unicode.GetString(ReceiveByte);
System.Console.WriteLine(ReceiveData);
}
else
{
break;
}
//}
//catch (Exception ex)
//{
// if (!ReadServerSocket.Connected)
// {
// System.Console.WriteLine("Client is finished!! bye~~");
// break;
// }
//}
}
}
StopTheThread(RealConnectedSocket);
}
void StopTheThread(long RealConnectedSocket)
{
Thread RemoveThread = (Thread)HTThreadHolder[RealConnectedSocket];
lock (this)
{
HTSocketHolder.Remove(RealConnectedSocket);
HTThreadHolder.Remove(RealConnectedSocket);
}
RemoveThread.Abort();
}
//---------------TCP/IP Functions Finish Line------------------//
void CreateDefaultState()
{
_state = new diffdrive.DriveDifferentialTwoWheelState();
_state.LeftWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.RightWheel = new Microsoft.Robotics.Services.Motor.Proxy.WheeledMotorState();
_state.LeftWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
_state.RightWheel.MotorState = new Microsoft.Robotics.Services.Motor.Proxy.MotorState();
}
void UpdateStateFromSimulation()
{
if (_entity != null)
{
_state.TimeStamp = DateTime.Now;
_state.LeftWheel.MotorState.CurrentPower = _entity.LeftWheel.Wheel.MotorTorque;
_state.RightWheel.MotorState.CurrentPower = _entity.RightWheel.Wheel.MotorTorque;
_state.IsEnabled = _entity.IsEnabled;
}
}
void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)
{
InsertEntityNotificationHandler(ins);
base.Start();
// Listen on the main port for requests and call the appropriate handler.
MainPortInterleave.CombineWith(
new Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<simengine.InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive<simengine.DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler)
),
new ConcurrentReceiverGroup()
)
);
}
void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
{
_entity = (simengine.DifferentialDriveEntity)ins.Body;
_entity.ServiceContract = Contract.Identifier;
// create default state based on the physics entity
if(_entity.ChassisShape != null)
_state.DistanceBetweenWheels = _entity.ChassisShape.BoxState.Dimensions.X;
_state.LeftWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
_state.RightWheel.MotorState.PowerScalingFactor = _entity.MotorTorqueScaling;
//SpawnIterator(TestDriveDistanceAndRotateDegrees);
}
void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
{
_entity = null;
}
/// <summary>
/// Get handler retrieves service state
/// </summary>
/// <param name="get"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
public IEnumerator<ITask> GetHandler(HttpGet get)
{
UpdateStateFromSimulation();
get.ResponsePort.Post(new HttpResponseType(_state));
yield break;
}
/// <summary>
/// Get handler retrieves service state
/// </summary>
/// <param name="get"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
public IEnumerator<ITask> GetHandler(diffdrive.Get get)
{
UpdateStateFromSimulation();
get.ResponsePort.Post(_state);
yield break;
}
#region Subscribe Handling
/// <summary>
/// Subscribe to Differential Drive service
/// </summary>
/// <param name="subscribe"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
public IEnumerator<ITask> SubscribeHandler(diffdrive.Subscribe subscribe)
{
Activate(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
},
delegate(Exception ex) { LogError(ex); }
));
yield break;
}
/// <summary>
/// Subscribe to Differential Drive service
/// </summary>
/// <param name="subscribe"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
public IEnumerator<ITask> ReliableSubscribeHandler(diffdrive.ReliableSubscribe subscribe)
{
Activate(Arbiter.Choice(
SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_subMgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null));
},
delegate(Exception ex) { LogError(ex); }
));
yield break;
}
#endregion
/// <summary>
/// Handler for drive request
/// </summary>
/// <param name="driveDistance"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> DriveDistanceHandler(diffdrive.DriveDistance driveDistance)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
if (!_state.IsEnabled)
{
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("DriveDistance request to disabled drive.");
yield break;
}
if ((driveDistance.Body.Power > 1.0f) || (driveDistance.Body.Power < -1.0f))
{
// invalid drive power
driveDistance.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in DriveDistanceHandler.");
yield break;
}
_state.DriveDistanceStage = driveDistance.Body.DriveDistanceStage;
if (driveDistance.Body.DriveDistanceStage == diffdrive.DriveStage.InitialRequest)
{
Port<simengine.OperationResult> entityResponse = new Port<simengine.OperationResult>();
Activate(Arbiter.Receive<simengine.OperationResult>(false, entityResponse, delegate(simengine.OperationResult result)
{
// post a message to ourselves indicating that the drive distance has completed
diffdrive.DriveDistanceRequest req = new diffdrive.DriveDistanceRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.DriveDistanceStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.DriveDistanceStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.DriveDistance(req));
}));
_entity.DriveDistance((float)driveDistance.Body.Distance, (float)driveDistance.Body.Power, entityResponse);
diffdrive.DriveDistanceRequest req2 = new diffdrive.DriveDistanceRequest(0, 0);
req2.DriveDistanceStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.DriveDistance(req2));
}
else
{
base.SendNotification(_subMgrPort, driveDistance);
}
driveDistance.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Handler for rotate request
/// </summary>
/// <param name="rotate"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> RotateHandler(diffdrive.RotateDegrees rotate)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
if (!_state.IsEnabled)
{
rotate.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("RotateDegrees request to disabled drive.");
yield break;
}
_state.RotateDegreesStage = rotate.Body.RotateDegreesStage;
if (rotate.Body.RotateDegreesStage == diffdrive.DriveStage.InitialRequest)
{
Port<simengine.OperationResult> entityResponse = new Port<simengine.OperationResult>();
Activate(Arbiter.Receive<simengine.OperationResult>(false, entityResponse, delegate(simengine.OperationResult result)
{
// post a message to ourselves indicating that the drive distance has completed
diffdrive.RotateDegreesRequest req = new diffdrive.RotateDegreesRequest(0, 0);
switch (result)
{
case simengine.OperationResult.Error:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Canceled:
req.RotateDegreesStage = diffdrive.DriveStage.Canceled;
break;
case simengine.OperationResult.Completed:
req.RotateDegreesStage = diffdrive.DriveStage.Completed;
break;
}
_mainPort.Post(new diffdrive.RotateDegrees(req));
}));
_entity.RotateDegrees((float)rotate.Body.Degrees, (float)rotate.Body.Power, entityResponse);
diffdrive.RotateDegreesRequest req2 = new diffdrive.RotateDegreesRequest(0, 0);
req2.RotateDegreesStage = diffdrive.DriveStage.Started;
_mainPort.Post(new diffdrive.RotateDegrees(req2));
}
else
{
base.SendNotification(_subMgrPort, rotate);
}
rotate.ResponsePort.Post(DefaultUpdateResponseType.Instance);
yield break;
}
/// <summary>
/// Handler for setting the drive power
/// </summary>
/// <param name="setPower"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> SetPowerHandler(diffdrive.SetDrivePower setPower)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
if (!_state.IsEnabled)
{
setPower.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("SetPower request to disabled drive.");
yield break;
}
if ((setPower.Body.LeftWheelPower > 1.0f) || (setPower.Body.LeftWheelPower < -1.0f) ||
(setPower.Body.RightWheelPower > 1.0f) || (setPower.Body.RightWheelPower < -1.0f))
{
// invalid drive power
setPower.ResponsePort.Post(Fault.FromException(new Exception("Invalid Power parameter.")));
LogError("Invalid Power parameter in SetPowerHandler.");
yield break;
}
// Call simulation entity method for setting wheel torque
_entity.SetMotorTorque(
(float)(setPower.Body.LeftWheelPower),
(float)(setPower.Body.RightWheelPower));
//insert debug
//System.Console.WriteLine(setPower.Body.LeftWheelPower);
//System.Console.WriteLine(setPower.Body.RightWheelPower);
float HWLeft = (float)(setPower.Body.LeftWheelPower);
float HWRight = (float)(setPower.Body.RightWheelPower);
setHardwarePowerHandler(HWLeft, HWRight);
//UpdateStateFromSimulation();
setPower.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
void setHardwarePowerHandler(float leftPower, float rightPower)
{
string direction = "";
float temp = leftPower - rightPower;
//hardware action part
if ((leftPower == 0) && (rightPower == 0))
{
direction = "b";
System.Console.WriteLine("Stop!!");
}//stop action
else if ((leftPower > 0) && (rightPower > 0))
{
if ((temp > -0.02) && (temp < 0.02))
{
direction = "g";
System.Console.WriteLine("Go Straight!!");
}//go Straight
else if (temp > 0.02)
{
direction = "L";
System.Console.WriteLine("Turn Right!!");
}//turn right
else if (temp < -0.02)
{
direction = "R";
System.Console.WriteLine("Turn Left!!");
}//turn left
}//positive action
//data sending
if (TCPListnerAcceptSocket.Connected)
{
Byte[] SendBuffer;
string strBuffer;
strBuffer = String.Format("{0}", direction);
SendBuffer = System.Text.Encoding.Unicode.GetBytes(strBuffer);
TCPListnerAcceptSocket.Send(SendBuffer, 0, SendBuffer.Length, 0);
System.Console.WriteLine("send to the message!!");
}
}
/// <summary>
/// Handler for setting the drive speed
/// </summary>
/// <param name="setSpeed"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> SetSpeedHandler(diffdrive.SetDriveSpeed setSpeed)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
if (!_state.IsEnabled)
{
setSpeed.ResponsePort.Post(Fault.FromException(new Exception("Drive is not enabled.")));
LogError("SetSpeed request to disabled drive.");
yield break;
}
_entity.SetVelocity(
(float)setSpeed.Body.LeftWheelSpeed,
(float)setSpeed.Body.RightWheelSpeed);
//System.Console.WriteLine(setSpeed.Body.LeftWheelSpeed);
//System.Console.WriteLine(setSpeed.Body.RightWheelSpeed);
UpdateStateFromSimulation();
setSpeed.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update notification for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
/// <summary>
/// Handler for enabling or disabling the drive
/// </summary>
/// <param name="enable"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Concurrent)]
public IEnumerator<ITask> EnableHandler(diffdrive.EnableDrive enable)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
_state.IsEnabled = enable.Body.Enable;
_entity.IsEnabled = _state.IsEnabled;
UpdateStateFromSimulation();
enable.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
/// <summary>
/// Handler when the drive receives an all stop message
/// </summary>
/// <param name="estop"></param>
/// <returns></returns>
[ServiceHandler(ServiceHandlerBehavior.Exclusive)]
public IEnumerator<ITask> AllStopHandler(diffdrive.AllStop estop)
{
if (_entity == null)
throw new InvalidOperationException("Simulation entity not registered with service");
_entity.SetMotorTorque(0,0);
_entity.SetVelocity(0);
// AllStop disables the drive
_entity.IsEnabled = false;
UpdateStateFromSimulation();
estop.ResponsePort.Post(DefaultUpdateResponseType.Instance);
// send update for entire state
_subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest));
yield break;
}
// Test the DriveDistance and RotateDegrees messages
IEnumerator<ITask> TestDriveDistanceAndRotateDegrees()
{
yield return Arbiter.Choice(
_mainPort.RotateDegrees(90, 0.2),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f) { LogInfo(LogGroups.Console, "RotateDegrees Fault"); }
);
yield return Arbiter.Choice(
_mainPort.RotateDegrees(-90, 0.2),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault f) { LogInfo(LogGroups.Console, "RotateDegrees Fault"); }
);
}
}
}
샘플폴더를 열어서 SimulatedDifferentialDrive.cs 중에서 위에 빨간 부분을 추가해 주면 하드웨어와 통신 완료 입니다. 코드를 짜집기 하고 컨트롤도 간단하게 하느라 후진 동작도 없지만 저런 식으로 그냥 해당 서비스 값만 넘겨주거나 받거나 하면 되기 때문에 실제 코드 한번 보시면 이해하시는데는 큰 무리가 없을거라 생각합니다.
현재 센서, 비전 등의 각 종 서비스를 종합해서 실제 미로를 MSRDS를 이용해서 찾는 프로그램을 만들고 있는데 개발이 완료되는데로 코드와 함께 설명 올리도록 하겠습니다.
우선은 처음 연결하시는 법을 너무 난해해 하시는거 같아서 이해하기 쉽게 코드 짜집기를 보여드렸습니다^^. 코드가 더러워도 양해 바랍니다^^.
