File content as of revision 4:29bf39bd97a7:

#include <mbed.h>
#include <ros.h>
#include <std_msgs/String.h>

DigitalOut grab(p11); //0:閉じる 1:開く
DigitalOut snap(p12); //0:縮む 1:伸びる
std_msgs::String action;
int k_action=0;
int o_action=0;
int m_action=0;
void messageCallback(const std_msgs::String &msg)
{
    action.data=msg.data;
}

ros::NodeHandle nh;
ros::Subscriber<std_msgs::String> sub("shoot_action",&messageCallback);
DigitalOut led1(LED1);
DigitalOut led2(LED2);
DigitalOut led3(LED3);
DigitalOut led4(LED4);

void led_on(int l1,int l2,int l3,int l4)
{
    led1=l1;
    led2=l2;
    led3=l3;
    led4=l4;
}

int main(int argc, char **argv)
{
    nh.initNode();
    nh.subscribe(sub);
    while(1) {
        if((*(action.data)== 'k')&&(k_action==0)) {
            snap = 1;
            grab = 0;
            k_action++;
            led_on(1,0,0,0);
            wait(5);       

            grab = 1;
            o_action++;
            led_on(0,1,0,0);
        }
        

        else if((*(action.data)=='m')&&(m_action==0)) {
            led_on(1,1,0,0);
            snap = 0;
            m_action++;
            //pc.printf("a\n");
            wait(0.195);             //TZ1:0.18 TZ2:0.18 TZ3:0.167,0.168 椅子(高度45[cm]):0.195
            grab = 1;
            wait(3);
            snap=1;

        }
        //一連の動作が終了したかを判断する。
        if((k_action==1)&&(o_action==1)&&(m_action==1)) {
            k_action=0;
            o_action=0;
            m_action=0;
        }
        nh.spinOnce();
    }

}