main.cpp

00001 #define USE_MBED
00002 
00003 #ifdef USE_MBED
00004 #include "mbed.h"
00005 #include "motor_driver_bridge/motor_driver_bridge_mbed.h"
00006 #include "catchrobo_sim/ros_bridge_mbed.h"
00007 const float MBED2ROS_DT = 0.1; // 10Hz
00008 #else
00009 #include <ros/ros.h>
00010 #include "sim_only/mbed.h"
00011 #include "sim_only/motor_driver_bridge_sim.h"
00012 #include "sim_only/ros_bridge_sim.h"
00013 #include "sim_only/ticker_sim.h"
00014 const float MBED2ROS_DT = 0.01;
00015 #endif
00016 
00017 #include <std_msgs/Float32MultiArray.h>
00018 #include "catchrobo_sim/robot_manager.h"
00019 #include "catchrobo_sim/gripper_manager.h"
00020 
00021 // const float SPIN_FREQUENCY_s = 0.001;
00022 const float MBED2GRIPPER_DT = 0.1;
00023 const float MBED2MOTOR_DT = 0.005; // 200Hz
00024 const int SERIAL_BAUD_RATE = 115200;
00025 const float ARRIVE_THRESHOLD_RAD[] = {0.1, 0.1, 0.1};
00026 const float FRICTION[] = {0.2, 0, 0};
00027 const float GRIPPER_THRESHOLD_RAD = 0.1;
00028 const float ESTIMATE_ERROR_LIMIT_RAD = 0.5;
00029 
00030 MotorDriverBridge motor_driver_bridge;
00031 RosBridge ros_bridge;
00032 RobotManager robot_manager;
00033 GripperManager gripper_manager;
00034 EnableManager enable_manager;
00035 
00036 void enableAll(bool is_enable)
00037 {
00038     for (int i = 0; i < N_MOTORS; i++)
00039     {
00040         motor_driver_bridge.enableMotor(i, is_enable);
00041         wait(0.1);
00042     }
00043 }
00044 
00045 void motorDriverCallback(const StateStruct &input)
00046 {
00047     robot_manager.setCurrentState(input);
00048 };
00049 
00050 void rosCallback(const catchrobo_msgs::MyRosCmd &command)
00051 {
00052     if (command.mode == catchrobo_msgs::MyRosCmd::PEG_IN_HOLE_MODE)
00053     {
00054         bool result[N_MOTORS] = {};
00055         robot_manager.arriveCheck(result);
00056         for (int i = 0; i < N_MOTORS; i++)
00057         {
00058             if (result[i])
00059             {
00060                 catchrobo_msgs::ErrorCode error;
00061                 error.id = i;
00062                 error.error_code = catchrobo_msgs::ErrorCode::FINISH;
00063                 ros_bridge.publishError(error);
00064                 // ros_bridge.publishFinishFlag(i);
00065                 // ros::Duration dt = ros::Time::now() - t;
00066                 // ROS_INFO_STREAM("dt " << dt.toSec() );
00067             }
00068         }
00069         return;
00070     }
00071     robot_manager.setRosCmd(command);
00072     gripper_manager.setRosCmd(command);
00073 }
00074 
00075 void mbed2MotorDriverTimerCallback()
00076 {
00077 
00078     // ros::Time t = ros::Time::now();
00079     //// enable check
00080     catchrobo_msgs::ErrorCode error;
00081     sensor_msgs::JointState joint_state;
00082     robot_manager.getJointState(joint_state);
00083     enable_manager.check(joint_state, error);
00084 
00085     //// errorならdisable指示およびerror のpublish
00086     if (error.error_code != catchrobo_msgs::ErrorCode::NONE)
00087     {
00088         enableAll(false);
00089         enable_manager.setCurrentEnable(false);
00090         ros_bridge.publishError(error);
00091     }
00092 
00093     if (enable_manager.getEnable()) //// enableならtをすすめる
00094     {
00095         robot_manager.nextStep(MBED2MOTOR_DT);
00096     }
00097     else //// disableなら脱力指示
00098     {
00099         robot_manager.disable();
00100     }
00101     //// update target value
00102     ControlStruct control[N_MOTORS] = {};
00103     ControlResult::ControlResult result[N_MOTORS] = {};
00104     robot_manager.getMotorDrivesCommand(control, result);
00105     for (int i = 0; i < N_MOTORS; i++)
00106     {
00107         motor_driver_bridge.publish(control[i]);
00108         if (result[i] == ControlResult::FINISH)
00109         {
00110             error.id = i;
00111             error.error_code = catchrobo_msgs::ErrorCode::FINISH;
00112             ros_bridge.publishError(error);
00113             // ros_bridge.publishFinishFlag(i);
00114             // ros::Duration dt = ros::Time::now() - t;
00115             // ROS_INFO_STREAM("dt " << dt.toSec() );
00116         }
00117     }
00118 }
00119 
00120 void mbed2RosTimerCallback()
00121 {
00122     //// radius
00123     // sensor_msgs::JointState joint_state;
00124     // robot_manager.getJointState(joint_state);
00125 
00126     std_msgs::Float32MultiArray joint_rad;
00127     robot_manager.getJointRad(joint_rad);
00128     joint_rad.data[N_MOTORS] = gripper_manager.getJointRad();
00129     ros_bridge.publishJointState(joint_rad);
00130 };
00131 void enableCallback(const catchrobo_msgs::EnableCmd &input)
00132 {
00133     enable_manager.setCmd(input);
00134     enableAll(input.is_enable);
00135     enable_manager.setCurrentEnable(input.is_enable);
00136 }
00137 
00138 void gripperTimerCallback()
00139 {
00140 
00141     ControlStruct control;
00142     ControlResult::ControlResult result;
00143     gripper_manager.getMotorDrivesCommand(control, result);
00144     gripper_manager.nextStep(MBED2GRIPPER_DT);
00145     motor_driver_bridge.publish(control);
00146     // ROS_INFO_STREAM("gripper id " << control.id <<" p " << control.p_des);
00147     // ROS_INFO_STREAM(control);
00148     if (result == ControlResult::FINISH)
00149     {
00150         catchrobo_msgs::ErrorCode error;
00151         error.id = N_MOTORS;
00152         error.error_code = catchrobo_msgs::ErrorCode::FINISH;
00153         ros_bridge.publishError(error);
00154         // ros_bridge.publishFinishFlag(i);
00155     }
00156 }
00157 
00158 int main(int argc, char **argv)
00159 {
00160 #ifndef USE_MBED
00161     ros::init(argc, argv, "mbed_sim");
00162     ros::NodeHandle nh("");
00163     motor_driver_bridge.setNodeHandlePtr(&nh);
00164     ros_bridge.setNodeHandlePtr(&nh);
00165 #endif
00166     robot_manager.init(ARRIVE_THRESHOLD_RAD, FRICTION, ESTIMATE_ERROR_LIMIT_RAD);
00167     gripper_manager.init(GRIPPER_THRESHOLD_RAD, ESTIMATE_ERROR_LIMIT_RAD);
00168 
00169     ros_bridge.init(SERIAL_BAUD_RATE, rosCallback, enableCallback);
00170 
00171     int motor_directions[] = {-1, -1, -1, 1};
00172     motor_driver_bridge.init(motorDriverCallback, motor_directions);
00173 
00174     //// 初期値を暫定原点にする。後にROS指示で原点だしを行う
00175     for (size_t i = 0; i < N_MOTORS; i++)
00176     {
00177         motor_driver_bridge.setOrigin(i);
00178         wait(0.5);
00179     }
00180 
00181     //// motor driverへの指示開始
00182     Ticker ticker_motor_driver_send;
00183     ticker_motor_driver_send.attach(&mbed2MotorDriverTimerCallback, MBED2MOTOR_DT);
00184 
00185     //// ros へのフィードバック開始
00186     Ticker ticker;
00187     ticker.attach(&mbed2RosTimerCallback, MBED2ROS_DT);
00188 
00189     Ticker ticker_gripper;
00190     ticker_gripper.attach(&gripperTimerCallback, MBED2GRIPPER_DT);
00191 
00192     ros_bridge.spin();
00193 }