Esempio n. 1
0
        /// <summary>
        /// Send velocities to service
        /// </summary>
        /// <param name="velocities">Velocities to sent [m/s]</param>
        public void SendVelocities(JointValues velocities)
        {
            var point = new trajectory_msgs.JointTrajectoryPoint
            {
                time_from_start = TimeSpan.FromSeconds(0.008).ToDurationMessage(),
                velocities      = velocities.ToArray(),
            };

            var trajectory = new trajectory_msgs.JointTrajectory()
            {
                joint_names = velocities.JointSet.ToArray(),
                points      = new [] { point }
            };

            publisherJoggingCommand.Publish(trajectory);
        }
        /// <summary>
        /// write the jp_get
        /// </summary>
        void writelisten(jp recMsg)
        {
            lock (recMsg)
            {
                try
                {
                    //position:Xx,Xy,theta
                    //velocities:Vx,Vy,W
                    //accelerations:Ax,Ay
                    //efforts:ID

                    v1.ID = Convert.ToInt32(recMsg.effort[0].ToString());
                    v1.status1.position.lati  = Convert.ToDouble(recMsg.positions[0].ToString());                                                                                                      //传入数据用于匹配和计算距离
                    v1.status1.position.lonti = Convert.ToDouble(recMsg.positions[1]);
                    v1.status1.speed          = Math.Sqrt(Math.Pow(Convert.ToDouble(recMsg.velocities[0].ToString()), 2) + Math.Pow(Convert.ToDouble(recMsg.velocities[1].ToString()), 2)) * 10;       //放大十倍
                    v1.status1.angle          = Convert.ToDouble(recMsg.positions[2].ToString()) * 180 / Math.PI + 180;                                                                                //转换 [-180,+180] [0,360]
                    v1.status1.acceleration   = Math.Sqrt(Math.Pow(Convert.ToDouble(recMsg.accelerations[0].ToString()), 2) + Math.Pow(Convert.ToDouble(recMsg.accelerations[1].ToString()), 2)) * 10; //放大10倍
                    r1.getMap();
                    v1.mapmatch();
                    v1.caly();//计算位置

                    #region 寻找目的地及connector
                    if (v1.path1.lanestart.ID == 1)
                    {
                        if (fx == 1)//左转
                        {
                            v1.path1.laneend.ID = 8;
                            v1.path1.findend();
                        }
                        if (fx == 2)//直行
                        {
                            v1.path1.laneend.ID = 7;
                            v1.path1.findend();
                        }
                        if (fx == 3)//右转
                        {
                            v1.path1.laneend.ID = 6;
                            v1.path1.findend();
                        }
                    }
                    if (v1.path1.lanestart.ID == 2)
                    {
                        if (fx == 1)//左转
                        {
                            v1.path1.laneend.ID = 5;
                            v1.path1.findend();
                        }
                        if (fx == 2)//直行
                        {
                            v1.path1.laneend.ID = 8;
                            v1.path1.findend();
                        }
                        if (fx == 3)//右转
                        {
                            v1.path1.laneend.ID = 7;
                            v1.path1.findend();
                        }
                    }
                    if (v1.path1.lanestart.ID == 3)
                    {
                        if (fx == 1)//左转
                        {
                            v1.path1.laneend.ID = 6;
                            v1.path1.findend();
                        }
                        if (fx == 2)//直行
                        {
                            v1.path1.laneend.ID = 5;
                            v1.path1.findend();
                        }
                        if (fx == 3)//右转
                        {
                            v1.path1.laneend.ID = 8;
                            v1.path1.findend();
                        }
                    }
                    if (v1.path1.lanestart.ID == 4)
                    {
                        if (fx == 1)//左转
                        {
                            v1.path1.laneend.ID = 7;
                            v1.path1.findend();
                        }
                        if (fx == 2)//直行
                        {
                            v1.path1.laneend.ID = 6;
                            v1.path1.findend();
                        }
                        if (fx == 3)//右转
                        {
                            v1.path1.laneend.ID = 5;
                            v1.path1.findend();
                        }
                    }

                    #endregion //描述拓扑关系

                    v1.path1.getcon();

                    #region 读出数据
                    if (v1.shibie == 0)
                    {
                        textBox3.Text     = Convert.ToString(v1.status1.conid);
                        vehicle1.linkid11 = v1.status1.conid;
                    }
                    else
                    {
                        textBox3.Text     = Convert.ToString(v1.status1.linkid);
                        vehicle1.linkid11 = v1.status1.linkid;
                    }
                    textBox1.Text         = Convert.ToString(v1.status1.position.lati);
                    textBox2.Text         = Convert.ToString(v1.status1.position.lonti);
                    textBox4.Text         = Convert.ToString(v1.status1.angle);
                    textBox5.Text         = Convert.ToString(v1.status1.speed);
                    textBox6.Text         = Convert.ToString(v1.status1.y);
                    textBox7.Text         = Convert.ToString(v1.shibie);
                    textBox8.Text         = Convert.ToString(v1.status1.acceleration);
                    vehicle1.coord11      = v1.status1.y;
                    vehicle1.speed11      = v1.status1.speed;
                    vehicle1.acceleration = v1.status1.acceleration;
                    vehicle1.angle11      = v1.status1.angle;

                    #endregion
                }
                catch (Exception ex) { MessageBox.Show(ex.ToString()); }
            }
        }
 /// <summary>
 /// subscriber callback
 /// </summary>
 /// <param name="msg"></param>
 public void subCallback(jp msg)
 {
     writelisten(msg);
 }