public void AddRobotInfo(RobotInfo ifo)
{
_robots.Add(ifo);
if (OnRobotAdded != null)
OnRobotAdded(this, ifo);
}
public void FillRepo(Robot[] allRobots)
{
robotsInfo.Clear();
robotsTypeCount.Clear();
foreach (Robot r in allRobots)
{
RobotInfo info = new RobotInfo();
info.Id = r.id;
info.IsFree = true;
info.Type = r.type;
info.Robot = r;
unsafe
{
info.InitialPosition = new Vector(r.position[0], r.position[1], r.position[2]);
info.InitialRotation = new Vector(r.rotation[0], r.rotation[1], r.rotation[2]);
}
robotsInfo.Add(r.id, info);
int value;
if (robotsTypeCount.TryGetValue(info.Type, out value))
{
robotsTypeCount[info.Type] = value + 1;
}
else
{
robotsTypeCount.Add(info.Type, 1);
}
}
}
public void Init(RobotInfo ifo)
{
if (!ovrPro.Open(COvrvisionUnity.OV_CAMVR_FULL, 0.15f))
{
return;
}
else
{
Thread.Sleep(100);
if (true)
{
ovrPro.SetExposure(conf_exposure);
ovrPro.SetGain(conf_gain);
ovrPro.SetBLC(conf_blc);
ovrPro.SetWhiteBalanceR(conf_wb_r);
ovrPro.SetWhiteBalanceG(conf_wb_g);
ovrPro.SetWhiteBalanceB(conf_wb_b);
ovrPro.SetWhiteBalanceAutoMode(conf_wb_auto);
}
}
Thread.Sleep(100);
UpdateOvrvisionSetting();
ovrPro.useOvrvisionTrack_Calib = true;
ovrPro.useProcessingQuality = COvrvisionUnity.OV_CAMQT_DMSRMP;
for (int i = 0; i < 2; ++i)
{
camTex [i] = new Texture2D(ovrPro.imageSizeW, ovrPro.imageSizeH, TextureFormat.BGRA32, false);
camTex [i].wrapMode = TextureWrapMode.Clamp;
camTex [i].Apply();
camPtr[i] = camTex [i].GetNativeTexturePtr();
}
_bufferID = 0;
}
public Robot(RobotInfo robotInfo, ParticleInfo particleInfo, SensorInfo sensorInfo, Environment env, bool doMapping = false)
{
r = new Random();
// Robot
Pose = new Pose(robotInfo.Location);
Meta = robotInfo;
// Sensor
Sensor = new Sensor(sensorInfo);
// Environment
Environment = env;
DoMapping = doMapping;
// Particle
ParticleMeta = particleInfo;
if (ParticleMeta.Count <= 0)
{
return;
}
Particles = new Particle[ParticleMeta.Count];
float w = 1.0f / ParticleMeta.Count;
for (int i = 0; i < ParticleMeta.Count; i++)
{
if (DoMapping)
{
Particles[i] = new Particle(Pose, w, ref Environment);
}
else
{
Particles[i] = new Particle(Pose, w);
}
}
}
public virtual void OnRobotStatusUpdated(RobotInfoManager mngr,RobotInfo ifo)
{
}
public virtual void OnRobotAdded(RobotInfoManager mngr, RobotInfo ifo)
{
}
int _Process()
{
if (_client == null)
return -1;
IPEndPoint ep=null;
byte[] data;
try
{
data=_client.Receive (ref ep);
}catch{
return 0;
}
if (data == null || data.Length == 0)
return 0;
var reader = new BinaryReader (new MemoryStream (data));
int msg = reader.ReadInt32 ();
switch (msg) {
case (int)Messages.Presence:
{
RobotInfo ifo=new RobotInfo();
ifo.Read(reader);
if(OnRobotInfoDetected!=null)
OnRobotInfoDetected(ifo);
}break;
case (int)Messages.DepthData:
break;
case (int)Messages.DepthSize:
break;
case (int)Messages.IsStereo:
break;
case (int)Messages.CameraConfig:
if(OnCameraConfig!=null)
OnCameraConfig(reader.ReadStringNative());
break;
case (int)Messages.ReportMessage:
if(OnReportMessage!=null)
{
int code=reader.ReadInt32();
string str=reader.ReadStringNative();
OnReportMessage(code,str);
}
break;
case (int)Messages.BumpSensorMessage:
if(OnBumpSensor!=null)
{
int count=reader.ReadInt32();
float[] values=new float[count];
for(int i=0;i<count;++i)
{
values[i]=reader.ReadSingle();
}
OnBumpSensor(values);
}
break;
case (int)Messages.IRSensorMessage:
if(OnIRSensor!=null)
{
int count=reader.ReadInt32();
float[] values=new float[count];
for(int i=0;i<count;++i)
{
values[i]=reader.ReadSingle();
}
OnIRSensor(values);
}
break;
case (int)Messages.BatteryLevel:
if(OnBatteryLevel!=null)
OnBatteryLevel(reader.ReadInt32());
break;
case (int)Messages.ClockSync:
break;
case (int)Messages.JointValues:
if(OnJointValues!=null)
{
int count=reader.ReadInt32();
float[] values=new float[count];
for(int i=0;i<count;++i)
{
values[i]=reader.ReadSingle();
}
OnJointValues(values);
}
break;
case (int)Messages.RobotStatus:
if(OnRobotStatus!=null)
{
int val=reader.ReadInt32();
OnRobotStatus((ERobotControllerStatus)val);
}
break;
case (int)Messages.NetValue:
if(OnServiceNetValue!=null)
{
string name=reader.ReadStringNative();
int port=reader.ReadInt32();
OnServiceNetValue(name,port);
}
break;
default:
if(OnMessage!=null)
OnMessage(msg,reader);
break;
}
return 0;
}
public void ConnectRobot(RobotInfo r)
{
if (_connected) {
if(r.IP==_RobotIP.IP)
return;
DisconnectRobot ();
}
_RobotIP = r;
RobotConnector.TargetPorts ports = new RobotConnector.TargetPorts ();
ports.CommPort = Settings.Instance.GetPortValue ("CommPort", 6000);
ports.RobotIP = _RobotIP.IP;
_connector.ConnectRobotIP (ports);
_connected=true;
/*
if (VideoStream != null) {
VideoStream.SetRemoteHost(_RobotIP.IP,ports);
}*/
if (OnRobotConnected!=null) {
OnRobotConnected(ports);
}
LogSystem.Instance.Log ("Connecting to Robot:" + _RobotIP.Name, LogSystem.LogType.Info);
}
public void SetRobotInfo(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
_robotIfo = ifo;
RobotConnector.Connector.SendData(TxKitEars.ServiceName, "AudioParameters", "", false, true);
}
public void Init(RobotInfo ifo)
{
Media = Resources.Load <MovieTexture> (Path.GetFileNameWithoutExtension(ifo.MediaPath));
Media.Play();
Media.loop = true;
}
void OnRobotConnected(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
SetMessage("Please wait to be connected");
PowerIcon.SetPowered(true);
}
public void SetRobotInfo(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
_robotIfo = ifo;
}/*
void OnRobotConnected(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
SetRobotInfo(ifo, ports);
_audioInited = false;
}
int _Process(byte[] data, IPEndPoint ep)
{
var reader = new BinaryReader(new MemoryStream(data));
int msg = reader.ReadInt32();
switch (msg)
{
case (int)Messages.Presence:
{
RobotInfo ifo = new RobotInfo();
ifo.Read(reader);
if (ep != null)
{
ifo.IP = ep.Address.ToString();
}
_OnRobotInfoDetected(ifo);
} break;
case (int)Messages.DepthData:
break;
case (int)Messages.DepthSize:
break;
case (int)Messages.IsStereo:
break;
case (int)Messages.CameraConfig:
_OnCameraConfig(reader.ReadStringNative());
break;
case (int)Messages.AudioConfig:
_OnAudioConfig(reader.ReadStringNative());
break;
case (int)Messages.AudioPlayerConfig:
_OnAudioPlayerConfig(reader.ReadStringNative());
break;
case (int)Messages.ReportMessage:
{
int code = reader.ReadInt32();
string str = reader.ReadStringNative();
_OnReportMessage(code, str);
}
break;
case (int)Messages.BumpSensorMessage:
{
int count = reader.ReadInt32();
float[] values = new float[count];
for (int i = 0; i < count; ++i)
{
values[i] = reader.ReadSingle();
}
_OnBumpSensor(values);
}
break;
case (int)Messages.IRSensorMessage:
{
int count = reader.ReadInt32();
float[] values = new float[count];
for (int i = 0; i < count; ++i)
{
values[i] = reader.ReadSingle();
}
_OnIRSensor(values);
}
break;
case (int)Messages.BatteryLevel:
_OnBatteryLevel(reader.ReadInt32());
break;
case (int)Messages.ClockSync:
break;
case (int)Messages.JointValues:
{
int count = reader.ReadInt32();
float[] values = new float[count];
for (int i = 0; i < count; ++i)
{
values[i] = reader.ReadSingle();
}
// Debug.Log ("Received joints values: " + count);
_OnJointValues(values);
}
break;
case (int)Messages.RobotStatus:
{
int val = reader.ReadInt32();
_OnRobotStatus((ERobotControllerStatus)val);
}
break;
case (int)Messages.NetValue:
{
string name = reader.ReadStringNative();
int port = reader.ReadInt32();
_OnServiceNetValue(name, port);
}
break;
case (int)Messages.Capabilities:
{
string caps = reader.ReadStringNative();
//Debug.Log (caps);
_OnRobotCapabilties(caps);
}
break;
default:
_OnMessage(msg, reader);
break;
}
return(0);
}
public void Init(RobotInfo ifo)
{
Init(ifo.CameraIndex);
}
public LocationInfo(Location location, bool isOnMap, bool isToxic, TileType tileType, RobotInfo robotInfo)
{
HasRobot = false;
Location = location;
RobotInfo = null;
IsToxic = isToxic;
IsOnMap = isOnMap;
TileType = tileType;
if (robotInfo != null)
{
HasRobot = true;
RobotInfo = robotInfo;
}
}
// Use this for initialization
void Start()
{
_connector = new RobotConnector ();
switch (HeadControllerType) {
case AppManager.HeadControllerType.Oculus:
default:
_connector.HeadController=new OculusHeadController();
break;
}
switch (BaseControllerType) {
case AppManager.BaseControllerType.Oculus:
default:
_connector.BaseController=new OculusBaseController();
break;
}
_connector.RobotCommunicator = new RemoteRobotCommunicator ();
_connector.DataCommunicator.OnMessage += OnMessage;
_connector.DataCommunicator.OnRobotInfoDetected += OnRobotInfoDetected;
_connector.DataCommunicator.OnRobotStatus += OnRobotStatus;
_connector.DataCommunicator.OnJointValues += OnJointValues;
_connector.DataCommunicator.OnBumpSensor += OnBumpSensor;
_connector.DataCommunicator.OnServiceNetValue += _OnServiceNetValue;
_connector.StartDataCommunicator ();
if(RobotIndex>=0)
_RobotIP=AppManager.Instance.RobotManager.GetRobotInfo(RobotIndex);
//Send Detect Message to scan network for the available robots
_connector.RobotCommunicator.SetData ("detect", _connector.DataCommunicator.Port.ToString(), true,false);
_connector.RobotCommunicator.BroadcastMessage (Settings.Instance.GetPortValue("CommPort",6000));
_connector.RobotCommunicator.RemoveData ("detect");
if (Debugger != null) {
Debugger.AddDebugElement(new DebugRobotStatus(this));
}
//VideoStream.SetConnectionComponent (this);
_OnStarted ();
}
void OnRobotInfoDetected(RobotInfo ifo)
{
Debug.Log ("Robot detected: " + ifo.IP);
if(_RobotIP==null)
_RobotIP = ifo;
}
public abstract bool Connect(RobotInfo ifo);
public void LoadRobots(string path)
{
XmlReader reader=XmlReader.Create(path);
if (reader == null) {
LogSystem.Instance.Log("RobotInfoManager::LoadRobots()- Failed to load Robots File:"+path,LogSystem.LogType.Error);
return;
}
long ID = 0;
while (reader.Read()) {
if(reader.Name=="Robot")
{
RobotInfo r=new RobotInfo();
r.LoadXML(reader);
r.ID=ID++;
_robots.Add(r);
}
}
}
public static void Show(this FrameworkElement Owner, RobotInfo infoPrev)
{
CarEdit.Show(Owner, infoPrev);
}
public static Rox.Robot ToToolboxRobot(RobotInfo robot_info, int chain_number)
{
CheckList(robot_info.chains, $"could not find kinematic chain number {chain_number}");
if (chain_number >= robot_info.chains.Count)
{
throw new ArgumentException($"invalid kinematic chain number {chain_number}");
}
var chain = robot_info.chains[chain_number];
int joint_count = chain.joint_numbers.Length;
for (int i = 1; i < joint_count; i++)
{
if (chain.joint_numbers[i - 1] >= chain.joint_numbers[i])
{
throw new ArgumentException($"joint numbers must be increasing in chain number {chain_number}");
}
if (chain.joint_numbers[i] >= robot_info.joint_info.Count)
{
throw new ArgumentException($"joint number out of bounds in chain number {chain_number}");
}
}
CheckList(chain.H, $"invalid shape for H in chain number {chain_number}", joint_count);
CheckList(chain.P, $"invalid shape for P in chain number {chain_number}", joint_count + 1);
var H = Matrix <double> .Build.Dense(3, joint_count);
for (int i = 0; i < joint_count; i++)
{
H[0, i] = chain.H[i].x;
H[1, i] = chain.H[i].y;
H[2, i] = chain.H[i].z;
}
var P = Matrix <double> .Build.Dense(3, joint_count + 1);
for (int i = 0; i < joint_count + 1; i++)
{
P[0, i] = chain.P[i].x;
P[1, i] = chain.P[i].y;
P[2, i] = chain.P[i].z;
}
var joint_type = new Rox.JointType[joint_count];
var joint_lower_limit = new double[joint_count];
var joint_upper_limit = new double[joint_count];
var joint_vel_limit = new double[joint_count];
var joint_acc_limit = new double[joint_count];
var joint_names = new string[joint_count];
for (int i = 0; i < joint_count; i++)
{
var j = robot_info.joint_info[i];
switch (j.joint_type)
{
case com.robotraconteur.robotics.joints.JointType.revolute:
joint_type[i] = Rox.JointType.Revolute;
break;
case com.robotraconteur.robotics.joints.JointType.prismatic:
joint_type[i] = Rox.JointType.Prismatic;
break;
default:
throw new ArgumentException($"invalid joint type: {j.joint_type}");
}
if (j.joint_limits == null)
{
throw new ArgumentException("joint_limits must not be null");
}
joint_lower_limit[i] = j.joint_limits.lower;
joint_upper_limit[i] = j.joint_limits.upper;
joint_vel_limit[i] = j.joint_limits.velocity;
joint_acc_limit[i] = j.joint_limits.acceleration;
joint_names[i] = j.joint_identifier?.name ?? "";
}
string root_link_name = chain.link_identifiers?.FirstOrDefault()?.name;
string tip_link_name = chain.flange_identifier?.name;
var tf_tool = GeometryConverter.ToTransform(chain.flange_pose);
var r_tool = tf_tool.R;
var p_tool = tf_tool.P;
var rox_robot = new Rox.Robot(H, P, joint_type, joint_lower_limit, joint_upper_limit, joint_vel_limit,
joint_acc_limit, null, r_tool, p_tool, joint_names, root_link_name, tip_link_name);
return(rox_robot);
}
public void Init(RobotInfo ifo)
{
Init();
}
void OnRobotConnected(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
SetRobotInfo(ifo, ports);
}
public virtual void OnRobotStatusUpdated(RobotInfoManager mngr, RobotInfo ifo)
{
}
void _OnRobotConnected(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
OnRobotConnected();
}
public RobotConnector()
{
_robotIfo = new RobotInfo ();
_ports = new TargetPorts ();
DataCommunicator = new RobotDataCommunicator ();
}
public void Init(RobotInfo ifo)
{
Init(ifo, "None");
}
void OnRobotConnected(RobotInfo ifo, RobotConnector.TargetPorts ports)
{
RobotCommunicator = RobotConnector.Connector.RobotCommunicator;
}
public virtual void OnRobotAdded(RobotInfoManager mngr, RobotInfo ifo)
{
}
public uint GetRobotInfo(uint nRobotID, ref RobotInfo myRobotInfo)
{
uint nValue = 0;
try
{
nValue = PTRobot_GetRobotInfo( nRobotID, myRobotInfo);
}
catch (Exception e)
{
MessageBox.Show("PTRobot_GetRobotInfo() failed:\n" + e.Message);
}
return nValue;
}
