File content as of revision 3:a45557a0dcb8:

#include <mbed.h>
#include <vector>  

#include "myRos.h"
#include "myCan.h"
#include <ros.h>
#include <ros/time.h>
#include <std_msgs/Empty.h>
#include "odom.h"

//prototype
void odom_update_callback();

//Create subscliber
ros::Subscriber<std_msgs::Empty> ack_sub("nucleo/ack_from_pc", &ack_from_pc_cb);

//Initialize GPIO
DigitalOut myled(LED1);
DigitalOut  led0(PC_0);
DigitalOut  led1(PC_1);
DigitalOut  led2(PB_0);
DigitalIn   mysw(PC_13);

void led0_toggle(void) { led0 = !led0; }
void led1_toggle(void) { led1 = !led1; }
void led2_toggle(void) { led2 = !led2; }

//Create Odom instance
Odom odom;
    
int main() {
    //Initialize TIM
    Timer GetTick;
    GetTick.start();
    uint32_t debug_ts = 0;
    uint32_t ros_loop_ts = 0;
    
    //Initialize ROS
    My_Ros my_ros(&odom);
    my_ros.set_subscliber(ack_sub);
    
    My_Can my_can(&odom);
    my_can.initialize_amcl_attach(&my_ros, &My_Ros::enable_initialize_amcl);
    my_can.led_toggle_attach(&led1_toggle);
    
    std::vector<Odom_Abstract*> odom_container;
    odom_container.push_back(&my_ros);
    odom_container.push_back(&my_can);

    while(1) {
        //For debug
        if(debug_ts <= GetTick.read_ms()){
            debug_ts = GetTick.read_ms() + 100;
            led0_toggle();
        }

        //For odom loop
        if(ros_loop_ts <= GetTick.read_ms()){        
            ros_loop_ts = GetTick.read_ms() + 10;
            for(std::vector<Odom_Abstract*>::iterator it = odom_container.begin(); it != odom_container.end(); ++it){
                (*it)->loop();
            }
        }
        
        if(mysw == 0){
            while(mysw == 0);
            my_can.test();
        }
        
        //角度計算用:ポーリングに置いとく
        odom.update_angle();
    }
}