sr_robot_lib.hpp
Go to the documentation of this file.
00001 
00027 #ifndef _SR_ROBOT_LIB_HPP_
00028 #define _SR_ROBOT_LIB_HPP_
00029 
00030 #include <boost/smart_ptr.hpp>
00031 #include <boost/ptr_container/ptr_vector.hpp>
00032 #include <boost/thread.hpp>
00033 #include <vector>
00034 #include <deque>
00035 
00036 //used to publish debug values
00037 #include <std_msgs/Int16.h>
00038 
00039 #include <sr_hardware_interface/sr_actuator.hpp>
00040 
00041 #include <diagnostic_msgs/DiagnosticStatus.h>
00042 #include <diagnostic_updater/DiagnosticStatusWrapper.h>
00043 
00044 #include <sr_robot_msgs/NullifyDemand.h>
00045 #include <sr_robot_msgs/SetDebugData.h>
00046 
00047 #include <sr_utilities/sr_math_utils.hpp>
00048 #include <sr_utilities/calibration.hpp>
00049 #include <sr_utilities/thread_safe_map.hpp>
00050 
00051 #include <sr_self_test/sr_self_test.hpp>
00052 
00053 #include "sr_robot_lib/sr_joint_motor.hpp"
00054 #include "sr_robot_lib/generic_tactiles.hpp"
00055 
00056 #include <sr_external_dependencies/types_for_external.h>
00057 extern "C"
00058 {
00059   #include <sr_external_dependencies/external/0220_palm_edc/0220_palm_edc_ethercat_protocol.h>
00060   #include <sr_external_dependencies/external/0230_palm_edc_TS/0230_palm_edc_ethercat_protocol.h>
00061   #include <sr_external_dependencies/external/0320_palm_edc_muscle/0320_palm_edc_ethercat_protocol.h>
00062   #include <sr_external_dependencies/external/simplemotor-bootloader/bootloader.h>
00063 }
00064 
00065 namespace operation_mode
00066 {
00067   namespace robot_state
00068   {
00069     enum RobotState
00070     {
00071       INITIALIZATION,
00072       OPERATION,
00073       SHUTDOWN
00074     };
00075   }
00076 }
00077 
00078 namespace crc_unions
00079 {
00080   typedef union
00081   {
00082     int16u word;
00083     int8u byte[2];
00084   } union16;
00085 }
00086 
00087 namespace shadow_robot
00088 {
00089   template <class StatusType, class CommandType>
00090   class SrRobotLib
00091   {
00092   public:
00093     SrRobotLib(pr2_hardware_interface::HardwareInterface *hw);
00094     virtual ~SrRobotLib() {};
00095 
00104     virtual void update(StatusType* status_data) = 0;
00105 
00111     virtual void build_command(CommandType* command) = 0;
00112 
00118     void build_tactile_command(CommandType* command);
00119 
00125     void update_tactile_info(StatusType* status);
00126 
00131     virtual void add_diagnostics(std::vector<diagnostic_msgs::DiagnosticStatus> &vec,
00132                          diagnostic_updater::DiagnosticStatusWrapper &d) = 0;
00133 
00137     void reinitialize_sensors();
00138 
00151     bool nullify_demand_callback( sr_robot_msgs::NullifyDemand::Request& request,
00152                                   sr_robot_msgs::NullifyDemand::Response& response );
00153 
00155     shadow_joints::CalibrationMap calibration_map;
00156 
00162     boost::shared_ptr<tactiles::GenericTactiles<StatusType, CommandType> > tactiles;
00163     boost::shared_ptr<tactiles::GenericTactiles<StatusType, CommandType> > tactiles_init;
00164 
00169     int main_pic_idle_time;
00170 
00176     int main_pic_idle_time_min;
00177 
00179     operation_mode::device_update_state::DeviceUpdateState tactile_current_state;
00180 
00181   protected:
00183     boost::ptr_vector<shadow_joints::Joint> joints_vector;
00184 
00193     virtual void initialize(std::vector<std::string> joint_names, std::vector<int> actuator_ids,
00194                             std::vector<shadow_joints::JointToSensor> joint_to_sensors,
00195                             std::vector<sr_actuator::SrGenericActuator*> actuators) = 0;
00196 
00204     void calibrate_joint(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data);
00205 
00206 
00215     void process_position_sensor_data(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp, StatusType* status_data, double timestamp);
00216 
00226     sr_actuator::SrActuatorState* get_joint_actuator_state(boost::ptr_vector<shadow_joints::Joint>::iterator joint_tmp);
00227 
00235     std::vector<shadow_joints::JointToSensor> read_joint_to_sensor_mapping();
00236 
00243     shadow_joints::CalibrationMap read_joint_calibration();
00244 
00255     std::vector<generic_updater::UpdateConfig> read_update_rate_configs(std::string base_param, int nb_data_defined, const char* human_readable_data_types[], const int32u data_types[]);
00256 
00257 
00259     boost::shared_ptr<shadow_robot::JointCalibration> calibration_tmp;
00260 
00261 
00263     ros::NodeHandle nh_tilde;
00264 
00266     ros::NodeHandle nodehandle_;
00267 
00268 
00269 #ifdef DEBUG_PUBLISHER
00270 
00271     static const int nb_debug_publishers_const;
00272     std::vector<ros::Publisher> debug_publishers;
00280     std::vector<boost::shared_ptr<std::pair<int, int> > > debug_motor_indexes_and_data;
00281     //static const int debug_mutex_lock_wait_time;
00282     //boost::shared_mutex debug_mutex;
00283     ros::NodeHandle node_handle;
00284     std_msgs::Int16 msg_debug;
00285 #endif
00286 
00288     std::vector<generic_updater::UpdateConfig> generic_sensor_update_rate_configs_vector;
00290     std::vector<generic_updater::UpdateConfig> pst3_sensor_update_rate_configs_vector;
00292     std::vector<generic_updater::UpdateConfig> biotac_sensor_update_rate_configs_vector;
00294     std::vector<generic_updater::UpdateConfig> ubi0_sensor_update_rate_configs_vector;
00295 
00296 
00297     static const int nb_sensor_data;
00298     static const char* human_readable_sensor_data_types[];
00299     static const int32u sensor_data_types[];
00300 
00301 
00303     bool nullify_demand_;
00305     ros::ServiceServer nullify_demand_server_;
00306 
00308     void checkSelfTests();
00309 
00310     boost::shared_ptr<SrSelfTest> self_tests_;
00311 
00313     boost::shared_ptr<boost::thread> self_test_thread_;
00314   };//end class
00315 }//end namespace
00316 
00317 /* For the emacs weenies in the crowd.
00318 Local Variables:
00319    c-basic-offset: 2
00320 End:
00321 */
00322 
00323 #endif


sr_robot_lib
Author(s): Ugo Cupcic / ugo@shadowrobot.com , software@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:37:50