File content as of revision 5:17ede03f1fa5:

 #include "mbed.h"
#include <string> 
#include "JY901.h"
#include "PID.h"
#include "define.h"

#include "Quaternion.h"

#include <ros.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_broadcaster.h>
#include <tf/tf.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/PoseStamped.h>
#include <std_msgs/Empty.h>
#include <std_srvs/Trigger.h>
#include <std_srvs/Empty.h>

#include "robot_operator.h"
#include "device.h"

// To do
// まれにmbed実行エラーが発生(LED1点滅)
// (my_odomで返すvel_xは差分を取るより周波数から計算した方がいいか。多分良い)
//

ros::NodeHandle mecanum_node;

void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){
    moveStop();
    robot_vel_controll(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z);
}

bool is_stop = false;

void cmd_pose2d_callback(const geometry_msgs::Pose2D& cmd_pose2d){
    //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
    moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta, &is_stop);
}

void goal_callback(const geometry_msgs::PoseStamped& goal){
    //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta);
    Quaternion tmp_q(goal.pose.orientation.w, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z);
    moveStart(goal.pose.position.x, goal.pose.position.y, tmp_q.getEulerAngles().x, &is_stop);
}

void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) {
  res.success = true;
  res.message = "Stop";
  moveStop();
}

void odom_update()
{
    my_odometry.update();
}

//ros::ServiceServer<geometry_msgs::Pose2D,std_msgs::Empty> start_service("wheel/move_start", &move_start_service);
ros::ServiceServer<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_service("wheel/move_stop", &move_stop_service);
ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("wheel/cmd_vel", &cmd_vel_callback);
//ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback);
ros::Subscriber<geometry_msgs::PoseStamped> goal_sub("/move_base_simple/goal", &goal_callback);

int main() {
    BusOut LEDs(LED1, LED2, LED3, LED4);
    coastAllMotor();
    mecanum_node.initNode();
    mecanum_node.subscribe(cmd_vel_sub);
    mecanum_node.subscribe(goal_sub);
    mecanum_node.advertiseService(stop_service);
    
    nav_msgs::Odometry odom;
    ros::Publisher odom_pub("wheel/odom", &odom);
    mecanum_node.advertise(odom_pub);
    tf::TransformBroadcaster odom_broadcaster;
    odom_broadcaster.init(mecanum_node);
    
    ros::Time current_time, last_time;
    current_time = mecanum_node.now();
    last_time = mecanum_node.now();
    
    Ticker ticker_odom;
    ticker_odom.attach(&odom_update, 0.010);
    
    while (1) {
        mecanum_node.spinOnce();
        current_time = mecanum_node.now();
        
        wait_ms(500);
        
        if(is_stop)
        {
            LEDs = 0x0F;
        }
        else
        {
            LEDs = LEDs+1;
        }
        
        //since all odometry is 6DOF we'll need a quaternion created from yaw
        geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta);
    
        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";
    
        odom_trans.transform.translation.x = my_odometry.x;
        odom_trans.transform.translation.y = my_odometry.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 = "odom";
    
        //set the position
        odom.pose.pose.position.x = my_odometry.x;
        odom.pose.pose.position.y = my_odometry.y;
        odom.pose.pose.position.z = 0.0;
        odom.pose.pose.orientation = odom_quat;
    
        //set the velocity
        odom.child_frame_id = "base_link";
        odom.twist.twist.linear.x = my_odometry.vel_x;
        odom.twist.twist.linear.y = my_odometry.vel_y;
        odom.twist.twist.angular.z = my_odometry.vel_theta;
    
        //publish the message
        odom_pub.publish(&odom);
    }
}