// 3Dマップから得たwallsデータ
void wallsCallback(const wall_follower::walls::ConstPtr& msg)
{
dist2.Clear();
dist2.front = msg->front;
dist2.left = msg->left;
dist2.left2 = msg->left2;
dist2.right = msg->right;
dist2.right2 = msg->right2;
}
//-----------------------------------------------------
// URGデータのコールバック関数
// 計測距離データをロボット座標での3次元点データに変換したものから
// ロボット周囲の障害物までの距離を計算する
//-----------------------------------------------------
void wallsCallback(const wall_follower::walls::ConstPtr& msg)
{
dist.Clear();
dist.front = msg->front;
dist.left = msg->left;
dist.left2 = msg->left2;
dist.right = msg->right;
dist.right2 = msg->right2;
//ROS_INFO("%f %f %f %f %f", dist.front, dist.right, dist.left, dist.right2, dist.left2);
}
void wallsCallback(const wall_follower::walls::ConstPtr& msg)
{
dist.Clear();
dist.front = msg->front;
dist.left = msg->left;
dist.left2 = msg->left2;
dist.right = msg->right;
dist.right2 = msg->right2;
distL = (dist.left + dist.left2)/2;
distR = (dist.right + dist.right2)/2;
//ROS_INFO("%f %f %f %f %f", dist.front, dist.right, dist.left, dist.right2, dist.left2);//debug
//ROS_INFO("%f %f", distL, distR);//debug
if(!flag_init2)
{
flag_init2 = 1;
ROS_INFO("wallsCallback");
}
}
//-----------------------------------------------------
// URGデータのコールバック関数
// 計測距離データをロボット座標での3次元点データに変換したものから
// ロボット周囲の障害物までの距離を計算する
//-----------------------------------------------------
void cloudCallback(const sensor_msgs::PointCloud::ConstPtr& msg)
{
double urg_x, urg_y;
wall_follower::walls walls;
dist.Clear();
for(int i = 0; i < (int)msg->points.size(); i++){
urg_x = msg->points[i].x * 1000; // [m] から [mm] に変換
urg_y = msg->points[i].y * 1000; // [m] から [mm] に変換
if(urg_x >= 100){
//前方
if( (urg_y > -(ROBOT_WIDTH/2+10)) && (urg_y < +(ROBOT_WIDTH/2+10)) )
{
if(urg_x < dist.front)
{
dist.front = (int)urg_x;
}
}
if(urg_x <= ROAD_WIDTH/2){
//右前の方向
if ((-urg_y > 21) && (-urg_y < dist.right) )
{
dist.right = -(int)urg_y;
}
//左前の方向
if( (urg_y > 21) && (urg_y < dist.left))
{
dist.left = (int)urg_y;
}
}
}
else if(urg_x < 100 && urg_x >= -ROBOT_WIDTH/2+100)//後方のデータ
{
//右後ろの方向
if ( (-urg_y > 21) && (-urg_y < dist.right2))
{
dist.right2 = -(int)urg_y;
}
//左後ろの方向
if ( (urg_y > 21) && (urg_y < dist.left2))
{
dist.left2 = (int)urg_y;
}
}
}
//ROS_INFO("F=%4.0f L=%4.0f L2=%4.0f R=%4.0f R2=%4.0f", dist.front, dist.left, dist.left2, dist.right, dist.right2);
// 水平LRFとジンバルLRFのうち小さい方を採用
walls.front = (dist.front < dist2.front) ? dist.front : dist2.front;
walls.left = (dist.left < dist2.left) ? dist.left : dist2.left;
walls.left2 = (dist.left2 < dist2.left2) ? dist.left2 : dist2.left2;
walls.right = (dist.right < dist2.right) ? dist.right : dist2.right;
walls.right2 = (dist.right2 < dist2.right2) ? dist.right2 : dist2.right2;
ROS_INFO("F=%4.0f L=%4.0f L2=%4.0f R=%4.0f R2=%4.0f", walls.front, walls.left, walls.left2, walls.right, walls.right2);
walls_pub.publish(walls);
}
