在MSRDS2008的机器人仿真中,有时候希望能够记录仿真机器人的运动轨迹,这就要求能够随时获得仿真机器人的位置。

方法一、创建Pioneer 3DX实体的时候,不要在仿真服务类的函数中定义Pioneer 3DX实体局部变量,然后用return的方法返回,譬如在AddPionner3DX、CreateMotorBase函数中定义robotEntity。这样的话,直接用RobotEntity.State.Pose.Position返回的位置信息全部为零。需要在仿真服务类中直接定义一个成员变量,如:Pioneer3DX robotEntity,然后可以在AddPionner3DX和CreateMotorBase函数中直接使用robotEntity这个成员变量创建Pioneer 3DX实体。这样的话,用robotEntity.State.Pose.Position可以获得机器人的位置了。

方法二:这个方法可以从MSRDS的仿真例子5“Environment with Terrain”中看到,也就是直接query仿真引擎,要求返回某个仿真实体的位置信息。
                VisualEntity tmp = new VisualEntity();
                tmp.State.Name = “MotorBase”;

                yield return Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(tmp),
                    delegate(QuerySimulationEntityResponseType response)
                    {
                        Console.WriteLine(string.Format(”Position: {0} {1} {2}”,
                            response.Entity.State.Pose.Position.X,
                            response.Entity.State.Pose.Position.Y,
                            response.Entity.State.Pose.Position.Z));
                    },
                    delegate(Fault f) { Console.WriteLine(”Query failed.”); }
                );

                yield return Arbiter.Receive(false, TimeoutPort(1000), delegate { });

Advertisements