diff --git a/sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp b/sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp index c5c28109..2fe6bb85 100644 --- a/sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp +++ b/sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp @@ -139,7 +139,6 @@ class SrMotorHandLib : */ std::vector read_joint_to_motor_mapping(); - static const int nb_motor_data; static const char *human_readable_motor_data_types[]; static const int32u motor_data_types[]; diff --git a/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp b/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp index 451f4f4f..4fb6fa45 100644 --- a/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp +++ b/sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp @@ -26,6 +26,8 @@ #ifndef _SR_ROBOT_LIB_HPP_ #define _SR_ROBOT_LIB_HPP_ +#include + #include #include #include @@ -35,9 +37,10 @@ #include #include #include - +#include // used to publish debug values #include +#include #include @@ -313,7 +316,6 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; @@ -331,6 +333,10 @@ class SrRobotLib ros::NodeHandle node_handle; std_msgs::Int16 msg_debug; #endif + std_msgs::Int16 msg_debug_tom; + std_msgs::Int16MultiArray msg_array_tom_r; + std_msgs::Int16MultiArray msg_array_tom_l; + std_msgs::Int16MultiArray msg_array_tom_pwm; // The update rate for each sensor information type std::vector generic_sensor_update_rate_configs_vector; @@ -354,7 +360,9 @@ class SrRobotLib static const double tactile_timeout; ros::Duration tactile_init_max_duration; ros::Timer tactile_check_init_timeout_timer; - + boost::shared_ptr > rt_pub_all_r; + boost::shared_ptr > rt_pub_all_l; + boost::shared_ptr > rt_pub_all_pwm; /// A mutual exclusion object to ensure that the intitialization timeout event does work without threading issues boost::shared_ptr lock_tactile_init_timeout_; diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 03ad56f6..6cfb4f03 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -54,6 +54,7 @@ using generic_updater::MotorDataChecker; using boost::shared_ptr; using boost::static_pointer_cast; + namespace shadow_robot { @@ -89,6 +90,44 @@ namespace shadow_robot ROS_INFO("Using TORQUE control."); } +const unsigned int data_sz = 20; +this->msg_array_tom_r.layout.dim.push_back(std_msgs::MultiArrayDimension()); +this->msg_array_tom_l.layout.dim.push_back(std_msgs::MultiArrayDimension()); +this->msg_array_tom_pwm.layout.dim.push_back(std_msgs::MultiArrayDimension()); + +this->msg_array_tom_pwm.layout.dim[0].size = data_sz; +this->msg_array_tom_pwm.layout.dim[0].stride = 1; +this->msg_array_tom_pwm.layout.dim[0].label = "bla"; + +this->msg_array_tom_r.layout.dim[0].size = data_sz; +this->msg_array_tom_r.layout.dim[0].stride = 1; +this->msg_array_tom_r.layout.dim[0].label = "bla"; + +this->msg_array_tom_l.layout.dim[0].size = data_sz; +this->msg_array_tom_l.layout.dim[0].stride = 1; +this->msg_array_tom_l.layout.dim[0].label = "bla"; + +this->msg_array_tom_l.data.resize(data_sz); +this->msg_array_tom_r.data.resize(data_sz); +this->msg_array_tom_pwm.data.resize(data_sz); + + + this->rt_pub_all_l = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 100)); + + this->rt_pub_all_r = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 100)); + + + this->rt_pub_all_pwm = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_pwm", 100)); + + this->rt_pub_all_pwm->msg_ = this->msg_array_tom_pwm; + this->rt_pub_all_l->msg_ = this->msg_array_tom_l; + this->rt_pub_all_r->msg_ = this->msg_array_tom_r; + + + #ifdef DEBUG_PUBLISHER this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const); for (int i = 0; i < this->nb_debug_publishers_const; ++i) @@ -188,6 +227,17 @@ namespace shadow_robot } } // end for joint + if (this->rt_pub_all_l->trylock()) + this->rt_pub_all_l->unlockAndPublish(); + + + if (this->rt_pub_all_r->trylock()) + this->rt_pub_all_r->unlockAndPublish(); + + + if (this->rt_pub_all_pwm->trylock()) + this->rt_pub_all_pwm->unlockAndPublish(); + // then we read the tactile sensors information this->update_tactile_info(status_data); } // end update() @@ -681,6 +731,14 @@ namespace shadow_robot actuator->motor_state_.strain_gauge_left_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); + //if (this->rt_pub_all_l->trylock()) + this->rt_pub_all_l->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_; + + + // if (this->rt_pub_all_r->trylock()) + this->rt_pub_all_r->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_; + + #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { @@ -694,6 +752,7 @@ namespace shadow_robot actuator->motor_state_.strain_gauge_right_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); + #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { @@ -706,6 +765,7 @@ namespace shadow_robot case MOTOR_DATA_PWM: actuator->motor_state_.pwm_ = static_cast (static_cast (status_data->motor_data_packet[index_motor_in_msg].misc)); + this->rt_pub_all_pwm->msg_.data[actuator_wrapper->motor_id] = actuator->motor_state_.pwm_; #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19)