gripper_manager.h

00001 #pragma once
00002 
00003 #include "catchrobo_sim/define.h"
00004 #include "catchrobo_sim/motor_manager.h"
00005 #include "catchrobo_sim/servo_manager.h"
00006 #include "catchrobo_sim/enable_manager.h"
00007 #include "catchrobo_sim/obstacle_avoidance.h"
00008 #include "motor_driver_bridge/motor_driver_struct.h"
00009 
00010 #include <std_msgs/Float32MultiArray.h>
00011 #include <sensor_msgs/JointState.h>
00012 #include <catchrobo_msgs/MyRosCmd.h>
00013 #include <catchrobo_msgs/MyRosCmdArray.h>
00014 #include <catchrobo_msgs/EnableCmd.h>
00015 #include <catchrobo_msgs/ErrorCode.h>
00016 
00017 #include <vector>
00018 #include <string>
00019 
00020 #define TORQUE_OUTPUT
00021 // #
00022 
00023 class GripperManager
00024 {
00025 public:
00026     GripperManager() : motor_num_(N_MOTORS)
00027     {
00028     }
00029 
00030     ////本当はMyRosCmdArrayを受け取るのがキレイだが、配列要素数を取得する計算がPCとmbedで変わってしまうため、MyRosCmdで受け取るようにしている。
00031     void setRosCmd(const catchrobo_msgs::MyRosCmd &command)
00032     {
00033         if (command.id >= motor_num_)
00034         {
00035             servo_manager_.setRosCmd(command);
00036             servo_manager_.resetT();
00037         }
00038     };
00039 
00040     void getMotorDrivesCommand(ControlStruct &cmd, ControlResult::ControlResult &result)
00041     {
00042         servo_manager_.getCmd(cmd, result);
00043         ///// cmdのidを上書き。初期値である0になっている場合があるため
00044         cmd.id = motor_num_;
00045     };
00046 
00047     void nextStep(float dt)
00048     {
00049         servo_manager_.nextStep(dt);
00050     }
00051 
00052     float getJointRad()
00053     {
00054         StateStruct state;
00055         servo_manager_.getState(state);
00056         return state.position;
00057     };
00058     void init(float arrive_threshold, float estimate_error_limit)
00059     {
00060         servo_manager_.init(arrive_threshold, estimate_error_limit, 0);
00061     }
00062 
00063 private:
00064     ServoManager servo_manager_;
00065 
00066     const int motor_num_;
00067 };