// ================================================================================================
/// <summary>
/// SubScriber CallBack
/// </summary>
/// <param name="dt"></param>
//private void cbSubScriber_VSlam(RosSharp.visualization_msgs.Marker dt)
private void cbSubScriber_VSlam(RosSharp.geometry_msgs.Twist dt)
{
#if false
if (dt.color.r == 0.0f && dt.color.g == 0.0f && dt.color.b == 0.5f && dt.color.a == 1.0f)
{
Console.WriteLine("seqid:" + dt.header.seq.ToString());
Console.WriteLine("time:" + dt.header.stamp.Ticks.ToString() + " ,MS " + dt.header.stamp.Millisecond.ToString());
Console.WriteLine("pose: ");// + dt.pose.position.x.ToString() + "," + dt.pose.position.y.ToString() + "," + dt.pose.position.z.ToString());
Console.WriteLine(" position: ");// + dt.pose.position.x.ToString() + "," + dt.pose.position.y.ToString() + "," + dt.pose.position.z.ToString());
Console.WriteLine(" x:" + dt.pose.position.x.ToString());
Console.WriteLine(" y:" + dt.pose.position.y.ToString());
Console.WriteLine(" z:" + dt.pose.position.z.ToString());
//Console.WriteLine("color:" + dt.color.r.ToString() + "," + dt.color.g.ToString() + "," + dt.color.b.ToString());
}
#endif
// カメラに対して
// 左右 X: 左 -1.0 <----> +1.0 右
// 上下 Y: 上 +1.0 <----> -1.0 下
// 奥・手前 Z: 手前 +1.0 <----> -1.0 奥
// 特定の色のマーカー情報を読む
//if ( dt.color.r == 0.0f && dt.color.g == 0.0f && dt.color.b == 0.5f && dt.color.a == 1.0f )
{
// 単位変換 m -> mm
#if true
vslamPlotX = dt.linear.x * 1000.0;
vslamPlotY = dt.linear.y * 1000.0;
vslamPlotZ = dt.linear.z * 1000.0; // とりあえず Z軸も保持
#else
vslamPlotX = dt.pose.position.x * 1000.0;
vslamPlotY = dt.pose.position.y * 1000.0;
vslamPlotZ = dt.pose.position.z * 1000.0; // とりあえず Z軸も保持
#endif
#if false
// Quatanionから角度(向き)へ変換
vslamAng = MatrixMath.QuaternionToAngle((float)dt.pose.orientation.x,
(float)dt.pose.orientation.y,
(float)dt.pose.orientation.z,
(float)dt.pose.orientation.w);
#endif
#if true
// キューに入っているベクトルを足して方向に変換
{
vSlamMoveVecQue.Enqueue(dt.linear);
Vector3 dirVec = new Vector3();
Vector3 prevVec = vSlamMoveVecQue.First();
foreach (var vec in vSlamMoveVecQue)
{
dirVec.x += (vec.x - prevVec.x);
dirVec.y += (vec.y - prevVec.y);
dirVec.z += (vec.z - prevVec.z);
prevVec = vec;
}
double vecLeng = Math.Sqrt(dirVec.x * dirVec.x) + Math.Sqrt(dirVec.y * dirVec.y);
if (vecLeng*1000.0 > 1.0)
{
// 正規化
dirVec.x /= vecLeng;
dirVec.y /= vecLeng;
// 方向へ変換
vslamAng = Math.Atan2(dirVec.y, dirVec.x);
}
if (vSlamMoveVecQue.Count > 10)
{
vSlamMoveVecQue.Dequeue();
}
}
#endif
}
}
/// <summary>
/// hokuyo-node /scan
/// </summary>
/// <param name="dt"></param>
private void cbSubScriber_URG(RosSharp.sensor_msgs.LaserScan dt)
{
if (dt.ranges.Count-1 <= urg_scan.Length)
{
for (int i = 0; i < dt.ranges.Count-1; i++)
{
urg_scan[i] = dt.ranges[i];
}
}
else
{
// データ数が異なる場合
Console.WriteLine("urg data OverFlow: dt.ranges.Count " + dt.ranges.Count.ToString() + "/ urg_scan.Length " + urg_scan.Length.ToString());
}
}
/// <summary>
/// [debug] clock
/// </summary>
/// <param name="dt"></param>
private void cbSubScriber_Clock(RosSharp.rosgraph_msgs.Clock dt)
{
//Console.WriteLine("time:" + dt.clock.ToString() );
rosClock = dt.clock;
}
/// <summary>
/// SubScriber CallBack
/// </summary>
/// <param name="dt"></param>
private void cbSubScriber_rofif_pub(RosSharp.geometry_msgs.Twist dt)
{
amclPlotX = dt.linear.x;
amclPlotY = dt.linear.y;
amclAng = dt.angular.z;
}
/// <summary>
/// test
/// </summary>
/// <param name="dt"></param>
private void callBackTurtle(RosSharp.geometry_msgs.Twist dt)
{
Console.WriteLine("vec:" + dt.linear.x.ToString() + "," + dt.linear.y.ToString() + "," + dt.linear.z.ToString());
}
