Go to the documentation of this file.00001
00027 #ifndef _SR_MOTOR_ROBOT_LIB_HPP_
00028 #define _SR_MOTOR_ROBOT_LIB_HPP_
00029
00030 #include "sr_robot_lib/sr_robot_lib.hpp"
00031
00032 #include <sr_robot_msgs/ForceController.h>
00033 #include <sr_robot_msgs/ControlType.h>
00034 #include <sr_robot_msgs/ChangeControlType.h>
00035 #include <sr_robot_msgs/MotorSystemControls.h>
00036 #include <sr_robot_msgs/ChangeMotorSystemControls.h>
00037
00038 #include "sr_robot_lib/motor_updater.hpp"
00039 #include "sr_robot_lib/motor_data_checker.hpp"
00040
00041 #include <string>
00042 #include <queue>
00043 #include <utility>
00044 #include <list>
00045 #include <vector>
00046
00047 namespace shadow_robot
00048 {
00049 template<class StatusType, class CommandType>
00050 class SrMotorRobotLib :
00051 public SrRobotLib<StatusType, CommandType>
00052 {
00053 public:
00054 SrMotorRobotLib(hardware_interface::HardwareInterface *hw, ros::NodeHandle nh, ros::NodeHandle nhtilde,
00055 std::string device_id, std::string joint_prefix);
00056
00065 void update(StatusType *status_data);
00066
00073 void build_command(CommandType *command);
00074
00079 void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00080 diagnostic_updater::DiagnosticStatusWrapper &d);
00081
00085 void reinitialize_motors();
00086
00087
00088
00089 operation_mode::device_update_state::DeviceUpdateState motor_current_state;
00090
00091
00092 protected:
00100 virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
00101 std::vector<shadow_joints::JointToSensor> joint_to_sensors) = 0;
00102
00110 void calibrate_joint(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
00111
00119 void read_additional_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data);
00120
00129 void process_position_sensor_data(std::vector<shadow_joints::Joint>::iterator joint_tmp, StatusType *status_data,
00130 double timestamp);
00131
00140 std::vector<std::pair<std::string, bool> > humanize_flags(int flag);
00141
00158 void generate_force_control_config(int motor_index, int max_pwm, int sg_left, int sg_right,
00159 int f, int p, int i, int d, int imax,
00160 int deadband, int sign);
00161
00169 sr_actuator::SrMotorActuator *get_joint_actuator(std::vector<shadow_joints::Joint>::iterator joint_tmp)
00170 {
00171 return static_cast<sr_actuator::SrMotorActuator *>(joint_tmp->actuator_wrapper->actuator);
00172 }
00173
00179 boost::shared_ptr<generic_updater::MotorUpdater<CommandType> > motor_updater_;
00180
00181
00187 typedef std::pair<int, std::vector<crc_unions::union16> > ForceConfig;
00191 std::queue<ForceConfig, std::list<ForceConfig> > reconfig_queue;
00192
00193 int config_index;
00194
00195
00196 std::queue<int16_t, std::list<int16_t> > reset_motors_queue;
00197
00198
00199
00200 int motor_index_full;
00201
00202 int index_motor_in_msg;
00203
00204 int8u crc_byte;
00205 int16u crc_result;
00206 int8u crc_i;
00207
00208
00209
00210 std::vector<generic_updater::UpdateConfig> motor_update_rate_configs_vector;
00211
00212 boost::shared_ptr<generic_updater::MotorDataChecker> motor_data_checker;
00213
00214
00215
00216 sr_robot_msgs::ControlType control_type_;
00225 bool control_type_changed_flag_;
00226
00227 ros::ServiceServer change_control_type_;
00228
00229
00230 boost::shared_ptr<boost::mutex> lock_command_sending_;
00231
00241 bool change_control_type_callback_(sr_robot_msgs::ChangeControlType::Request &request,
00242 sr_robot_msgs::ChangeControlType::Response &response);
00243
00253 bool change_control_parameters(int16_t control_type);
00254
00255
00256 std::queue<std::vector<sr_robot_msgs::MotorSystemControls>,
00257 std::list<std::vector<sr_robot_msgs::MotorSystemControls> > > motor_system_control_flags_;
00258
00259 ros::ServiceServer motor_system_control_server_;
00260
00271 bool motor_system_controls_callback_(sr_robot_msgs::ChangeMotorSystemControls::Request &request,
00272 sr_robot_msgs::ChangeMotorSystemControls::Response &response);
00273 };
00274 }
00275
00276
00277
00278
00279
00280
00281
00282 #endif