28 #include <boost/algorithm/string.hpp> 40 std::string gazebo_bumper_topic) :
43 touch_freshdata(false)
55 ::geometry_msgs::Vector3 v;
60 ROS_DEBUG(
"Touch by %s", msg.header.frame_id.c_str());
65 if (msg.states.size() > 0)
67 size_t nb_wrench = msg.states[0].wrenches.size();
68 for (
unsigned int i = 0; i < nb_wrench; i++)
70 v.x = v.x + msg.states[0].wrenches[i].force.x;
71 v.y = v.y + msg.states[0].wrenches[i].force.y;
72 v.z = v.z + msg.states[0].wrenches[i].force.z;
74 v.x = v.x / nb_wrench;
75 v.y = v.y / nb_wrench;
76 v.z = v.z / nb_wrench;
77 tmp_value = sqrt(pow(v.x, 2) + pow(v.y, 2) + pow(v.z, 2));
110 std::vector<std::vector<std::string> > all_names =
get_all_names();
112 for (
unsigned int i = 0; i < all_names[0].size(); ++i)
117 "contacts/" + all_names[0][i] +
"/distal")));
135 int main(
int argc,
char **argv)
137 ros::init(argc, argv,
"sr_tactile_sensor");
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
virtual double get_touch_data()
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
SrGazeboVirtualTactileSensor(std::string name, std::string gazebo_topic)
int main(int argc, char **argv)
virtual ~SrGazeboVirtualTactileSensor()
std::vector< boost::shared_ptr< SrGenericTactileSensor > > tactile_sensors
SrGazeboVirtualTactileSensorManager()
~SrGazeboVirtualTactileSensorManager()
void callback(const gazebo_msgs::ContactsState &msg)
std::vector< std::vector< std::string > > get_all_names()