$search
00001 #include <ros/ros.h> 00002 #include <visualization_msgs/Marker.h> 00003 #include <gazebo_plugins/ContactsState.h> 00004 //#include <geometry_msgs/Point.h> 00005 00006 #include <string> 00007 00008 #include <boost/thread.hpp> 00009 00010 //messages 00011 #include <std_msgs/Float64.h> 00012 00013 #define COUNTDOWN_MAX 5 00014 #define FF 0 00015 #define MF 1 00016 #define RF 2 00017 #define LF 3 00018 #define TH 4 00019 00020 //a ros subscriber (will be instantiated later on) 00021 ros::Subscriber sub[5]; 00022 ros::Publisher marker_pub; 00023 std_msgs::Float64::_data_type data[5]; 00024 boost::mutex update_mutex; 00025 int colors[8][3]={{0,0,0},{1,0,0},{0,1,0},{0,0,1},{1,1,0},{1,0,1},{0,1,1},{1,1,1}}; 00026 // Set our initial shape type to be a cube 00027 uint32_t shape = visualization_msgs::Marker::ARROW; 00028 int countdown[5] = {COUNTDOWN_MAX,COUNTDOWN_MAX,COUNTDOWN_MAX,COUNTDOWN_MAX,COUNTDOWN_MAX} ; 00029 00030 void publish_marker_special(unsigned int id, std::string framesuffix, const::geometry_msgs::Vector3& force, int col) 00031 { 00032 visualization_msgs::Marker marker; 00033 // Set the frame ID and timestamp. See the TF tutorials for information on these. 00034 marker.header.frame_id = "/"+framesuffix; 00035 marker.header.stamp = ros::Time::now(); 00036 00037 // Set the namespace and id for this marker. This serves to create a unique ID 00038 // Any marker sent with the same namespace and id will overwrite the old one 00039 marker.ns = "touch"; 00040 marker.id = id; 00041 00042 // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 00043 marker.type = shape; 00044 00045 // Set the marker action. Options are ADD and DELETE 00046 marker.action = visualization_msgs::Marker::ADD; 00047 geometry_msgs::Point mypoint; 00048 00049 // Set the pose of the marker. 00050 // Set start point 00051 float phalanx_thickness=0.007; 00052 float phalanx_length=0.01; 00053 mypoint.x= 0; 00054 mypoint.y= phalanx_thickness; 00055 mypoint.z= phalanx_length; 00056 marker.points.push_back(mypoint); 00057 00058 // Set end point 00059 mypoint.x= (force.x/20.0); 00060 mypoint.y= (phalanx_thickness+force.y/20.0); 00061 mypoint.z= phalanx_length+force.z/20.0; 00062 marker.points.push_back(mypoint); 00063 00064 // Set the scale of the marker -- 1x1x1 here means 1m on a side 00065 marker.scale.x = 0.0025; 00066 marker.scale.y = 0.004; 00067 marker.scale.z = 0; 00068 00069 // Set the color -- be sure to set alpha to something non-zero! 00070 marker.color.r = colors[col][0]; 00071 marker.color.g = colors[col][1]; 00072 marker.color.b = colors[col][2]; 00073 marker.color.a = 1.0; 00074 00075 marker.lifetime = ros::Duration(.5); 00076 00077 // Publish the marker 00078 marker_pub.publish(marker); 00079 } 00080 void publish_marker(unsigned int id, std::string framesuffix, float force) 00081 { 00082 if(force>0.01) 00083 { 00084 visualization_msgs::Marker marker; 00085 // Set the frame ID and timestamp. See the TF tutorials for information on these. 00086 marker.header.frame_id = "/srh/position/"+framesuffix; 00087 marker.header.stamp = ros::Time::now(); 00088 00089 // Set the namespace and id for this marker. This serves to create a unique ID 00090 // Any marker sent with the same namespace and id will overwrite the old one 00091 marker.ns = "touch"; 00092 marker.id = id; 00093 00094 // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 00095 marker.type = shape; 00096 00097 // Set the marker action. Options are ADD and DELETE 00098 marker.action = visualization_msgs::Marker::ADD; 00099 geometry_msgs::Point mypoint; 00100 // Set the pose of the marker. 00101 // Set start point 00102 float phalanx_thickness=0.007; 00103 float phalanx_length=0.01; 00104 mypoint.x= 0; 00105 mypoint.y= phalanx_thickness; 00106 mypoint.z= phalanx_length; 00107 marker.points.push_back(mypoint); 00108 00109 // Set end point 00110 mypoint.x= 0; 00111 mypoint.y= (phalanx_thickness+force/20.0); 00112 mypoint.z= phalanx_length; 00113 marker.points.push_back(mypoint); 00114 00115 // Set the scale of the marker -- 1x1x1 here means 1m on a side 00116 marker.scale.x = 0.0025; 00117 marker.scale.y = 0.004; 00118 marker.scale.z = 0; 00119 00120 // Set the color -- be sure to set alpha to something non-zero! 00121 marker.color.r = (force)>0.0?(force):0.0; 00122 marker.color.g = (1.0f-force)>0.0?(1.0f-force):0.0; 00123 marker.color.b = 0.0f; 00124 marker.color.a = 1.0; 00125 00126 marker.lifetime = ros::Duration(); 00127 00128 // Publish the marker 00129 marker_pub.publish(marker); 00130 } 00131 } 00132 00133 void callback_ff(const gazebo_plugins::ContactsState& msg) 00134 { 00135 // Publish the marker 00136 // once every countdown 00137 update_mutex.lock(); 00138 countdown[FF]--; 00139 if(countdown[FF]<=0) 00140 { 00141 countdown[FF]=COUNTDOWN_MAX; 00142 for(unsigned int i=0;i<msg.states[0].wrenches.size();i++) 00143 { 00144 const::geometry_msgs::Vector3& v = msg.states[0].wrenches[i].force; 00145 publish_marker_special(FF,"ffdistal", v,i); 00146 } 00147 } 00148 update_mutex.unlock(); 00149 } 00150 00151 void callback_mf(const gazebo_plugins::ContactsState& msg) 00152 { 00153 // Publish the marker 00154 // once every countdown 00155 update_mutex.lock(); 00156 countdown[MF]--; 00157 if(countdown[MF]<=0) 00158 { 00159 countdown[MF]=COUNTDOWN_MAX; 00160 for(unsigned int i=0;i<msg.states[0].wrenches.size();i++) 00161 { 00162 const::geometry_msgs::Vector3& v = msg.states[0].wrenches[i].force; 00163 publish_marker_special(MF,"mfdistal", v,i); 00164 } 00165 } 00166 update_mutex.unlock(); 00167 } 00168 00169 void callback_rf(const gazebo_plugins::ContactsState& msg) 00170 { 00171 // Publish the marker 00172 // once every countdown 00173 update_mutex.lock(); 00174 countdown[RF]--; 00175 if(countdown[RF]<=0) 00176 { 00177 countdown[RF]=COUNTDOWN_MAX; 00178 for(unsigned int i=0;i<msg.states[0].wrenches.size();i++) 00179 { 00180 const::geometry_msgs::Vector3& v = msg.states[0].wrenches[i].force; 00181 publish_marker_special(RF,"rfdistal", v,i); 00182 } 00183 } 00184 update_mutex.unlock(); 00185 } 00186 00187 void callback_lf(const gazebo_plugins::ContactsState& msg) 00188 { 00189 // Publish the marker 00190 // once every countdown 00191 update_mutex.lock(); 00192 countdown[LF]--; 00193 if(countdown[LF]<=0) 00194 { 00195 countdown[LF]=COUNTDOWN_MAX; 00196 for(unsigned int i=0;i<msg.states[0].wrenches.size();i++) 00197 { 00198 const::geometry_msgs::Vector3& v = msg.states[0].wrenches[i].force; 00199 publish_marker_special(LF,"lfdistal", v,i); 00200 } 00201 } 00202 update_mutex.unlock(); 00203 } 00204 00205 void callback_th(const gazebo_plugins::ContactsState& msg) 00206 { 00207 // Publish the marker 00208 // once every countdown 00209 update_mutex.lock(); 00210 countdown[TH]--; 00211 if(countdown[TH]<=0) 00212 { 00213 countdown[TH]=COUNTDOWN_MAX; 00214 for(unsigned int i=0;i<msg.states[0].wrenches.size();i++) 00215 { 00216 const::geometry_msgs::Vector3& v = msg.states[0].wrenches[i].force; 00217 publish_marker_special(TH,"thdistal", v,i); 00218 } 00219 } 00220 update_mutex.unlock(); 00221 } 00222 00223 00224 int main( int argc, char** argv ) 00225 { 00226 ros::init(argc, argv, "bumper_markers"); 00227 ros::NodeHandle n; 00228 ros::Rate r(10); 00229 marker_pub = n.advertise<visualization_msgs::Marker>("bumper_markers", 1); 00230 00231 sub[0] = n.subscribe("/ffdistal_bumper/state", 2, callback_ff); 00232 sub[1] = n.subscribe("/mfdistal_bumper/state", 2, callback_mf); 00233 sub[2] = n.subscribe("/rfdistal_bumper/state", 2, callback_rf); 00234 sub[3] = n.subscribe("/lfdistal_bumper/state", 2, callback_lf); 00235 sub[4] = n.subscribe("/thdistal_bumper/state", 2, callback_th); 00236 00237 std_msgs::Float64::_data_type cur_data[5] = {0}; 00238 00239 while (ros::ok()) 00240 { 00241 update_mutex.lock(); 00242 for (int i=0; i<5; i++) 00243 cur_data[i] = data[i]; 00244 update_mutex.unlock(); 00245 00246 00247 /*ROS_ERROR("TACTILE SENSOR READING: %f %f %f %f %f", 00248 cur_data[0], 00249 cur_data[1], 00250 cur_data[2], 00251 cur_data[3], 00252 cur_data[4]);*/ 00253 r.sleep(); 00254 ros::spinOnce(); 00255 } 00256 } 00257