$search
00001 #include <ros/ros.h> 00002 #include <visualization_msgs/Marker.h> 00003 //#include <geometry_msgs/Point.h> 00004 00005 #include <string> 00006 00007 #include <boost/thread.hpp> 00008 00009 //messages 00010 #include <std_msgs/Float64.h> 00011 00012 //a ros subscriber (will be instantiated later on) 00013 ros::Subscriber sub[5]; 00014 ros::Publisher marker_pub; 00015 std_msgs::Float64::_data_type data[5]; 00016 boost::mutex update_mutex; 00017 float reduction_factor=10; 00018 // Set our initial shape type to be a cube 00019 uint32_t shape = visualization_msgs::Marker::ARROW; 00020 00021 00022 void publish_marker(unsigned int id, std::string framesuffix, float force) 00023 { 00024 if(force>0.01) 00025 { 00026 visualization_msgs::Marker marker; 00027 // Set the frame ID and timestamp. See the TF tutorials for information on these. 00028 marker.header.frame_id = "/srh/position/"+framesuffix; 00029 marker.header.stamp = ros::Time::now(); 00030 00031 // Set the namespace and id for this marker. This serves to create a unique ID 00032 // Any marker sent with the same namespace and id will overwrite the old one 00033 marker.ns = "touch"; 00034 marker.id = id; 00035 00036 // Set the marker type. Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER 00037 marker.type = shape; 00038 00039 // Set the marker action. Options are ADD and DELETE 00040 marker.action = visualization_msgs::Marker::ADD; 00041 geometry_msgs::Point mypoint; 00042 // Set the pose of the marker. 00043 // Set start point 00044 float phalanx_thickness=0.007; 00045 float phalanx_length=0.01; 00046 mypoint.x= 0; 00047 mypoint.y= phalanx_thickness; 00048 mypoint.z= phalanx_length; 00049 marker.points.push_back(mypoint); 00050 00051 // Set end point 00052 mypoint.x= 0; 00053 mypoint.y= (phalanx_thickness+force/20.0); 00054 mypoint.z= phalanx_length; 00055 marker.points.push_back(mypoint); 00056 00057 // Set the scale of the marker -- 1x1x1 here means 1m on a side 00058 marker.scale.x = 0.0025; 00059 marker.scale.y = 0.004; 00060 marker.scale.z = 0; 00061 00062 // Set the color -- be sure to set alpha to something non-zero! 00063 marker.color.r = (force)>0.0?(force):0.0; 00064 marker.color.g = (1.0f-force)>0.0?(1.0f-force):0.0; 00065 marker.color.b = 0.0f; 00066 marker.color.a = 1.0; 00067 00068 marker.lifetime = ros::Duration(); 00069 00070 // Publish the marker 00071 marker_pub.publish(marker); 00072 } 00073 } 00074 00075 00076 void callback_ff(const std_msgs::Float64ConstPtr& msg) 00077 { 00078 update_mutex.lock(); 00079 data[0] = msg->data; 00080 publish_marker(0,"ffdistal", msg->data/reduction_factor); 00081 update_mutex.unlock(); 00082 } 00083 void callback_mf(const std_msgs::Float64ConstPtr& msg) 00084 { 00085 update_mutex.lock(); 00086 data[1] = msg->data; 00087 publish_marker(1,"mfdistal", msg->data/reduction_factor); 00088 update_mutex.unlock(); 00089 } 00090 void callback_rf(const std_msgs::Float64ConstPtr& msg) 00091 { 00092 update_mutex.lock(); 00093 data[2] = msg->data; 00094 publish_marker(2,"rfdistal", msg->data/reduction_factor); 00095 update_mutex.unlock(); 00096 } 00097 void callback_lf(const std_msgs::Float64ConstPtr& msg) 00098 { 00099 update_mutex.lock(); 00100 data[3] = msg->data; 00101 publish_marker(3,"lfdistal", msg->data/reduction_factor); 00102 update_mutex.unlock(); 00103 } 00104 void callback_th(const std_msgs::Float64ConstPtr& msg) 00105 { 00106 update_mutex.lock(); 00107 data[4] = msg->data; 00108 publish_marker(4,"thdistal", msg->data/reduction_factor); 00109 update_mutex.unlock(); 00110 } 00111 00112 int main( int argc, char** argv ) 00113 { 00114 ros::init(argc, argv, "sr_tactile_rviz_marker"); 00115 ros::NodeHandle n; 00116 ros::Rate r(10); 00117 marker_pub = n.advertise<visualization_msgs::Marker>("sr_tactile_markers", 1); 00118 00119 sub[0] = n.subscribe("/sr_tactile/touch/ff", 2, callback_ff); 00120 sub[1] = n.subscribe("/sr_tactile/touch/mf", 2, callback_mf); 00121 sub[2] = n.subscribe("/sr_tactile/touch/rf", 2, callback_rf); 00122 sub[3] = n.subscribe("/sr_tactile/touch/lf", 2, callback_lf); 00123 sub[4] = n.subscribe("/sr_tactile/touch/th", 2, callback_th); 00124 00125 std_msgs::Float64::_data_type cur_data[5] = {0}; 00126 00127 while (ros::ok()) 00128 { 00129 update_mutex.lock(); 00130 for (int i=0; i<5; i++) 00131 cur_data[i] = data[i]; 00132 update_mutex.unlock(); 00133 00134 00135 /*ROS_ERROR("TACTILE SENSOR READING: %f %f %f %f %f", 00136 cur_data[0], 00137 cur_data[1], 00138 cur_data[2], 00139 cur_data[3], 00140 cur_data[4]);*/ 00141 r.sleep(); 00142 ros::spinOnce(); 00143 } 00144 } 00145 00146 00147 /* For the emacs weenies in the crowd. 00148 Local Variables: 00149 c-basic-offset: 2 00150 End: 00151 */