Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <visualization_msgs/Marker.h>
00003
00004
00005 #include <string>
00006
00007 #include <boost/thread.hpp>
00008
00009
00010 #include <std_msgs/Float64.h>
00011
00012
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
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
00028 marker.header.frame_id = "/srh/position/"+framesuffix;
00029 marker.header.stamp = ros::Time::now();
00030
00031
00032
00033 marker.ns = "touch";
00034 marker.id = id;
00035
00036
00037 marker.type = shape;
00038
00039
00040 marker.action = visualization_msgs::Marker::ADD;
00041 geometry_msgs::Point mypoint;
00042
00043
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
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
00058 marker.scale.x = 0.0025;
00059 marker.scale.y = 0.004;
00060 marker.scale.z = 0;
00061
00062
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
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
00136
00137
00138
00139
00140
00141 r.sleep();
00142 ros::spinOnce();
00143 }
00144 }
00145
00146
00147
00148
00149
00150
00151