C++ OccupancyGrid::GetCentroid方法代码示例

int main(int argc, char** argv){
    double cruising_speed = 0.1;
    char* baseframe;
    char* childframe;
    if (argc < 4) {
        ROS_ERROR("(%i) usage: hallwaydrive <speed> <base frame> <child frame>\n", argc);
        return -1;    
    }
    else {    
        cruising_speed = atof(argv[1]);
        baseframe = argv[2];
        childframe = argv[3];
    }
    ros::init(argc, argv, "odometry_publisher");
    ros::NodeHandle n;
    ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
    tf::TransformBroadcaster odom_broadcaster;
    ros::Rate loop_rate(5);
    ros::Time current_time;
    
    ros::Publisher speedController = n.advertise<Speeds>("speeds_bus", 50);
    
    /*message_filters::Subscriber<Position2D> pos2DSub(n, "/pos2d_bus", 1);
    pos2DSub.registerCallback(roombaOdomCallback);
    
    message_filters::Subscriber<LaserScan> laserSub(n, "/scan", 1);
    laserSub.registerCallback(laserCallback);*/
    
    ros::Subscriber pos2DSub = n.subscribe("pos2d_bus", 100, roombaOdomCallback);
    ros::Subscriber laserSub = n.subscribe("scan", 100, laserCallback);

    while(n.ok()){
        current_time = ros::Time::now();

        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(pose.angle);

        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = baseframe;
        odom_trans.child_frame_id = childframe;

        odom_trans.transform.translation.x = pose.x;
        odom_trans.transform.translation.y = pose.y;
        odom_trans.transform.translation.z = 0.0;
        odom_trans.transform.rotation = odom_quat;

        //send the transform
        odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry message over ROS
        nav_msgs::Odometry odom;
        odom.header.stamp = current_time;
        odom.header.frame_id = baseframe;

        //set the position
        odom.pose.pose.position.x = pose.x;
        odom.pose.pose.position.y = pose.y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;


        //ROS_INFO("Odometry: (%.3lf, %.3lf) at %.3lf radians\n", pose.x, pose.y, pose.angle);

        double speed = 0.0, angle = 0.0;
        /*if (grid.closeObstacle) {
            double closestAngle = grid.getObstacleLaserAngle();
            ROS_INFO("Close Obstacle at angle %.3lf at dist %lf", closestAngle, grid.minDist);
            //If there's an obstacle really close, turn away from that obstacle
            if (closestAngle < 0)
                angle = closestAngle + 3.141/2.0;
            else
                angle = closestAngle - 3.141/2.0;
            angle /= (1.0 + grid.minDist*4.0);
        }
        else {*/
            Point centroid = grid.GetCentroid();
            angle = centroid.getAngle();
            angle = angle / 3.141;
            angle = angle*angle*angle;
            angle *= 3.141;
        //}
        speed = cruising_speed / (1.0 + 5.0 * fabs(angle));

        //Publish the speed message
        Speeds speedMsg;
        speedMsg.forward = speed;
        speedMsg.rotate = angle;
        
        if(!n.hasParam("/hallwaydrive/automatic")) {
            ROS_ERROR("Unable to find parameter automatic");
            return -1;
        }
        
        int automatic;
        n.param("/hallwaydrive/automatic", automatic, 0);
        //ROS_INFO("automatic = (%i)", automatic);
        if (automatic == 1) { //Only send commands if the user wants
        //automatic control; otherwise they will interfere with the controller GUI
//.........这里部分代码省略.........