From 9f622893276d1b1e99b22488f8cebc5b3ca4930e Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 11:42:08 +0000 Subject: [PATCH 01/14] debug --- .../include/sr_robot_lib/sr_robot_lib.hpp | 4 ++++ sr_robot_lib/src/sr_motor_robot_lib.cpp | 18 +++++++++++++++++- sr_robot_lib/src/sr_robot_lib.cpp | 1 + 3 files changed, 22 insertions(+), 1 deletion(-) 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..7c15347b 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 @@ -318,6 +318,8 @@ class SrRobotLib // These publishers are useful for debugging static const int nb_debug_publishers_const; std::vector debug_publishers; + static const int nb_debug_publishers_const_tom; + std::vector debug_publishers_tom; /* * A vector containing pairs: * - associate a motor index @@ -326,10 +328,12 @@ class SrRobotLib * This vector has the same size as the debug_publishers vector. */ std::vector > > debug_motor_indexes_and_data; + std::vector > > debug_motor_indexes_and_data_tom; // static const int debug_mutex_lock_wait_time; // boost::shared_mutex debug_mutex; ros::NodeHandle node_handle; std_msgs::Int16 msg_debug; + std_msgs::Int16 msg_debug_tom; #endif // The update rate for each sensor information type diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 03ad56f6..bb889144 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -98,6 +98,13 @@ namespace shadow_robot this->debug_publishers.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); } #endif + this->debug_motor_indexes_and_data_tom.resize(this->nb_debug_publishers_const_tom); + for (int i = 0; i < this->nb_debug_publishers_const_tom; ++i) + { + ostringstream ss; + ss << "srh/tom_debug_" << i; + this->debug_publishers_tom.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); + } } template @@ -684,7 +691,7 @@ namespace shadow_robot #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { - // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); + // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug.data = actuator->motor_state_.strain_gauge_left_; this->debug_publishers[0].publish(this->msg_debug); } @@ -884,6 +891,15 @@ namespace shadow_robot break; } + //for (i=0; i<4; i++) + //{ + if (actuator_wrapper->motor_id == 1) + { + this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; + this->debug_publishers_tom[1].publish(this->msg_debug_tom); + } + //} + if (read_torque) { actuator->motor_state_.force_unfiltered_ = diff --git a/sr_robot_lib/src/sr_robot_lib.cpp b/sr_robot_lib/src/sr_robot_lib.cpp index 5d54d532..d1c5ea79 100644 --- a/sr_robot_lib/src/sr_robot_lib.cpp +++ b/sr_robot_lib/src/sr_robot_lib.cpp @@ -72,6 +72,7 @@ namespace shadow_robot // max of 20 publishers for debug template const int SrRobotLib::nb_debug_publishers_const = 20; + const int SrRobotLib::nb_debug_publishers_const_tom = 4; // template // const int SrRobotLib::debug_mutex_lock_wait_time = 100; #endif From 41473b5d45798f19b3911b2d841c1718c08cd9f0 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 11:49:19 +0000 Subject: [PATCH 02/14] debug --- sr_robot_lib/src/sr_motor_hand_lib.cpp | 5 +++++ sr_robot_lib/src/sr_motor_robot_lib.cpp | 4 ++++ 2 files changed, 9 insertions(+) diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp index 768578a8..949a876d 100644 --- a/sr_robot_lib/src/sr_motor_hand_lib.cpp +++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp @@ -542,6 +542,11 @@ namespace shadow_robot return false; } + this->debug_motor_indexes_and_data_tom[request.publisher_index] = shared_ptr >(new pair()); + + this->debug_motor_indexes_and_data_tom[request.publisher_index]->first = request.motor_index; + this->debug_motor_indexes_and_data_tom[request.publisher_index]->second = request.motor_data_type; + this->debug_motor_indexes_and_data[request.publisher_index] = shared_ptr >(new pair()); this->debug_motor_indexes_and_data[request.publisher_index]->first = request.motor_index; diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index bb889144..ef114108 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -290,6 +290,8 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) + if (publisher_index < 4) + BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { @@ -653,6 +655,8 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) + if (publisher_index < 4) + BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { From a16bea498bf4a2932a9bace98bfb7af79d60221e Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 12:02:57 +0000 Subject: [PATCH 03/14] debug --- sr_robot_lib/src/sr_motor_hand_lib.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp index 949a876d..d44c8ba4 100644 --- a/sr_robot_lib/src/sr_motor_hand_lib.cpp +++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp @@ -97,6 +97,9 @@ namespace shadow_robot ros::NodeHandle nhtilde, string device_id, string joint_prefix) : SrMotorRobotLib(hw, nh, nhtilde, device_id, joint_prefix) + , debug_service( + nh_tilde.advertiseService("set_debug_publishers", &SrMotorHandLib::set_debug_data_to_publish, this)) +#endif { // read the motor polling frequency from the parameter server this->motor_update_rate_configs_vector = this->read_update_rate_configs("motor_data_update_rate/", nb_motor_data, From 4fa1e60f96bb611534a27f306f3219dbabd70248 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 12:20:27 +0000 Subject: [PATCH 04/14] test --- .../sr_robot_lib/sr_motor_hand_lib.hpp | 7 +++++-- .../sr_robot_lib/sr_motor_robot_lib.hpp | 4 +++- .../include/sr_robot_lib/sr_robot_lib.hpp | 4 +++- sr_robot_lib/src/sr_motor_hand_lib.cpp | 13 +++++------- sr_robot_lib/src/sr_motor_robot_lib.cpp | 20 ++++++++++--------- sr_robot_lib/src/sr_robot_lib.cpp | 4 +++- 6 files changed, 30 insertions(+), 22 deletions(-) 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..34e8ace5 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 @@ -23,7 +23,10 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif +#define DEBUG_PUBLISHER #ifndef _SR_MOTOR_HAND_LIB_HPP_ #define _SR_MOTOR_HAND_LIB_HPP_ @@ -138,7 +141,7 @@ class SrMotorHandLib : * @return a vector of motor indexes, ordered by joint. */ std::vector read_joint_to_motor_mapping(); - + ros::ServiceServer debug_service; static const int nb_motor_data; static const char *human_readable_motor_data_types[]; diff --git a/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp b/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp index 8dabd16b..fd2bae4e 100644 --- a/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp +++ b/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp @@ -22,7 +22,9 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif #ifndef _SR_MOTOR_ROBOT_LIB_HPP_ #define _SR_MOTOR_ROBOT_LIB_HPP_ 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 7c15347b..f786d23f 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 @@ -22,7 +22,9 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif #ifndef _SR_ROBOT_LIB_HPP_ #define _SR_ROBOT_LIB_HPP_ diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp index d44c8ba4..1fc098c6 100644 --- a/sr_robot_lib/src/sr_motor_hand_lib.cpp +++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp @@ -24,7 +24,9 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif #include "sr_robot_lib/sr_motor_hand_lib.hpp" #include #include @@ -98,8 +100,8 @@ namespace shadow_robot string joint_prefix) : SrMotorRobotLib(hw, nh, nhtilde, device_id, joint_prefix) , debug_service( - nh_tilde.advertiseService("set_debug_publishers", &SrMotorHandLib::set_debug_data_to_publish, this)) -#endif + nhtilde.advertiseService("set_debug_publishers", &SrMotorHandLib::set_debug_data_to_publish, this)) + { // read the motor polling frequency from the parameter server this->motor_update_rate_configs_vector = this->read_update_rate_configs("motor_data_update_rate/", nb_motor_data, @@ -545,11 +547,6 @@ namespace shadow_robot return false; } - this->debug_motor_indexes_and_data_tom[request.publisher_index] = shared_ptr >(new pair()); - - this->debug_motor_indexes_and_data_tom[request.publisher_index]->first = request.motor_index; - this->debug_motor_indexes_and_data_tom[request.publisher_index]->second = request.motor_data_type; - this->debug_motor_indexes_and_data[request.publisher_index] = shared_ptr >(new pair()); this->debug_motor_indexes_and_data[request.publisher_index]->first = request.motor_index; diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index ef114108..e60fc0dc 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -23,7 +23,9 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif #include "sr_robot_lib/sr_motor_robot_lib.hpp" #include #include @@ -98,13 +100,13 @@ namespace shadow_robot this->debug_publishers.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); } #endif - this->debug_motor_indexes_and_data_tom.resize(this->nb_debug_publishers_const_tom); + /*this->debug_motor_indexes_and_data_tom.resize(this->nb_debug_publishers_const_tom); for (int i = 0; i < this->nb_debug_publishers_const_tom; ++i) { ostringstream ss; ss << "srh/tom_debug_" << i; this->debug_publishers_tom.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); - } + }*/ } template @@ -290,8 +292,8 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) - if (publisher_index < 4) - BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) + //if (publisher_index < 4) + //BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { @@ -655,8 +657,8 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) - if (publisher_index < 4) - BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) + //if (publisher_index < 4) + // BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { @@ -897,11 +899,11 @@ namespace shadow_robot //for (i=0; i<4; i++) //{ - if (actuator_wrapper->motor_id == 1) + /*if (actuator_wrapper->motor_id == 1) { this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; this->debug_publishers_tom[1].publish(this->msg_debug_tom); - } + }*/ //} if (read_torque) diff --git a/sr_robot_lib/src/sr_robot_lib.cpp b/sr_robot_lib/src/sr_robot_lib.cpp index d1c5ea79..e262e8fb 100644 --- a/sr_robot_lib/src/sr_robot_lib.cpp +++ b/sr_robot_lib/src/sr_robot_lib.cpp @@ -22,7 +22,9 @@ * * */ - +#ifndef DEBUG_PUBLISHER +#define DEBUG_PUBLISHER +#endif #include "sr_robot_lib/sr_robot_lib.hpp" #include #include From e21f9a47ce0656b572c5116623000acb04533e74 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 12:47:42 +0000 Subject: [PATCH 05/14] testing --- sr_robot_lib/include/sr_robot_lib/sr_motor_hand_lib.hpp | 5 +---- sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp | 4 +--- sr_robot_lib/include/sr_robot_lib/sr_robot_lib.hpp | 6 ++---- sr_robot_lib/src/sr_motor_hand_lib.cpp | 6 +----- sr_robot_lib/src/sr_motor_robot_lib.cpp | 8 +++++--- sr_robot_lib/src/sr_robot_lib.cpp | 4 +--- 6 files changed, 11 insertions(+), 22 deletions(-) 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 34e8ace5..18bfc178 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 @@ -23,10 +23,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif -#define DEBUG_PUBLISHER + #ifndef _SR_MOTOR_HAND_LIB_HPP_ #define _SR_MOTOR_HAND_LIB_HPP_ diff --git a/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp b/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp index fd2bae4e..8dabd16b 100644 --- a/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp +++ b/sr_robot_lib/include/sr_robot_lib/sr_motor_robot_lib.hpp @@ -22,9 +22,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif + #ifndef _SR_MOTOR_ROBOT_LIB_HPP_ #define _SR_MOTOR_ROBOT_LIB_HPP_ 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 f786d23f..6cc8abcd 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 @@ -22,9 +22,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif + #ifndef _SR_ROBOT_LIB_HPP_ #define _SR_ROBOT_LIB_HPP_ @@ -315,7 +313,7 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - + ros::Publisher fast_sg_l_pub; #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp index 1fc098c6..226d0d39 100644 --- a/sr_robot_lib/src/sr_motor_hand_lib.cpp +++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp @@ -24,9 +24,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif + #include "sr_robot_lib/sr_motor_hand_lib.hpp" #include #include @@ -99,8 +97,6 @@ namespace shadow_robot ros::NodeHandle nhtilde, string device_id, string joint_prefix) : SrMotorRobotLib(hw, nh, nhtilde, device_id, joint_prefix) - , debug_service( - nhtilde.advertiseService("set_debug_publishers", &SrMotorHandLib::set_debug_data_to_publish, this)) { // read the motor polling frequency from the parameter server diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index e60fc0dc..498f6e48 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -23,9 +23,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif + #include "sr_robot_lib/sr_motor_robot_lib.hpp" #include #include @@ -91,6 +89,10 @@ namespace shadow_robot ROS_INFO("Using TORQUE control."); } + ostringstream sss; + sss << "srh/debug_sg_"; + this->fast_sg_l_pub = this->nh_tilde.template advertise(sss.str().c_str(), 100); + #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) diff --git a/sr_robot_lib/src/sr_robot_lib.cpp b/sr_robot_lib/src/sr_robot_lib.cpp index e262e8fb..d1c5ea79 100644 --- a/sr_robot_lib/src/sr_robot_lib.cpp +++ b/sr_robot_lib/src/sr_robot_lib.cpp @@ -22,9 +22,7 @@ * * */ -#ifndef DEBUG_PUBLISHER -#define DEBUG_PUBLISHER -#endif + #include "sr_robot_lib/sr_robot_lib.hpp" #include #include From 73eaeb960f5c6e27e4c8cfc3bf272945a3e73ce0 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 13:32:18 +0000 Subject: [PATCH 06/14] testing --- .../include/sr_robot_lib/sr_robot_lib.hpp | 12 +++++-- sr_robot_lib/src/sr_motor_robot_lib.cpp | 35 +++++++++++++++++-- 2 files changed, 42 insertions(+), 5 deletions(-) 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 6cc8abcd..5c7af5df 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 @@ -313,7 +313,15 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - ros::Publisher fast_sg_l_pub; + ros::Publisher fast_sg_l_pub_1; + ros::Publisher fast_sg_l_pub_2; + ros::Publisher fast_sg_l_pub_3; + + ros::Publisher fast_sg_r_pub_1; + ros::Publisher fast_sg_r_pub_2; + ros::Publisher fast_sg_r_pub_3; + std::vector fast_sg_l; + std::vector fast_sg_r; #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; @@ -333,8 +341,8 @@ class SrRobotLib // boost::shared_mutex debug_mutex; ros::NodeHandle node_handle; std_msgs::Int16 msg_debug; - std_msgs::Int16 msg_debug_tom; #endif + std_msgs::Int16 msg_debug_tom; // The update rate for each sensor information type std::vector generic_sensor_update_rate_configs_vector; diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 498f6e48..3f694d44 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -89,9 +89,22 @@ namespace shadow_robot ROS_INFO("Using TORQUE control."); } - ostringstream sss; - sss << "srh/debug_sg_"; - this->fast_sg_l_pub = this->nh_tilde.template advertise(sss.str().c_str(), 100); + + this->fast_sg_l.resize(3); + for (int i=0; i<3; i++) + { + ostringstream sss; + sss << "srh/debug_sg_l_" << i; + this->fast_sg_l.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); + } + + this->fast_sg_r.resize(3); + for (int i=0; i<3; i++) + { + ostringstream sss; + sss << "srh/debug_sg_r_" << i; + this->fast_sg_r.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); + } #ifdef DEBUG_PUBLISHER this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const); @@ -696,6 +709,16 @@ namespace shadow_robot actuator->motor_state_.strain_gauge_left_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); + + if (actuator_wrapper->motor_id < 4){ + // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); + this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; + this->fast_sg_l[actuator_wrapper->motor_id].publish(this->msg_debug_tom); + } + + + + #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { @@ -709,6 +732,12 @@ namespace shadow_robot actuator->motor_state_.strain_gauge_right_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); + if (actuator_wrapper->motor_id < 4){ + // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); + this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_right_; + this->fast_sg_r[actuator_wrapper->motor_id].publish(this->msg_debug_tom); + } + #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { From 0b5aa765b2017e7674efc08d6aedf3a84d8ae6a2 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 29 Jul 2021 15:08:31 +0000 Subject: [PATCH 07/14] tidy --- .../sr_robot_lib/sr_motor_hand_lib.hpp | 1 - .../include/sr_robot_lib/sr_robot_lib.hpp | 10 ---- sr_robot_lib/src/sr_motor_hand_lib.cpp | 1 - sr_robot_lib/src/sr_motor_robot_lib.cpp | 48 +++++-------------- sr_robot_lib/src/sr_robot_lib.cpp | 1 - 5 files changed, 11 insertions(+), 50 deletions(-) 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 18bfc178..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 @@ -138,7 +138,6 @@ class SrMotorHandLib : * @return a vector of motor indexes, ordered by joint. */ std::vector read_joint_to_motor_mapping(); - ros::ServiceServer debug_service; static const int nb_motor_data; static const char *human_readable_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 5c7af5df..4a72bf72 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 @@ -313,21 +313,12 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - ros::Publisher fast_sg_l_pub_1; - ros::Publisher fast_sg_l_pub_2; - ros::Publisher fast_sg_l_pub_3; - - ros::Publisher fast_sg_r_pub_1; - ros::Publisher fast_sg_r_pub_2; - ros::Publisher fast_sg_r_pub_3; std::vector fast_sg_l; std::vector fast_sg_r; #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; std::vector debug_publishers; - static const int nb_debug_publishers_const_tom; - std::vector debug_publishers_tom; /* * A vector containing pairs: * - associate a motor index @@ -336,7 +327,6 @@ class SrRobotLib * This vector has the same size as the debug_publishers vector. */ std::vector > > debug_motor_indexes_and_data; - std::vector > > debug_motor_indexes_and_data_tom; // static const int debug_mutex_lock_wait_time; // boost::shared_mutex debug_mutex; ros::NodeHandle node_handle; diff --git a/sr_robot_lib/src/sr_motor_hand_lib.cpp b/sr_robot_lib/src/sr_motor_hand_lib.cpp index 226d0d39..768578a8 100644 --- a/sr_robot_lib/src/sr_motor_hand_lib.cpp +++ b/sr_robot_lib/src/sr_motor_hand_lib.cpp @@ -97,7 +97,6 @@ namespace shadow_robot ros::NodeHandle nhtilde, string device_id, string joint_prefix) : SrMotorRobotLib(hw, nh, nhtilde, device_id, joint_prefix) - { // read the motor polling frequency from the parameter server this->motor_update_rate_configs_vector = this->read_update_rate_configs("motor_data_update_rate/", nb_motor_data, diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 3f694d44..95505869 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -89,22 +89,16 @@ namespace shadow_robot ROS_INFO("Using TORQUE control."); } - - this->fast_sg_l.resize(3); - for (int i=0; i<3; i++) - { - ostringstream sss; - sss << "srh/debug_sg_l_" << i; - this->fast_sg_l.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); - } - - this->fast_sg_r.resize(3); - for (int i=0; i<3; i++) - { - ostringstream sss; - sss << "srh/debug_sg_r_" << i; - this->fast_sg_r.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); - } + this->fast_sg_l.resize(3); + this->fast_sg_r.resize(3); + for (int i=0; i<3; i++) + { + ostringstream ssr, ssl; + ssl << "srh/debug_sg_l_" << i; + ssr << "srh/debug_sg_r_" << i; + this->fast_sg_l.push_back(this->nh_tilde.template advertise(ssl.str().c_str(), 100)); + this->fast_sg_r.push_back(this->nh_tilde.template advertise(ssr.str().c_str(), 100)); + } #ifdef DEBUG_PUBLISHER this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const); @@ -115,13 +109,6 @@ namespace shadow_robot this->debug_publishers.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); } #endif - /*this->debug_motor_indexes_and_data_tom.resize(this->nb_debug_publishers_const_tom); - for (int i = 0; i < this->nb_debug_publishers_const_tom; ++i) - { - ostringstream ss; - ss << "srh/tom_debug_" << i; - this->debug_publishers_tom.push_back(this->node_handle.template advertise(ss.str().c_str(), 100)); - }*/ } template @@ -307,8 +294,6 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) - //if (publisher_index < 4) - //BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { @@ -672,8 +657,6 @@ namespace shadow_robot if (this->debug_mutex.try_lock()) { BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data) - //if (publisher_index < 4) - // BOOST_FOREACH(debug_pair, this->debug_motor_indexes_and_data_tom) { if (debug_pair != NULL) { @@ -722,7 +705,7 @@ namespace shadow_robot #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { - // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); + // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug.data = actuator->motor_state_.strain_gauge_left_; this->debug_publishers[0].publish(this->msg_debug); } @@ -928,15 +911,6 @@ namespace shadow_robot break; } - //for (i=0; i<4; i++) - //{ - /*if (actuator_wrapper->motor_id == 1) - { - this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; - this->debug_publishers_tom[1].publish(this->msg_debug_tom); - }*/ - //} - if (read_torque) { actuator->motor_state_.force_unfiltered_ = diff --git a/sr_robot_lib/src/sr_robot_lib.cpp b/sr_robot_lib/src/sr_robot_lib.cpp index d1c5ea79..5d54d532 100644 --- a/sr_robot_lib/src/sr_robot_lib.cpp +++ b/sr_robot_lib/src/sr_robot_lib.cpp @@ -72,7 +72,6 @@ namespace shadow_robot // max of 20 publishers for debug template const int SrRobotLib::nb_debug_publishers_const = 20; - const int SrRobotLib::nb_debug_publishers_const_tom = 4; // template // const int SrRobotLib::debug_mutex_lock_wait_time = 100; #endif From 4cde2c60e800bd4537edbd715408be0cf12c6972 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Fri, 30 Jul 2021 08:47:42 +0000 Subject: [PATCH 08/14] rt not working --- .../include/sr_robot_lib/sr_robot_lib.hpp | 13 ++++++++++--- sr_robot_lib/src/sr_motor_robot_lib.cpp | 18 ++++++++++++++---- 2 files changed, 24 insertions(+), 7 deletions(-) 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 4a72bf72..932a989a 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 @@ -35,7 +35,7 @@ #include #include #include - +#include // used to publish debug values #include @@ -313,8 +313,15 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - std::vector fast_sg_l; - std::vector fast_sg_r; + typedef realtime_tools::RealtimePublisher RtSgPub; + typedef boost::shared_ptr RtSgPubPtr; + RtSgPubPtr rt_sg_pub_; + + std::vector>> fast_sg_l; + std::vector>> fast_sg_r; +// std::vector fast_sg_r; + + #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 95505869..8fa949ca 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -88,9 +88,19 @@ namespace shadow_robot control_type_.control_type = sr_robot_msgs::ControlType::FORCE; ROS_INFO("Using TORQUE control."); } - this->fast_sg_l.resize(3); this->fast_sg_r.resize(3); + for (int i=0; i<3; i++) + { + ostringstream ssr, ssl; + ssl << "srh/debug_sg_l_" << i; + ssr << "srh/debug_sg_r_" << i; + this->fast_sg_l.push_back((new realtime_tools::RealtimePublisher(nhtilde, ssl.str().c_str(), 4))); + this->fast_sg_r.push_back((new realtime_tools::RealtimePublisher(nhtilde, ssr.str().c_str(), 4))); + } +/* + + for (int i=0; i<3; i++) { ostringstream ssr, ssl; @@ -98,7 +108,7 @@ namespace shadow_robot ssr << "srh/debug_sg_r_" << i; this->fast_sg_l.push_back(this->nh_tilde.template advertise(ssl.str().c_str(), 100)); this->fast_sg_r.push_back(this->nh_tilde.template advertise(ssr.str().c_str(), 100)); - } + }*/ #ifdef DEBUG_PUBLISHER this->debug_motor_indexes_and_data.resize(this->nb_debug_publishers_const); @@ -696,7 +706,7 @@ namespace shadow_robot if (actuator_wrapper->motor_id < 4){ // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; - this->fast_sg_l[actuator_wrapper->motor_id].publish(this->msg_debug_tom); + this->fast_sg_l[actuator_wrapper->motor_id]->publish(this->msg_debug_tom); } @@ -718,7 +728,7 @@ namespace shadow_robot if (actuator_wrapper->motor_id < 4){ // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_right_; - this->fast_sg_r[actuator_wrapper->motor_id].publish(this->msg_debug_tom); + this->fast_sg_r[actuator_wrapper->motor_id]->publish(this->msg_debug_tom); } #ifdef DEBUG_PUBLISHER From e4e04637c0e98b77a9058620e8a59ccf6ca23e23 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Fri, 30 Jul 2021 11:23:07 +0000 Subject: [PATCH 09/14] rt test --- .../include/sr_robot_lib/sr_robot_lib.hpp | 12 +++---- sr_robot_lib/src/sr_motor_robot_lib.cpp | 36 +++++++++++++++++-- 2 files changed, 37 insertions(+), 11 deletions(-) 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 5c7af5df..01039dd0 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 @@ -313,13 +315,6 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - ros::Publisher fast_sg_l_pub_1; - ros::Publisher fast_sg_l_pub_2; - ros::Publisher fast_sg_l_pub_3; - - ros::Publisher fast_sg_r_pub_1; - ros::Publisher fast_sg_r_pub_2; - ros::Publisher fast_sg_r_pub_3; std::vector fast_sg_l; std::vector fast_sg_r; #ifdef DEBUG_PUBLISHER @@ -366,7 +361,8 @@ 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_l; + boost::shared_ptr > rt_pub_r; /// 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 3f694d44..06ffd79a 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -89,6 +89,12 @@ namespace shadow_robot ROS_INFO("Using TORQUE control."); } + this->rt_pub_l = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_l", 4)); + + this->rt_pub_r = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_r", 4)); +/* this->fast_sg_l.resize(3); for (int i=0; i<3; i++) @@ -105,7 +111,7 @@ namespace shadow_robot sss << "srh/debug_sg_r_" << i; this->fast_sg_r.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); } - +*/ #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) @@ -710,13 +716,25 @@ namespace shadow_robot static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); + if (actuator_wrapper->motor_id == 1){ + if (this->rt_pub_l->trylock()) + { + std_msgs::Int16 int_message; + //int_message.header.stamp = ros::Time::now(); + int_message.data = actuator->motor_state_.strain_gauge_left_; + this->rt_pub_l->msg_ = int_message; + this->rt_pub_l->unlockAndPublish(); + } + } +/* + if (actuator_wrapper->motor_id < 4){ // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; this->fast_sg_l[actuator_wrapper->motor_id].publish(this->msg_debug_tom); } - +*/ #ifdef DEBUG_PUBLISHER @@ -731,13 +749,25 @@ namespace shadow_robot case MOTOR_DATA_SGR: actuator->motor_state_.strain_gauge_right_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); - +/* if (actuator_wrapper->motor_id < 4){ // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_right_; this->fast_sg_r[actuator_wrapper->motor_id].publish(this->msg_debug_tom); + }*/ + + if (actuator_wrapper->motor_id == 1){ + if (this->rt_pub_r->trylock()) + { + std_msgs::Int16 int_message; + // int_message.header.stamp = ros::Time::now(); + int_message.data = actuator->motor_state_.strain_gauge_right_; + this->rt_pub_r->msg_ = int_message; + this->rt_pub_r->unlockAndPublish(); + } } + #ifdef DEBUG_PUBLISHER if (actuator_wrapper->motor_id == 19) { From 395639addc866f846f9e4b6868521761dafb5d09 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 2 Aug 2021 11:18:04 +0000 Subject: [PATCH 10/14] realtime multiarray pub --- .../include/sr_robot_lib/sr_robot_lib.hpp | 5 +++++ sr_robot_lib/src/sr_motor_robot_lib.cpp | 21 +++++++++++++++++++ 2 files changed, 26 insertions(+) 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 668c3061..bfdf3417 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 @@ -40,6 +40,7 @@ #include // used to publish debug values #include +#include #include @@ -342,6 +343,8 @@ class SrRobotLib 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; // The update rate for each sensor information type std::vector generic_sensor_update_rate_configs_vector; @@ -367,6 +370,8 @@ class SrRobotLib ros::Timer tactile_check_init_timeout_timer; boost::shared_ptr > rt_pub_l; boost::shared_ptr > rt_pub_r; + boost::shared_ptr > rt_pub_all_r; + boost::shared_ptr > rt_pub_all_l; /// 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 ecdfaa4b..551fdf34 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -94,6 +94,13 @@ namespace shadow_robot this->rt_pub_r = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_r", 4)); + + this->rt_pub_all_l = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 4)); + + this->rt_pub_all_r = boost::shared_ptr >( + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 4)); + /* this->fast_sg_l.resize(3); @@ -726,6 +733,20 @@ namespace shadow_robot } */ + + if (this->rt_pub_all_l->trylock()) + { + this->msg_array_tom_l.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_; + this->rt_pub_all_l->msg_ = this->msg_array_tom_l; + this->rt_pub_all_l->unlockAndPublish(); + } + + if (this->rt_pub_all_r->trylock()) + { + this->msg_array_tom_r.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_; + this->rt_pub_all_r->msg_ = this->msg_array_tom_r; + this->rt_pub_all_r->unlockAndPublish(); + } #ifdef DEBUG_PUBLISHER From 3f9181394282ba21cd47c55dce67432b8a79ad36 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 2 Aug 2021 13:33:01 +0000 Subject: [PATCH 11/14] uneven pub rates --- sr_robot_lib/src/sr_motor_robot_lib.cpp | 33 ++++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 551fdf34..4704096b 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,18 +90,35 @@ 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_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->rt_pub_l = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_l", 4)); this->rt_pub_r = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_r", 4)); + //std::cout << "1\n"; this->rt_pub_all_l = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 4)); - + //std::cout << "2\n"; this->rt_pub_all_r = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 4)); - + //std::cout << "3\n"; /* this->fast_sg_l.resize(3); @@ -733,18 +751,27 @@ namespace shadow_robot } */ - + //std::cout << "01\n"; if (this->rt_pub_all_l->trylock()) { + //std::cout << "02\n"; +// this->msg_array_tom_l.data.push_back(actuator->motor_state_.strain_gauge_left_); this->msg_array_tom_l.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_; + //std::cout << "03\n"; this->rt_pub_all_l->msg_ = this->msg_array_tom_l; + //std::cout << "04\n"; this->rt_pub_all_l->unlockAndPublish(); } + //std::cout << "05\n"; if (this->rt_pub_all_r->trylock()) { + //std::cout << "06\n"; +// this->msg_array_tom_r.data.push_back(actuator->motor_state_.strain_gauge_right_); this->msg_array_tom_r.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_; + //std::cout << "07\n"; this->rt_pub_all_r->msg_ = this->msg_array_tom_r; + //std::cout << "08\n"; this->rt_pub_all_r->unlockAndPublish(); } From ec8f88fd4b6e7ea1471d4e7198800ce3f4cebc9e Mon Sep 17 00:00:00 2001 From: carebare47 Date: Mon, 2 Aug 2021 13:46:00 +0000 Subject: [PATCH 12/14] cleaning --- sr_robot_lib/src/sr_motor_robot_lib.cpp | 23 +++++++---------------- 1 file changed, 7 insertions(+), 16 deletions(-) diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index 4704096b..a5bd2210 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -114,10 +114,10 @@ this->msg_array_tom_r.data.resize(data_sz); //std::cout << "1\n"; this->rt_pub_all_l = boost::shared_ptr >( - new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 4)); + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 100)); //std::cout << "2\n"; this->rt_pub_all_r = boost::shared_ptr >( - new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 4)); + new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 100)); //std::cout << "3\n"; /* @@ -730,7 +730,7 @@ this->msg_array_tom_r.data.resize(data_sz); case MOTOR_DATA_SGL: actuator->motor_state_.strain_gauge_left_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); - +/* if (actuator_wrapper->motor_id == 1){ if (this->rt_pub_l->trylock()) @@ -742,7 +742,8 @@ this->msg_array_tom_r.data.resize(data_sz); this->rt_pub_l->unlockAndPublish(); } } -/* + + if (actuator_wrapper->motor_id < 4){ // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); @@ -751,27 +752,17 @@ this->msg_array_tom_r.data.resize(data_sz); } */ - //std::cout << "01\n"; if (this->rt_pub_all_l->trylock()) { - //std::cout << "02\n"; -// this->msg_array_tom_l.data.push_back(actuator->motor_state_.strain_gauge_left_); this->msg_array_tom_l.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_; - //std::cout << "03\n"; this->rt_pub_all_l->msg_ = this->msg_array_tom_l; - //std::cout << "04\n"; this->rt_pub_all_l->unlockAndPublish(); } - //std::cout << "05\n"; if (this->rt_pub_all_r->trylock()) { - //std::cout << "06\n"; -// this->msg_array_tom_r.data.push_back(actuator->motor_state_.strain_gauge_right_); this->msg_array_tom_r.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_; - //std::cout << "07\n"; this->rt_pub_all_r->msg_ = this->msg_array_tom_r; - //std::cout << "08\n"; this->rt_pub_all_r->unlockAndPublish(); } @@ -793,7 +784,7 @@ this->msg_array_tom_r.data.resize(data_sz); // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_right_; this->fast_sg_r[actuator_wrapper->motor_id].publish(this->msg_debug_tom); - }*/ + } if (actuator_wrapper->motor_id == 1){ if (this->rt_pub_r->trylock()) @@ -804,7 +795,7 @@ this->msg_array_tom_r.data.resize(data_sz); this->rt_pub_r->msg_ = int_message; this->rt_pub_r->unlockAndPublish(); } - } + }*/ #ifdef DEBUG_PUBLISHER From 25ec5487b062d48886757444e8015ef62a45d47d Mon Sep 17 00:00:00 2001 From: carebare47 Date: Tue, 3 Aug 2021 12:15:03 +0000 Subject: [PATCH 13/14] rt mult array --- .../include/sr_robot_lib/sr_robot_lib.hpp | 11 --- sr_robot_lib/src/sr_motor_robot_lib.cpp | 89 +++---------------- 2 files changed, 14 insertions(+), 86 deletions(-) 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 bfdf3417..b32515f0 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 @@ -316,15 +316,6 @@ class SrRobotLib /// Id of the ethercat device (alias) std::string device_id_; - typedef realtime_tools::RealtimePublisher RtSgPub; - typedef boost::shared_ptr RtSgPubPtr; - RtSgPubPtr rt_sg_pub_; - - std::vector>> fast_sg_l; - std::vector>> fast_sg_r; -// std::vector fast_sg_r; - - #ifdef DEBUG_PUBLISHER // These publishers are useful for debugging static const int nb_debug_publishers_const; @@ -368,8 +359,6 @@ 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_l; - boost::shared_ptr > rt_pub_r; boost::shared_ptr > rt_pub_all_r; boost::shared_ptr > rt_pub_all_l; /// A mutual exclusion object to ensure that the intitialization timeout event does work without threading issues diff --git a/sr_robot_lib/src/sr_motor_robot_lib.cpp b/sr_robot_lib/src/sr_motor_robot_lib.cpp index a5bd2210..a223074a 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -105,38 +105,15 @@ this->msg_array_tom_l.data.resize(data_sz); this->msg_array_tom_r.data.resize(data_sz); - - this->rt_pub_l = boost::shared_ptr >( - new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_l", 4)); - - this->rt_pub_r = boost::shared_ptr >( - new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_r", 4)); - - //std::cout << "1\n"; this->rt_pub_all_l = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_l", 100)); - //std::cout << "2\n"; + this->rt_pub_all_r = boost::shared_ptr >( new realtime_tools::RealtimePublisher(this->nh_tilde, "rt_sg_all_r", 100)); - //std::cout << "3\n"; -/* - this->fast_sg_l.resize(3); - for (int i=0; i<3; i++) - { - ostringstream sss; - sss << "srh/debug_sg_l_" << i; - this->fast_sg_l.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); - } + this->rt_pub_all_l->msg_ = this->msg_array_tom_l; + this->rt_pub_all_r->msg_ = this->msg_array_tom_r; - this->fast_sg_r.resize(3); - for (int i=0; i<3; i++) - { - ostringstream sss; - sss << "srh/debug_sg_r_" << i; - this->fast_sg_r.push_back(this->nh_tilde.template advertise(sss.str().c_str(), 100)); - } -*/ #ifdef DEBUG_PUBLISHER @@ -238,6 +215,13 @@ this->msg_array_tom_r.data.resize(data_sz); } } // 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(); + // then we read the tactile sensors information this->update_tactile_info(status_data); } // end update() @@ -730,41 +714,13 @@ this->msg_array_tom_r.data.resize(data_sz); case MOTOR_DATA_SGL: actuator->motor_state_.strain_gauge_left_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); -/* - - if (actuator_wrapper->motor_id == 1){ - if (this->rt_pub_l->trylock()) - { - std_msgs::Int16 int_message; - //int_message.header.stamp = ros::Time::now(); - int_message.data = actuator->motor_state_.strain_gauge_left_; - this->rt_pub_l->msg_ = int_message; - this->rt_pub_l->unlockAndPublish(); - } - } - + //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 (actuator_wrapper->motor_id < 4){ - // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); - this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_left_; - this->fast_sg_l[actuator_wrapper->motor_id]->publish(this->msg_debug_tom); - } - -*/ - if (this->rt_pub_all_l->trylock()) - { - this->msg_array_tom_l.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_left_; - this->rt_pub_all_l->msg_ = this->msg_array_tom_l; - this->rt_pub_all_l->unlockAndPublish(); - } - if (this->rt_pub_all_r->trylock()) - { - this->msg_array_tom_r.data[actuator_wrapper->motor_id] = actuator->motor_state_.strain_gauge_right_; - this->rt_pub_all_r->msg_ = this->msg_array_tom_r; - this->rt_pub_all_r->unlockAndPublish(); - } + // 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 @@ -779,23 +735,6 @@ this->msg_array_tom_r.data.resize(data_sz); case MOTOR_DATA_SGR: actuator->motor_state_.strain_gauge_right_ = static_cast (status_data->motor_data_packet[index_motor_in_msg].misc); -/* - if (actuator_wrapper->motor_id < 4){ - // ROS_ERROR_STREAM("SGL " <motor_state_.strain_gauge_left_); - this->msg_debug_tom.data = actuator->motor_state_.strain_gauge_right_; - this->fast_sg_r[actuator_wrapper->motor_id].publish(this->msg_debug_tom); - } - - if (actuator_wrapper->motor_id == 1){ - if (this->rt_pub_r->trylock()) - { - std_msgs::Int16 int_message; - // int_message.header.stamp = ros::Time::now(); - int_message.data = actuator->motor_state_.strain_gauge_right_; - this->rt_pub_r->msg_ = int_message; - this->rt_pub_r->unlockAndPublish(); - } - }*/ #ifdef DEBUG_PUBLISHER From 4a0efcc1e960c64282de2159152e753bcaf83b41 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Fri, 6 Aug 2021 11:56:27 +0000 Subject: [PATCH 14/14] publishing more --- .../include/sr_robot_lib/sr_robot_lib.hpp | 2 ++ sr_robot_lib/src/sr_motor_robot_lib.cpp | 17 +++++++++++++++++ 2 files changed, 19 insertions(+) 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 b32515f0..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 @@ -336,6 +336,7 @@ class SrRobotLib 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; @@ -361,6 +362,7 @@ class SrRobotLib 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 a223074a..6cfb4f03 100644 --- a/sr_robot_lib/src/sr_motor_robot_lib.cpp +++ b/sr_robot_lib/src/sr_motor_robot_lib.cpp @@ -93,6 +93,12 @@ namespace shadow_robot 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"; @@ -103,6 +109,7 @@ 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 >( @@ -111,6 +118,11 @@ this->msg_array_tom_r.data.resize(data_sz); 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; @@ -222,6 +234,10 @@ this->msg_array_tom_r.data.resize(data_sz); 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() @@ -749,6 +765,7 @@ this->msg_array_tom_r.data.resize(data_sz); 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)