29 #include <boost/algorithm/string.hpp> 39 std::string touch_name) :
43 if (name.find(
"th") != std::string::npos)
46 std::string tmp = boost::to_upper_copy(name);
49 tmp = boost::to_upper_copy(name);
56 std::string tmp = boost::to_upper_copy(name);
59 tmp = boost::to_upper_copy(name);
73 double tmp_value = 0.0;
74 int msg_length = msg->joints_list_length;
75 for (
size_t index_msg = 0; index_msg < msg_length; ++index_msg)
78 std::string sensor_name = msg->joints_list[index_msg].joint_name;
85 tmp_value += msg->joints_list[index_msg].joint_position;
112 std::vector<std::vector<std::string> > all_names =
get_all_names();
114 for (
unsigned int i = 0; i < all_names[0].size(); ++i)
136 int main(
int argc,
char **argv)
138 ros::init(argc, argv,
"sr_tactile_sensor");
~SrVirtualTactileSensor()
void callback(const sr_robot_msgs::joints_dataConstPtr &msg)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std::vector< boost::shared_ptr< SrGenericTactileSensor > > tactile_sensors
SrVirtualTactileSensor(std::string name, std::string touch_name)
virtual double get_touch_data()
~SrVirtualTactileSensorManager()
SrVirtualTactileSensorManager()
int main(int argc, char **argv)
std::vector< std::vector< std::string > > get_all_names()
std::vector< std::string > names_joints_linked