sr_tactile_rviz_marker.cpp
Go to the documentation of this file.
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 */


sr_tactile_sensors
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:09:36