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