/// <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); }