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