private void init()
{
try
{
rp = new RobotPostureProxy(ip, 9559);
rp.goToPosture("StandZero", 1);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
try
{
mp = new MotionProxy(ip, port);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
RArm = new Arms();
LArm = new Arms();
}
public static void MoveHead(MotionProxy motionProxy, float?headPitch, float?headYaw, bool isAbsolute, float time = 1.5f)
{
if (motionProxy == null || (!headPitch.HasValue && !headYaw.HasValue))
{
return;
}
List <string> names = new List <string>();
List <float[]> times = new List <float[]>();
List <float[]> keys = new List <float[]>();
if (headPitch.HasValue)
{
names.Add("HeadPitch");
keys.Add(new[] { headPitch.Value });
times.Add(new[] { time });
}
if (headYaw.HasValue)
{
names.Add("HeadYaw");
keys.Add(new[] { headYaw.Value });
times.Add(new[] { time });
}
var methodID = motionProxy.post.angleInterpolation(names, keys, times, isAbsolute);
motionProxy.wait(methodID, 0);
}
public void Connect(string ip)
{
try
{
_motion = new MotionProxy(ip,9559);
// --------------- prepare limits --------------------------
// three floats as an ArrayList for each joint in the chain
// min,max,maxNoLoadSpeedPerCycle
ArrayList limits = (ArrayList)_motion.getLimits("Head");
ArrayList yawLimits = (ArrayList)limits[0];
ArrayList pitchLimits = (ArrayList)limits[1];
_minYaw = (float)yawLimits[0];
_maxYaw = (float)yawLimits[1];
_minPitch = (float)pitchLimits[0];
_maxPitch = (float)pitchLimits[1];
Console.WriteLine("MinYaw: " + _minYaw + " MaxYaw:" + _maxYaw + " MinPitch: " + _minPitch + " MaxPitch:" + _maxPitch);
// give the joints some stiffness
_motion.stiffnessInterpolation("Head",1.0f,1.0f);
}
catch(Exception e)
{
Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
}
}
private void init()
{
try
{
rp = new RobotPostureProxy(ip, 9559);
rp.goToPosture("StandZero", 1);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
try
{
mp = new MotionProxy(ip, port);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
RArm = new Arms();
LArm = new Arms();
}
public void init()
{
//make sure nao is ready to move
try
{
rp = new RobotPostureProxy(ip, 9559);
rp.goToPosture("StandZero", 1);
Console.WriteLine("Connected to Nao with IP: " + ip);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
try
{
mp = new MotionProxy(ip, port);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
Arms RArm = new Arms();
}
static void ExecuteOnNao(List<float[]> naoSeq, bool isJointAction)
{
MotionProxy mp = new MotionProxy("127.0.0.1", 9559);
// do stuff with naoskeletons and real NAO
foreach (float[] act in naoSeq) {
float LShoulderRoll = act[(int)AngleConverter.NaoJointAngle.LShoulderRoll];
float LShoulderPitch = act[(int)AngleConverter.NaoJointAngle.LShoulderPitch];
float LElbowYaw = act[(int)AngleConverter.NaoJointAngle.LElbowYaw];
float LElbowRoll = act[(int)AngleConverter.NaoJointAngle.LElbowRoll];
Console.WriteLine("LShoulderRoll: " + LShoulderRoll);
Console.WriteLine("LShoulderPitch: " + LShoulderPitch);
/* mp.setAngles("LElbowYaw", LElbowYaw, 0.4f);
mp.setAngles("LElbowRoll", LElbowRoll, 0.4f);
mp.setAngles("LShoulderRoll", LShoulderRoll, 0.4f);
mp.setAngles("LShoulderPitch", LShoulderPitch, 0.4f);*/
String[] joints = new String[(int)AngleConverter.NaoJointAngle.count];
joints[(int)AngleConverter.NaoJointAngle.LElbowRoll] = "LElbowRoll";
joints[(int)AngleConverter.NaoJointAngle.LElbowYaw] = "LElbowYaw";
joints[(int)AngleConverter.NaoJointAngle.LShoulderPitch] = "LShoulderPitch";
joints[(int)AngleConverter.NaoJointAngle.LShoulderRoll] = "LShoulderRoll";
joints[(int)AngleConverter.NaoJointAngle.RElbowRoll] = "RElbowRoll";
joints[(int)AngleConverter.NaoJointAngle.RElbowYaw] = "RElbowYaw";
joints[(int)AngleConverter.NaoJointAngle.RShoulderPitch] = "RShoulderPitch";
joints[(int)AngleConverter.NaoJointAngle.RShoulderRoll] = "RShoulderRoll";
mp.angleInterpolationWithSpeed(joints, act, 0.3f);
System.Threading.Thread.Sleep(50);
}
Console.Read();
}
public Bewegen(RobotPostureProxy rpp, MotionProxy motion, MainWindow mw, TextToSpeechProxy tts)
{
this.rpp = rpp;
this.motion = motion;
this.mw = mw;
this.tts = tts;
}
public void init()
{
//make sure nao is ready to move
try
{
rp = new RobotPostureProxy(ip, 9559);
rp.goToPosture("StandZero", 1);
Console.WriteLine("Connected to Nao with IP: " + ip);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
try
{
mp = new MotionProxy(ip, port);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
Arms RArm = new Arms();
}
public NaoKinectProxy(String ip, int port)
{
_naoIP = ip;
_naoPort = port;
//Initiating connection and balancing Nao
_naoMotion = new MotionProxy(_naoIP, port);
}
public bool connect(string ip, bool onlymotion = false)
{
this.IP = ip;
bool b = false;
try
{
proxyMotion = new MotionProxy(ip, Port);
if (onlymotion == false)
{
bool bc = proxyMotion.setCollisionProtectionEnabled("Arms", false);
if (!bc)
Console.WriteLine("Failed to disable collision protection");
StartMotorMonitor();
proxyBehaviorManager = new BehaviorManagerProxy(ip, Port);
// Turn off the LEDs
proxyLed = new LedsProxy(ip, Port);
List<string> ledgroups = proxyLed.listGroups();
foreach (string ledgroup in ledgroups)
{
proxyLed.off(ledgroup);
}
// Speech
proxyTTS = new TextToSpeechProxy(ip, Port);
if (ip != "127.0.0.1")
{
// Sentinel
proxySentinel = new SentinelProxy(ip, Port);
// Turning off "Motor hot!"
proxySentinel.enableHeatMonitoring(false);
}
// camera
proxyCamera = new VideoDeviceProxy(ip, Port);
// IdleMovement
InitIdleMovement();
}
b = true;
}
catch (Exception e)
{
Console.WriteLine("MotionProxy.Connect Exception: " + e);
b = false;
}
IsConnected = b;
return b;
}
public static void StiffnessOn(MotionProxy proxy)
{
// We use the "Body" name to signify the collection of all joints
var pNames = "Body";
var pStiffnessLists = 1.0f;
var pTimeLists = 1.0f;
proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists);
}
//Control RArm with all Joints
public static void controlRArm(MotionProxy mp, float RSP, float RSR, float RER, float REY, float RWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" };
float[] newangles = { RSP, RSR, RER, REY, RWY };
float fractionMaxSpeed = 0.1f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
public Gaze(string ip, int port)
: this()
{
this.ip = ip;
this.port = port;
motion = new MotionProxy(ip, port);
bezierMotion = new Bezier(ip, port);
}
//Control RArm with all Joints
public static void controlRArm(MotionProxy mp, float RSP, float RSR, float RER, float REY, float RWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" };
float[] newangles = { RSP, RSR, RER, REY, RWY };
float fractionMaxSpeed = 0.1f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
private void button_connect_to_IP_Click(object sender, EventArgs e)
{
this.tts = new TextToSpeechProxy(this.textBox_IP.Text, 9559);
this.motion = new MotionProxy(this.textBox_IP.Text, 9559);
this.BackColor = Color.AliceBlue;
this.tts.say("connected.");
this.motion.setStiffnesses("Body", 0);
}
//Control LArm with all Joints
public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" };
float[] newangles = { LSP, LSR, LER, LEY, LWY };
float fractionMaxSpeed = 0.1f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
//Control LArm with all Joints
public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" };
float[] newangles = { LSP, LSR, LER, LEY, LWY };
float fractionMaxSpeed = 0.1f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
/// <summary>
/// Default constructor.
/// </summary>
public Grabber()
{
NaoState.Instance.OnConnect += BuildCameraAndProcessor;
NaoState.Instance.OnDisconnect += ClearCameraAndProcessor;
if (NaoState.Instance.Connected)
{
BuildCameraAndProcessor(NaoState.Instance.IP.ToString(), NaoState.Instance.Port);
}
motion = Proxies.GetProxy <MotionProxy>();
posture = Proxies.GetProxy <RobotPostureProxy>();
}
public void Initialisierung(String ip, Int32 port)
{
motion = new MotionProxy(ip, port);
rpp = new RobotPostureProxy(ip, port);
tts = new TextToSpeechProxy(ip, port);
Bewegen = new Bewegen(rpp, motion, mw, tts);
Start Start = new Start();
//Nao geht in die Startposition
Start.Startposition(rpp, tts);
}
public void Initialisierung(String ip, Int32 port)
{
motion = new MotionProxy(ip, port);
rpp = new RobotPostureProxy(ip, port);
tts = new TextToSpeechProxy(ip, port);
Nao_Start Nao_Start = new Nao_Start();
//Nao geht in die Startposition
Nao_Start.Startposition(rpp, tts);
}
private void button_connect_Click(object sender, EventArgs e)
{
string ip = this.textBox1.Text;
this.tts = new TextToSpeechProxy(ip, 9559);
this.motion = new MotionProxy(ip, 9559);
this.posture = new RobotPostureProxy(ip, 9559);
this.navigation = new NavigationProxy(ip, 9559);
Console.Beep();
this.label1.Text += "\nconnected.";
}
public NeutralMovement(MotionProxy motionproxy, object LockMotion, MovingSettings ms,
LedsProxy ledsproxy, object LockLeds)
{
setting = ms;
ALMotion = motionproxy;
this.LockMotion = LockMotion;
ALLeds = ledsproxy;
this.LockLeds = LockLeds;
mItems = setting.WhatToMove;
}
public void Connect(string ip)
{
try
{
_motion = new MotionProxy(ip, 9559);
// --------------- prepare limits --------------------------
// three floats as an ArrayList for each joint in the chain
// min,max,maxNoLoadSpeedPerCycle
ArrayList rightArmLimits = (ArrayList)_motion.getLimits("RArm");
ArrayList RSPitchLimits = (ArrayList)rightArmLimits[0];
_RSminPitch = (float)RSPitchLimits[0];
_RSmaxPitch = (float)RSPitchLimits[1];
ArrayList RSRollLimits = (ArrayList)rightArmLimits[1];
_RSminRoll = (float)RSRollLimits[0];
_RSmaxRoll = (float)RSRollLimits[1];
ArrayList REYawLimits = (ArrayList)rightArmLimits[2];
_REminYaw = (float)REYawLimits[0];
_REmaxYaw = (float)REYawLimits[1];
ArrayList RERollLimits = (ArrayList)rightArmLimits[3];
_REminRoll = (float)RERollLimits[0];
_REmaxRoll = (float)RERollLimits[1];
ArrayList leftArmLimits = (ArrayList)_motion.getLimits("LArm");
ArrayList LSPitchLimits = (ArrayList)leftArmLimits[0];
_LSminPitch = (float)LSPitchLimits[0];
_LSmaxPitch = (float)LSPitchLimits[1];
ArrayList LSRollLimits = (ArrayList)leftArmLimits[1];
_LSminRoll = (float)LSRollLimits[0];
_LSmaxRoll = (float)LSRollLimits[1];
ArrayList LEYawLimits = (ArrayList)leftArmLimits[2];
_LEminYaw = (float)LEYawLimits[0];
_LEmaxYaw = (float)LEYawLimits[1];
ArrayList LERollLimits = (ArrayList)leftArmLimits[3];
_LEminRoll = (float)LERollLimits[0];
_LEmaxRoll = (float)LERollLimits[1];
// give the joints some stiffness
_motion.stiffnessInterpolation("RArm", 1.0f, 1.0f);
_motion.stiffnessInterpolation("LArm", 1.0f, 1.0f);
}
catch (Exception e)
{
Console.Out.WriteLine("Elbow.Connect exception: " + e);
}
}
public NaoController(string ip)
{
try
{
this.ip = ip;
proxy = new MotionProxy(ip, 9559);
this.setStiffness(1.0f);
}
catch (Exception e)
{
Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
}
}
public ToNao2(String IP, int port, Kinect kinect, int avg)
{
this.motioProxy = new MotionProxy(IP, port);
this.memProxy = new MemoryProxy(IP, port);
this.postureproxy = new RobotPostureProxy(IP, port);
this.sensor = kinect;
this.jointAngles = new Dictionary <string, float>();
this.avgPoint = new Dictionary <JointType, SkeletonPoint>();
this.avg = avg;
this.iteration = 0;
setUpjointAngleDictionary();
}
private bool Rotate(MotionProxy motionProxy, ref float actualRotation)
{
if (actualRotation > this.MaxRotation)
{
return(false);
}
motionProxy.moveInit();
motionProxy.moveTo(0f, 0f, this.RotationAmount);
actualRotation += this.RotationAmount;
return(true);
}
public ToNao2(String IP, int port, Kinect kinect, int avg)
{
this.motioProxy = new MotionProxy(IP, port);
this.memProxy = new MemoryProxy(IP, port);
this.postureproxy = new RobotPostureProxy(IP, port);
this.sensor = kinect;
this.jointAngles = new Dictionary<string, float>();
this.avgPoint = new Dictionary<JointType, SkeletonPoint>();
this.avg = avg;
this.iteration = 0;
setUpjointAngleDictionary();
}
//Control LArm with all Joints
public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" };
float[] newangles = { LSP, LSR, LER, LEY, LWY};
//Make sure floats are in range
if (ShoulderPitch.ContainsValue(LSP) && LShoulderRoll.ContainsValue(LSR) && LElbowRoll.ContainsValue(LER) && ElbowYaw.ContainsValue(LEY))
{
float fractionMaxSpeed = 0.2f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
else return;
}
public void Disconnect()
{
try
{
if (naoMotion != null)
{
// unsubscribe so the NAO knows we do not need data from the Motion anymore
naoMotion.stiffnessInterpolation("Head", 0f, 0f);
naoMotion.stiffnessInterpolation("LArm", 0f, 0f);
naoMotion.stiffnessInterpolation("RArm", 0f, 0f);
}
}
catch
{ }
naoMotion = null;
}
//PostureProxy ALMotion = null;
/// <summary>
/// Connects to the motion system in the NAO robot
/// </summary>
/// <param name="ip"> ip address of the robot </param>
public void connect(string ip)
{
try
{
naoMotion = new MotionProxy(ip, 9559);
// naoMotion = new ALProxy();
// give joints stiffness
naoMotion.stiffnessInterpolation("Head", 0.6f, 0.6f);
naoMotion.stiffnessInterpolation("Body", 0.6f, 0.6f);
}
catch (Exception e)
{
MessageBox.Show("Exception occurred, error log in C:\\NAOserver\\exception.txt");
System.IO.File.WriteAllText(@"C:\\NAOserver\\exception.txt", e.ToString()); // write exepctions to text file
}
}
/// <summary>
/// Connects to the motion system in the NAO robot
/// </summary>
/// <param name="ip"> ip address of the robot </param>
public void connect(string ip)
{
try
{
naoMotion = new MotionProxy(ip, 9559);
// give joints stiffness
naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f);
}
catch (Exception e)
{
MessageBox.Show("Exception occurred, error log in C:\\NAOcam\\exception.txt");
System.IO.File.WriteAllText(@"C:\\NAOcam\\exception.txt", e.ToString()); // write exepctions to text file
}
}
/// <summary>
/// Connects to the motion system in the NAO robot
/// </summary>
/// <param name="ip"> ip address of the robot </param>
///
public void connect(string ip)
{
try
{
naoMotion = new MotionProxy(ip, 9559);
bmp = new BehaviorManagerProxy(ip, 9559);
// give joints stiffness
naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f);
}
catch (Exception e)
{
//MessageBox.Show("Exception occurred, error log in C:\\NAOcam\\exception.txt");
System.IO.File.WriteAllText(@"C:\\NAOcam\\exception.txt", e.ToString()); // write exepctions to text file
}
}
private int LookForMarks(MotionProxy motionProxy)
{
List <string> names = new List <string>();
List <float[]> times = new List <float[]>();
List <float[]> keys = new List <float[]>();
var time = 2f;
MotionHelper.MoveHead(motionProxy, 0f, 0f, true);
/*
* names.Add("HeadYaw");
* times.Add(new[] { 0.60000f, 1.12000f, 1.56000f, 2.20000f, 3.00000f, 3.76000f, 5.04000f, 7.20000f });
* keys.Add(new[] { 0.00149f, -0.27925f, -0.34826f, -0.47124f, -0.52360f, -0.28690f, -0.00940f, -0.00940f });
*/
/* // absolute
* names.Add("HeadPitch");
* times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f});
* keys.Add(new[] { 0f, 0.5f, 0f, 0f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f});
*
* names.Add("HeadYaw");
* times.Add(new[] { 2f, 4f, 6f, 9f, 15f, 18f, 21f, 22f, 28f, 29f, 35f, 38f});
* keys.Add(new[] { 0f, 0f, 0f, -1f, 1f, 0f, -1f, -1f, 1f, 1f, -1f, 0f });
*
*
* var methodID = motionProxy.post.angleInterpolation(names, keys, times, true);
*/
// relative
names.Add("HeadPitch");
times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
keys.Add(new[] { 0.5f, 0f, 0f, 0f, 0.25f, 0.25f, 0.5f, 0.5f, 0f });
names.Add("HeadYaw");
times.Add(new[] { 2f, 4f, 9f, 19f, 21f, 26f, 28f, 38f, 43f });
keys.Add(new[] { 0f, 0f, -1f, 1f, 1f, -1f, -1f, 1f, 0f });
var methodID = motionProxy.post.angleInterpolation(names, keys, times, false);
return(methodID);
}
//Control LArm with all Joints
public static void controlLArm(MotionProxy mp, float LSP, float LSR, float LER, float LEY, float LWY)
{
//Joint Controll
//Pitch=Rot(y), Roll=Rot(z), Yaw=Rot(x)
String[] names = { "LShoulderPitch", "LShoulderRoll", "LElbowRoll", "LElbowYaw", "LWristYaw" };
float[] newangles = { LSP, LSR, LER, LEY, LWY };
//Make sure floats are in range
if (ShoulderPitch.ContainsValue(LSP) && LShoulderRoll.ContainsValue(LSR) && LElbowRoll.ContainsValue(LER) && ElbowYaw.ContainsValue(LEY))
{
float fractionMaxSpeed = 0.2f;
mp.setAngles(names, newangles, fractionMaxSpeed);
}
else
{
return;
}
}
/// <summary>
/// Aligns the robot with the landmark
/// </summary>
/// <param name="motionProxy"></param>
/// <param name="markInfo"></param>
private void AlignWithMark(MotionProxy motionProxy, ArrayList markInfo)
{
// get mark shape infor
var shapeInfo = LandMarkHelper.Instance.GetShapeInfo(markInfo);
// get current yaw position
var currentYawPosition = motionProxy.getAngles("HeadYaw", true);
if (!currentYawPosition.IsNullOrEmpty() && shapeInfo[1] != null)
{
// move pitch to landmark
MotionHelper.MoveHead(motionProxy, shapeInfo[2] as float?, null, false);
// calculate robot rotation
var rotation = currentYawPosition[0] + (float)shapeInfo[1];
motionProxy.moveTo(0f, 0f, rotation);
// reset yaw
MotionHelper.MoveHead(motionProxy, null, 0f, true);
}
}
//connect to the Nao robot
private void ConnectToNao(string nao_ip_address, int nao_port)
{
bool success = true;
try
{
TextToSpeechProxy = new TextToSpeechProxy(nao_ip_address, nao_port);
BehaviorManagerProxy = new BehaviorManagerProxy(nao_ip_address, nao_port);
LedsProxy = new LedsProxy(nao_ip_address, nao_port);
VideoRecorderProxy = new VideoRecorderProxy(nao_ip_address, nao_port);
MotionProxy = new MotionProxy(nao_ip_address, nao_port);
AudioProxy = new AudioDeviceProxy(nao_ip_address, nao_port);
}
catch (Exception)
{
success = false;
}
ConnectButton.Dispatcher.BeginInvoke(DispatcherPriority.Normal,
new UpdateInterfaceAfterConnectDelegate(UpdateUserInterfaceAfterConnect), success);
}
/// <summary>
/// Connects to the motion system in the NAO robot
/// </summary>
/// <param name="ip"> ip address of the robot </param>
public void connect(string ip)
{
// Make sure the standard output directory exists
if (!Directory.Exists("C:\\NAO Motion\\"))
{
Directory.CreateDirectory("C:\\NAO Motion\\");
}
try
{
naoMotion = new MotionProxy(ip, 9559);
// give joints stiffness
naoMotion.stiffnessInterpolation("Head", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("LArm", 1.0f, 1.0f);
naoMotion.stiffnessInterpolation("RArm", 1.0f, 1.0f);;
}
catch (Exception e)
{
File.WriteAllText(@"C:\\NAO Motion\\exception.txt", e.ToString()); // Write exepctions to text file
}
}
public NaoController(string ip)
{
try
{
lastUpdate = null;
proxy = new MotionProxy(ip, 9559);
float stiffness = 1.0f;
float time = 1.0f;
/*
proxy.stiffnessInterpolation("Head", stiffness, time);
proxy.stiffnessInterpolation("LShoulderPitch", stiffness, time);
proxy.stiffnessInterpolation("RShoulderPitch", stiffness, time);
proxy.stiffnessInterpolation("LShoulderRoll", stiffness, time);
proxy.stiffnessInterpolation("RShoulderRoll", stiffness, time);
proxy.stiffnessInterpolation("LElbowYaw", stiffness, time);
proxy.stiffnessInterpolation("RElbowYaw", stiffness, time);
proxy.stiffnessInterpolation("LElbowRoll", stiffness, time);
proxy.stiffnessInterpolation("RElbowRoll", stiffness, time);
proxy.stiffnessInterpolation("RHipYawPitch", stiffness, time);
proxy.stiffnessInterpolation("LHipYawPitch", stiffness, time);
proxy.stiffnessInterpolation("LHipPitch", stiffness, time);
proxy.stiffnessInterpolation("RHipPitch", stiffness, time);
proxy.stiffnessInterpolation("LKneePitch", stiffness, time);
proxy.stiffnessInterpolation("RKneePitch", stiffness, time);
proxy.stiffnessInterpolation("LAnklePitch", stiffness, time);
proxy.stiffnessInterpolation("RAnklePitch", stiffness, time);
proxy.stiffnessInterpolation("LHipRoll", stiffness, time);
proxy.stiffnessInterpolation("RHipRoll", stiffness, time);
proxy.stiffnessInterpolation("LAnkleRoll", stiffness, time);
proxy.stiffnessInterpolation("RAnkleRoll", stiffness, time);
* */
}
catch (Exception e)
{
Console.Out.WriteLine("HeadPanning.Connect exception: " + e);
}
}
public NaoController(string ip, MainController main)
{
try
{
this.main = main;
this.ip = ip;
prevAng = PI;
proxy = new MotionProxy(ip, 9559);
if (this.ip != "127.0.0.1")
{
tts = new TextToSpeechProxy(this.ip, 9559);
}
else
{
tts = null;
}
//this.setStiffness(1.0f);
}
catch (Exception e)
{
Console.Out.WriteLine("Connect exception: " + e);
}
}
public tracker(string ip, int port)
{
this.ip = ip;
this.port = port;
motion = new MotionProxy(ip, port);
RobotPostureProxy pos = new RobotPostureProxy(ip, port);
ArrayList stiff = new ArrayList();
stiff.Add(1.0F);
stiff.Add(1.0F);
ArrayList joint = new ArrayList();
joint.Add("Head");
motion.setStiffnesses(joint, stiff);
pos.goToPosture("SitRelax", 0.6F);
track = new FaceTrackerProxy(ip, port);
track.setWholeBodyOn(false);
track.startTracker();
Thread.Sleep(30000);
track.stopTracker();
stiff.Clear();
stiff.Add(0.0F);
stiff.Add(0.0F);
motion.setStiffnesses(joint, stiff);
}
private bool LookDown(MotionProxy motionProxy)
{
// get initial head pitch position
var initialHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);
MotionHelper.MoveHead(motionProxy, this.HeadPitchStepAmount, null, false);
// get current pitch position
var currentHeadPitchPosition = motionProxy.getAngles("HeadPitch", true);
// return true if the current position changed
if (!initialHeadPitchPosition.IsNullOrEmpty() && !currentHeadPitchPosition.IsNullOrEmpty() &&
initialHeadPitchPosition[0] != currentHeadPitchPosition[0])
{
return(true);
}
else
{
// reset pitch position
MotionHelper.MoveHead(motionProxy, 0f, null, true);
return(false);
}
}
private void init()
{
try
{
rp = new RobotPostureProxy(ip, 9559);
rp.goToPosture("StandZero", 1);
}
catch (Exception)
{
MessageBox.Show("IP Adresse von Nao nicht erreichbar, prüfen Sie die Konfiguration!", "Fehler");
Application.Exit();
throw new Exception("Cannot connect to Proxy. Check IP-Configuration!");
}
try
{
List <String> joints = new List <string> {
"LArm", "RArm"
};
//mp = new MotionProxy(ip, port);
mpL = new MotionProxy(ip, port);
mpR = new MotionProxy(ip, port);
//mp.setStiffnesses(joints, 0.6f);
//List<float> angles = mp.getAngles(new String[] { "RShoulderPitch", "RShoulderRoll", "RElbowRoll", "RElbowYaw", "RWristYaw" } ,true);
}
catch (Exception)
{
throw new Exception("Cannot connect to Proxy");
}
RArm = new RArm(mpR);
LArm = new LArm(mpL);
}
/// <summary>
/// !!!Tested only with simulator!!!
///
/// Conclusion:
/// when two proxy instances in two threads operates different joints,
/// they are executed in the same time.
///
/// when two proxy instances in two threads operates the same joints at the same time,
/// they are scheduled to be executed one by one.
///
/// SOAP error will occur if two threads operates
/// the same instance of MotionProxy at the same time!
/// </summary>
public void TestMotionProxies()
{
MotionProxy mp1 = new MotionProxy(this.IP, Port);
MotionProxy mp2 = new MotionProxy(this.IP, Port);
List<string> jn1 = new List<string>(){"RShoulderPitch", "RShoulderRoll"};
List<float> jv1 = new List<float>(){0.0f, -1.0f};
List<float> jv3 = new List<float>() { -0.5f, 0.25f };
List<float> t1 = new List<float>() { 1.0f, 1.0f };
List<string> jn2 = new List<string>() { "RHipPitch", "RHipRoll" };
List<float> jv2 = new List<float>() { -0.5f, -0.5f };
List<float> t2 = new List<float>() { 1.0f, 1.0f };
BackgroundWorker bgw1 = new BackgroundWorker();
BackgroundWorker bgw2 = new BackgroundWorker();
bgw1.DoWork += delegate
{
mp1.angleInterpolation(jn1, jv1, t1, true);
};
bgw2.DoWork += delegate
{
// No problem so far; executed simultaneously
mp2.angleInterpolation(jn2, jv2, t2, true);
// No problem so far; scheduled one by one; order is not definite
//mp2.angleInterpolation(jn1, jv3, t2, true);
// SOAP error. sometimes caused at bgw1, sometimes bgw2
// some times no errors occurred, probably because
// the two threads were not operating at the same time coincidently
//mp1.angleInterpolation(jn2, jv2, t2, true);
// SOAP error. sometimes caused at bgw1, sometimes bgw2
// some times no errors occurred, probably because
// the two threads were not operating at the same time coincidently
//mp1.angleInterpolation(jn1, jv3, t2, true);
};
bgw1.RunWorkerAsync();
bgw2.RunWorkerAsync();
}
///<summary>
/// Creates proxes to be used by this class only.
/// </summary>
private void CreateMyProxies()
{
motion = Proxies.GetProxy <MotionProxy>();
battery = Proxies.GetProxy <BatteryProxy>();
memory = Proxies.GetProxy <MemoryProxy>();
}
void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
{
if (frameCount % 3 == 0) // increase the number after the mod to increase speed. in its current setup it skips 2 out of 3 frames and only processes the 3rd.
{
DepthImageFrame imageFrame = e.OpenDepthImageFrame();
if (imageFrame != null) // makes sure a frame exists.
{
short[] pixelData = new short[imageFrame.PixelDataLength]; // Creates an array the size of the image frame pixel length
imageFrame.CopyPixelDataTo(pixelData); // copies pixel data to the pixel data array.
int temp = 0; // initalizes depth.
int i = 0; // intializes the count.
for (int y = 0; y < 480; y += pointDen)// iterates through each row.
{
for (int x = 0; x < 640; x += pointDen) //iterates through each column in the row.
{
temp = ((ushort)pixelData[x + y * 640])>> 4;
((TranslateTransform3D)points[i].Transform).OffsetZ = temp; // sets the triangles z offset.
if (temp<400 && x >= 200 && x <= 400)
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Red)); // throws a read warning.
rotate = -.2f;
speed = .0f;
REDALERT = true;
}
else if (temp <500 && x >= 200 && x<= 400 && !REDALERT)
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Yellow)); // sets it to be yellow when it is in a cetain range.
objectDetected = true;
av += x;
count++;
// 3 triagles that are yellow, what x value? x value is on left side, rotate right.
}
else
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Blue)); // paints everyhting else blue.
}
i++;
}
}
if (objectDetected)
{
av = av / count;
av = av - 320;
if (av >= 0 && count > 5)
{
rotate = -.2f;
speed = .5f;
}
else if ( count > 5)
{
rotate = .2f;
speed = .5f;
}
}
if(pathChecked == true)
{
if (mp == null)
{
mp = new MotionProxy(ipBox.Text, 9559);
}
if (mp != null)
{
mp.setWalkTargetVelocity(speed, 0, rotate, .5f);
}
}
else if (pathChecked == false)
{
if (mp == null)
{
mp = new MotionProxy(ipBox.Text, 9559);
}
if (mp != null)
{
mp.stopMove();
}
}
}
}
speed = .6f;
rotate = 0;
av = 0;
count = 0;
objectDetected = false;
REDALERT = false;
pathChecked = false;
frameCount++;
}
public GrabWorker()
{
motion = Grabber.Instance.Motion;
posture = Grabber.Instance.Posture;
}
/// <summary>
/// Discards the proxies used by this class (intended to be called when they have been disposed).
/// </summary>
/// <param name="ip">The IP of the Nao disconnected from.</param>
/// <param name="port">The port number of the Nao disconnected from.</param>
private void ResetProxies(string ip, int port)
{
motion = null;
posture = null;
}
/// <summary>
/// Fetches new proxies to be used by this class.
/// </summary>
/// <param name="ip">The IP of the Nao connected to.</param>
/// <param name="port">The port number the Nao connected to.</param>
private void BuildProxies(string ip, int port)
{
motion = Proxies.GetProxy <MotionProxy>();
posture = Proxies.GetProxy <RobotPostureProxy>();
}
/// <summary>
/// Makes the robot look left and right
/// </summary>
/// <param name="motionProxy"></param>
/// <returns></returns>
private bool LookAround(MotionProxy motionProxy, LookDirection startLookDirection, ref LookDirection currentLookDirection)
{
// get initial head yaw position
var initialHeadYawPositionArray = motionProxy.getAngles("HeadYaw", true);
if (initialHeadYawPositionArray.IsNullOrEmpty())
{
// should not happen
return(false);
}
var initialHeadYawPosition = initialHeadYawPositionArray[0];
float maxHeadYaw, step;
float marginOfError = 0.01f;
var maxReached = false;
var directionChanged = currentLookDirection != startLookDirection;
if (currentLookDirection == LookDirection.Left)
{
maxHeadYaw = this.MaxHeadYaw;
step = this.HeadYawStepAmount;
// check if it's the last movement and ajust the step if it is
if (step + initialHeadYawPosition >= maxHeadYaw)
{
step = maxHeadYaw - initialHeadYawPosition;
}
// if it reached the maximum value we need to change the direction and update the step
if (step - marginOfError <= 0)
{
maxReached = true;
step = -this.HeadYawStepAmount;
}
}
else
{
// if it's the right direction we need to have negative values for max and step angles
maxHeadYaw = -this.MaxHeadYaw;
step = -this.HeadYawStepAmount;
// check if it's the last movement and ajust the step if it is
if (step + initialHeadYawPosition <= maxHeadYaw)
{
step = maxHeadYaw - initialHeadYawPosition;
}
// if it reached the maximum value we need to change the direction and update the step
if (step + marginOfError >= 0)
{
maxReached = true;
step = this.HeadYawStepAmount;
}
}
if (maxReached && directionChanged)
{
// no more movement allowed
// reset head yaw position to 0
MotionHelper.MoveHead(motionProxy, null, 0f, true);
return(false);
}
else
{
MotionHelper.MoveHead(motionProxy, null, step, maxReached);
if (maxReached)
{
currentLookDirection = currentLookDirection == LookDirection.Left ? LookDirection.Right : LookDirection.Left;
}
return(true);
}
}
///<summary>
/// Creates proxes to be used by this class only.
/// </summary>
private void CreateMyProxies()
{
motion = Proxies.GetProxy<MotionProxy>();
battery = Proxies.GetProxy<BatteryProxy>();
memory = Proxies.GetProxy<MemoryProxy>();
}
/// <summary>
/// Forces this classes' proxy properties to refresh.
/// </summary>
private void ResetProxies(string ip, int port)
{
Motion = null;
}
public Nao_Winkel(MotionProxy motion)
{
// TODO: Complete member initialization
this.motion = motion;
}
// Chaque client à son propre "main"
public void Thread()
{
//on demande au client de s'authentifier
sw.WriteLine("NICK!");
sw.Flush();
string nickString = null;
try
{
nickString = sr.ReadLine();
} catch(Exception e)
{
Fermerconnexion();
return;
}
string[] commande = null;
//on décortique la commande
commande = GetCommande(nickString);
//Le protocole dit : on doit envoyer NICK!LeNick - si ce n'est pas le cas on envoie le message d'erreur conséquent
if (!commande[0].Equals("NICK") || commande[1] == null )
{
//le client n'a pas fourni son nom
if (commande[1] == null)
{
commande[1] = "Mauvaise authentification: NICK!Authentifiant attendu";
}
//on se garde une trace sur la console serveur
Console.WriteLine(commande[1]);
//on envoie l'erreur au client
Erreur(commande[1]);
// on ferme la connexion
Fermerconnexion();
// on sort de la méthode - tuer le thread.
return;
}
if (!Regex.IsMatch(commande[1], @"^[a-zA-Z0-9_]+$"))
{
//on se garde une trace sur la console serveur
Console.WriteLine(commande[1] + " ne contient pas uniquement des caractère alphanumérique et des soulignés");
//on envoie l'erreur au client
Erreur("votre nom contient des caractère proscrit : uniquement A-Z a-z 0-9 _ sont authorisés");
// on ferme la connexion
Fermerconnexion();
// on sort de la méthode - tuer le thread.
return;
}
//Si le nom est déjà dans la liste, erreur
foreach (Client client in listeClient)
{
if (client.nick.ToUpper().Equals(commande[1].ToUpper()))
{
Erreur("Ce nom est déjà présent");
Fermerconnexion();
return;
}
}
//enregistrer le nom dans l'objet du client.
this.nick = commande[1];
//on se garde une trace
Console.WriteLine(this.nick + " vient de se connecter");
//on ajoute le client à la liste
AjouterClient(this);
//on repond au client avec HELO!.
Ecrire("HELO!" + nick);
//On doit diffuser que le client vient de se connecter
DiffuserCommande(this, "JOIN");
//tant que le client est actif (l'état va changer avec la commande QUIT!)
bool active = true;
//tant que le socket est connecté.
while (ClientConnecte() && active)
{
//bloquant - jusqu'à ce qu'une ligne arrive ou encore une déconnexion violente
string message = null;
try{
message = sr.ReadLine();
}
catch (Exception e)
{
Console.WriteLine("message null, on déconnecte ce socket : "+e.Message);
}
//lorsque le socket se fait débrancher, sr.ReadLine() va débloquer et message va être à null
if (message == null)
{
//on se garde une trace dans la console pour indiquer que le client vient de se déconnecter.
Console.WriteLine(this.nick + " vient de se déconnecter");
// on demande à etre retirer de la liste;
RetirerClient(this);
//On doit diffuser que le client vient de se déconnecter.
Diffuser(this, "QUIT", "Socket terminé");
//on ferme les tream
Fermerconnexion();
//on sors de la méthode ce qui va tuer le thread. fermeture propre.
return;
}
Console.WriteLine(" message explicite >> " + message);
//maintenant que les erreurs de deconnexions ont été géré.
//on décortique la commande recu.
commande = GetCommande(message);
switch (commande[0])
{
//message d'erreur
case "MSGE":
{
Erreur(commande[1]);
break;
}
//message à tous
case "MSGA":
{
if (commande[1] != null)
{
Diffuser(this, "MSGA", commande[1]);
}
else
{
Erreur("Le message ne peut pas être vide");
}
break;
}
//message privé
case "MSGP":
{
commande[1] = (commande[1] == null) ? "" : commande[1];
string[] destinataireMessage = AnalyseSousCommande(commande[1]);
if (destinataireMessage[0].Length == 0)
{
Erreur("Aucun destinataire spéficié");
}
else if (destinataireMessage[1].Length == 0)
{
Erreur("Aucun message spécifié");
}
else
{
Console.WriteLine("Tentative de message privé à envoyer à: " + destinataireMessage[0]);
bool envoye = false;
foreach (Client client in listeClient)
{
if (client.nick.ToUpper().Equals(destinataireMessage[0].ToUpper()))
{
Ecrire("SUCC!" + commande[0]);
client.Ecrire(commande[0] + "!" + this.nick + "!" + destinataireMessage[1]);
envoye = true;
}
}
if (!envoye)
{
this.Erreur("aucun destinataire trouvé");
}
}
break;
}
/*
* Le protocol dit que le serveur doit répondre avec
* LIST!nom1,nom2,nom3,nom4.....nomN
* vous devez implémenter cette méthode
* */
case "LIST":
{
StringBuilder sb = new StringBuilder();
sb.Append("LIST!");
bool append = false;
foreach (Client client in listeClient)
{
sb.Append(client.nick + ",");
append = true;
}
//enleve la dernière virgule;
if (append)
{
sb.Length -= 1;
}
Ecrire(sb.ToString());
break;
}
case "QUIT":
{
Ecrire("SUCC!" + commande[0]);
if (commande[1] == null)
{
Diffuser(this, commande[0], "bye bye !");
}
else
{
Diffuser(this, commande[0], commande[1]);
}
active = false;
break;
}
case "CONN":
{
if (naoTts == null || naoMotion == null || naoPosture == null)
{
Console.WriteLine("Connection à NAO");
//naoTts = new TextToSpeechProxy("192.168.2.134", 9559);
naoTts = new TextToSpeechProxy("127.0.0.1", 9559);
Console.WriteLine("TextToSpeechProxy connecté");
naoTts.setLanguage("FRENCH");
naoTts.setVolume(0.3f);
//naoMotion = new MotionProxy("192.168.2.134", 9559);
naoMotion = new MotionProxy("127.0.0.1", 9559);
Console.WriteLine("MotionProxy connecté");
//naoPosture = new RobotPostureProxy("192.168.2.134", 9559);
naoPosture = new RobotPostureProxy("127.0.0.1", 9559);
Console.WriteLine("RobotPostureProxy connecté");
Console.WriteLine("CONN terminé");
}
else Console.WriteLine("Déjà connecté");
break;
}
case "INIT":
{
if (naoTts != null && naoMotion != null && naoPosture != null)
{
naoMotion.setStiffnesses("Body", 1f);
if (naoPosture.goToPosture("StandZero", 1f))
naoTts.say("Initialisation terminée");
Console.WriteLine("Initialisation terminée");
}
else Console.WriteLine("CONN nécessaire avant INIT");
break;
}
case "TALK":
{
if (commande.Length > 1)
{
naoTts.say(commande[1]);
Console.WriteLine("NAO TextToSpeechProxy : {0}", commande[1]);
}
break;
}
case "STIF":
{
if(commande.Length > 1)
{
try
{
naoMotion.setStiffnesses("Body", float.Parse(commande[1]));
}
catch(Exception e)
{
Diffuser(this, "MSGA", "La commande STIF requiert un parametre valide");
}
}
else
{
Diffuser(this, "MSGA", "La commande STIF requiert un parametre");
}
break;
}
case "POST":
{
if(commande.Length == 2)
{
try
{
naoPosture.goToPosture(commande[1], 1f);
}
catch(Exception)
{
Diffuser(this, "MSGA", "Argument du POST invalide");
}
}
else
{
Diffuser(this, "MSGA", "POST requiert un argument");
}
break;
}
case "MOVE":
{
if(commande[1] != null)
{
string[] args = commande[1].Split('!');
args = args.Select(x => x.Replace(".", ",")).ToArray();
if(args.Length > 1)
{
string joint = args[0];
float angle = 0;
float.TryParse(args[1], out angle);
float vitesse = 0.1f;
try
{
naoMotion.setAngles(joint, angle, vitesse);
}
catch(Exception e)
{
Console.WriteLine(e);
}
}
}
break;
}
case "TEST":
{
if (commande.Length > 1)
{
string[] args = commande[1].Split('!');
args = args.Select(x => x.Replace(".", ",")).ToArray();
if (args.Length > 6)
{
string chainName = args[0];
int space = 0;
float[] position = {
float.Parse(args[1]),
float.Parse(args[2]),
float.Parse(args[3]),
float.Parse(args[4]),
float.Parse(args[5]),
float.Parse(args[6]),
};
int axisMask = 63;
naoMotion.setPosition(chainName, space, position.ToList<float>(), 0.4f, axisMask);
}
}
break;
}
case "RGHT":
{
if (commande.Length > 1)
{
string[] args = commande[1].Split('!');
args = args.Select(x => x.Replace(".", ",")).ToArray();
if (args.Length > 2)
{
float[] position = {
float.Parse(args[0]),
float.Parse(args[1]),
float.Parse(args[2]),
0f,
0f,
0f
};
float[] currPosition = naoMotion.getPosition("RArm",0,true).ToArray();
float[] deltaPosition = new float[6];
for(int i = 0; i < 6; i++)
{
deltaPosition[i] = (position[i] - currPosition[i]) / 10;
}
Diffuser(this, "MSGA", message);
naoMotion.changePosition("RArm", 0, position.ToList<float>(), 0.8f, 63);
}
}
break;
}
default:
{
Erreur("Commande non implémenté sur le serveur");
break;
}
}
}
Console.WriteLine(this.nick + " vient de se deconnecter");
// on se retire soi-même de la liste;
RetirerClient(this);
//on ferme les socket et les streams
Fermerconnexion();
//le thread va mourrir... sniff sniff
}
public Angles(MotionProxy mp, String[] joints)
{
currAngles = mp.getAngles(joints, true);
}
public GrabWorker()
{
motion = Grabber.Instance.Motion;
posture = Grabber.Instance.Posture;
}
public SitDownExecuter(MotionProxy motionProxy, RobotPoseProxy robotPoseProxy)
{
this.motionProxy = motionProxy;
this.robotPoseProxy = robotPoseProxy;
}
/// <summary>
/// Forces this classes' proxy properties to refresh.
/// </summary>
private void ResetProxies(string ip, int port)
{
Motion = null;
}
private void Button_Click_1(object sender, RoutedEventArgs e)
{
//Initialize the motion proxy using the text in the ipBox
MotionProxy mp = new MotionProxy(ipBox.Text, 9559);
//Initialize the text to speech proxy using the text in the ipBox
ttsp = new TextToSpeechProxy(ipBox.Text, 9559);
//Initialize the robots posture proxy using the text in the ipBox
RobotPostureProxy rpp = new RobotPostureProxy(ipBox.Text, 9559);
//Wakes the robot up, tells the robot to stand and say hello
mp.wakeUp();
rpp.goToPosture("StandInit", .5f);
ttsp.say("Hola");
//Sets up lighting of the camera with a white color and directs it in the appropriate direction
DirectionalLight DirLight1 = new DirectionalLight();
DirLight1.Color = Colors.White;
DirLight1.Direction = new Vector3D(1, 1, 1);
//Setting up the camera so that the picture we see is in a good position
PerspectiveCamera Camera1 = new PerspectiveCamera();
Camera1.FarPlaneDistance = 16000;
Camera1.NearPlaneDistance = 10;
//Zoomed in with a 45 degree field of view
Camera1.FieldOfView = 45;
//50 mm away from the point cloud and centered in the middle
Camera1.Position = new Point3D(320, 240, -50);
//Flips the camera so that we see everything right side up
Camera1.LookDirection = new Vector3D(0, 0, 1);
Camera1.UpDirection = new Vector3D(0, -1, 0);
//Creates a new model 3D group to hold the sets of 3D models
Model3DGroup modelGroup = new Model3DGroup();
//builds the 3D model
int i = 0;
for (int y = 0; y < 480; y += pointDen)
{
for (int x = 0; x < 640; x += pointDen)
{
points[i] = Triangle(x, y, pointDen);
points[i].Transform = new TranslateTransform3D(0, 0, 0);
modelGroup.Children.Add(points[i]);
i++;
}
}
//Adds the directional light to the model group
modelGroup.Children.Add(DirLight1);
ModelVisual3D modelsVisual = new ModelVisual3D();
modelsVisual.Content = modelGroup;
Viewport3D myViewport = new Viewport3D();
myViewport.IsHitTestVisible = false;
myViewport.Camera = Camera1;
myViewport.Children.Add(modelsVisual);
canvas1.Children.Add(myViewport);
myViewport.Height = canvas1.Height;
myViewport.Width = canvas1.Width;
Canvas.SetTop(myViewport, 0);
Canvas.SetLeft(myViewport, 0);
sensor = KinectSensor.KinectSensors[0];
sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
sensor.DepthFrameReady += DepthFrameReady;
sensor.Start();
}
private void Button_Click_1(object sender, RoutedEventArgs e)
{
//Initialize the motion proxy using the text in the ipBox
MotionProxy mp = new MotionProxy(ipBox.Text, 9559);
//Initialize the text to speech proxy using the text in the ipBox
ttsp = new TextToSpeechProxy(ipBox.Text, 9559);
//Initialize the robots posture proxy using the text in the ipBox
RobotPostureProxy rpp = new RobotPostureProxy(ipBox.Text, 9559);
//Wakes the robot up, tells the robot to stand and say hello
mp.wakeUp();
rpp.goToPosture("StandInit", .5f);
ttsp.say("Hola");
//Sets up lighting of the camera with a white color and directs it in the appropriate direction
DirectionalLight DirLight1 = new DirectionalLight();
DirLight1.Color = Colors.White;
DirLight1.Direction = new Vector3D(1, 1, 1);
//Setting up the camera so that the picture we see is in a good position
PerspectiveCamera Camera1 = new PerspectiveCamera();
Camera1.FarPlaneDistance = 16000;
Camera1.NearPlaneDistance = 10;
//Zoomed in with a 45 degree field of view
Camera1.FieldOfView = 45;
//50 mm away from the point cloud and centered in the middle
Camera1.Position = new Point3D(320, 240, -50);
//Flips the camera so that we see everything right side up
Camera1.LookDirection = new Vector3D(0, 0, 1);
Camera1.UpDirection = new Vector3D(0, -1, 0);
//Creates a new model 3D group to hold the sets of 3D models
Model3DGroup modelGroup = new Model3DGroup();
//builds the 3D model
int i = 0;
for (int y = 0; y < 480; y += pointDen)
{
for (int x = 0; x < 640; x += pointDen)
{
points[i] = Triangle(x, y, pointDen);
points[i].Transform = new TranslateTransform3D(0, 0, 0);
modelGroup.Children.Add(points[i]);
i++;
}
}
//Adds the directional light to the model group
modelGroup.Children.Add(DirLight1);
ModelVisual3D modelsVisual = new ModelVisual3D();
modelsVisual.Content = modelGroup;
Viewport3D myViewport = new Viewport3D();
myViewport.IsHitTestVisible = false;
myViewport.Camera = Camera1;
myViewport.Children.Add(modelsVisual);
canvas1.Children.Add(myViewport);
myViewport.Height = canvas1.Height;
myViewport.Width = canvas1.Width;
Canvas.SetTop(myViewport, 0);
Canvas.SetLeft(myViewport, 0);
sensor = KinectSensor.KinectSensors[0];
sensor.DepthStream.Enable(DepthImageFormat.Resolution640x480Fps30);
sensor.DepthFrameReady += DepthFrameReady;
sensor.Start();
}
void DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
{
if (frameCount % 3 == 0) // increase the number after the mod to increase speed. in its current setup it skips 2 out of 3 frames and only processes the 3rd.
{
DepthImageFrame imageFrame = e.OpenDepthImageFrame();
if (imageFrame != null) // makes sure a frame exists.
{
short[] pixelData = new short[imageFrame.PixelDataLength]; // Creates an array the size of the image frame pixel length
imageFrame.CopyPixelDataTo(pixelData); // copies pixel data to the pixel data array.
int temp = 0; // initalizes depth.
int i = 0; // intializes the count.
for (int y = 0; y < 480; y += pointDen) // iterates through each row.
{
for (int x = 0; x < 640; x += pointDen) //iterates through each column in the row.
{
temp = ((ushort)pixelData[x + y * 640]) >> 4;
((TranslateTransform3D)points[i].Transform).OffsetZ = temp; // sets the triangles z offset.
if (temp < 400 && x >= 200 && x <= 400)
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Red)); // throws a read warning.
rotate = -.2f;
speed = .0f;
REDALERT = true;
}
else if (temp < 500 && x >= 200 && x <= 400 && !REDALERT)
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Yellow)); // sets it to be yellow when it is in a cetain range.
objectDetected = true;
av += x;
count++;
// 3 triagles that are yellow, what x value? x value is on left side, rotate right.
}
else
{
points[i].Material = new DiffuseMaterial(new SolidColorBrush(Colors.Blue)); // paints everyhting else blue.
}
i++;
}
}
if (objectDetected)
{
av = av / count;
av = av - 320;
if (av >= 0 && count > 5)
{
rotate = -.2f;
speed = .5f;
}
else if (count > 5)
{
rotate = .2f;
speed = .5f;
}
}
if (pathChecked == true)
{
if (mp == null)
{
mp = new MotionProxy(ipBox.Text, 9559);
}
if (mp != null)
{
mp.setWalkTargetVelocity(speed, 0, rotate, .5f);
}
}
else if (pathChecked == false)
{
if (mp == null)
{
mp = new MotionProxy(ipBox.Text, 9559);
}
if (mp != null)
{
mp.stopMove();
}
}
}
}
speed = .6f;
rotate = 0;
av = 0;
count = 0;
objectDetected = false;
REDALERT = false;
pathChecked = false;
frameCount++;
}
