virtual_arm.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_hand/hand/virtual_arm.h"
00027 
00028 #include <time.h>
00029 #include <ros/ros.h>
00030 
00031 #ifdef GAZEBO
00032 #include <std_msgs/Float64.h>
00033 #include <std_srvs/Empty.h>
00034 
00035 #include <gazebo_msgs/SetModelConfiguration.h>
00036 #endif
00037 
00038 namespace shadowrobot
00039 {
00040 VirtualArm::VirtualArm() :
00041     SRArticulatedRobot()
00042 {
00043 #ifdef GAZEBO
00044     ROS_INFO("This ROS interface is built for Gazebo.");
00045     //initialises the subscriber to the Gazebo joint_states messages
00046     std::string prefix;
00047     std::string searched_param;
00048     n_tilde = ros::NodeHandle("~");
00049 
00050     n_tilde.searchParam("gazebo_joint_states_prefix", searched_param);
00051     n_tilde.param(searched_param, prefix, std::string());
00052     std::string full_topic = prefix + "joint_states";
00053     gazebo_subscriber = node.subscribe(full_topic, 2, &VirtualArm::gazeboCallback, this);
00054 #else
00055     ROS_INFO("This ROS interface is not built for Gazebo.");
00056 #endif
00057 
00058     srand(time(NULL));
00059     initializeMap();
00060 }
00061 
00062 VirtualArm::~VirtualArm()
00063 {
00064 }
00065 
00066 void VirtualArm::initializeMap()
00067 {
00068     joints_map_mutex.lock();
00069     JointData tmpData;
00070 
00071 #ifdef GAZEBO
00072     std::string topic_prefix = "/";
00073     std::string topic_suffix = "/command";
00074     std::string full_topic = "";
00075 #endif
00076 
00077     tmpData.min = -45.0;
00078     tmpData.max = 90.0;
00079 #ifdef GAZEBO
00080     full_topic = topic_prefix + "sa_sr_position_controller" + topic_suffix;
00081     gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00082     int tmp_index = 0;
00083     tmpData.publisher_index = tmp_index;
00084 #endif
00085     joints_map["ShoulderJRotate"] = tmpData;
00086     tmpData.min = 0.0;
00087     tmpData.max = 90.0;
00088 #ifdef GAZEBO
00089     full_topic = topic_prefix + "sa_ss_position_controller" + topic_suffix;
00090     gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00091     tmp_index ++;
00092     tmpData.publisher_index = tmp_index;
00093 #endif
00094     joints_map["ShoulderJSwing"] = tmpData;
00095     tmpData.min = 0.0;
00096     tmpData.max = 120.0;
00097 #ifdef GAZEBO
00098     full_topic = topic_prefix + "sa_es_position_controller" + topic_suffix;
00099     gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00100     tmp_index ++;
00101     tmpData.publisher_index = tmp_index;
00102 #endif
00103     joints_map["ElbowJSwing"] = tmpData;
00104     tmpData.min = -90.0;
00105     tmpData.max = 90.0;
00106 #ifdef GAZEBO
00107     full_topic = topic_prefix + "sa_er_position_controller" + topic_suffix;
00108     gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00109     tmp_index ++;
00110     tmpData.publisher_index = tmp_index;
00111 #endif
00112     joints_map["ElbowJRotate"] = tmpData;
00113 
00114     tmpData.min = 0.0;
00115     tmpData.max = 0.0;
00116     joints_map["arm_link"] = tmpData;
00117 
00118     joints_map_mutex.unlock();
00119 
00120 
00121 #ifdef GAZEBO
00122     //if we're using Gazebo, we want to start with the elbow bent to 120
00123     //first we stop the physics
00124     ros::ServiceClient gazebo_phys_client = node.serviceClient<std_srvs::Empty>("/gazebo/pause_physics");
00125     std_srvs::Empty empty_srv;
00126     gazebo_phys_client.waitForExistence();
00127     gazebo_phys_client.call(empty_srv);
00128 
00129     //then we set the ElbowJSwing in the model pose (the model is called arm_and_hand)
00130     ros::ServiceClient set_pos_client = node.serviceClient<gazebo_msgs::SetModelConfiguration>("/gazebo/set_model_configuration");
00131     gazebo_msgs::SetModelConfiguration model_srv;
00132     model_srv.request.model_name = "shadow_model";
00133     model_srv.request.urdf_param_name = "robot_description";
00134     model_srv.request.joint_names.push_back("ElbowJSwing");
00135     model_srv.request.joint_positions.push_back(2.0);
00136 
00137     set_pos_client.waitForExistence();
00138     set_pos_client.call(model_srv);
00139 
00140     //sends the correct target to the controller
00141     for (int i = 0; i < 500; ++i)
00142     {
00143       sendupdate("ElbowJSwing", 120.0);
00144       sleep(.01);
00145     }
00146 
00147     //and now we restart the physics
00148     gazebo_phys_client = node.serviceClient<std_srvs::Empty>("/gazebo/unpause_physics");
00149     gazebo_phys_client.waitForExistence();
00150     gazebo_phys_client.call(empty_srv);
00151 #endif
00152 }
00153 
00154 short VirtualArm::sendupdate( std::string joint_name, double target )
00155 {
00156     joints_map_mutex.lock();
00157 
00158     JointsMap::iterator iter = joints_map.find(joint_name);
00159 
00160 #ifdef GAZEBO
00161     std_msgs::Float64 target_msg;
00162 #endif
00163 
00164     //not found
00165     if( iter == joints_map.end() )
00166     {
00167         ROS_DEBUG("Joint %s not found.", joint_name.c_str());
00168         joints_map_mutex.unlock();
00169         return -1;
00170     }
00171 
00172     //joint found
00173     JointData tmpData(iter->second);
00174     if( target < tmpData.min )
00175         target = tmpData.min;
00176     if( target > tmpData.max )
00177         target = tmpData.max;
00178 
00179 #ifdef GAZEBO
00180     //gazebo targets are in radians
00181     target_msg.data = toRad(target);
00182     gazebo_publishers[tmpData.publisher_index].publish(target_msg);
00183 #else
00184     tmpData.position = target;
00185 #endif
00186     tmpData.target = target;
00187 
00188     joints_map[joint_name] = tmpData;
00189 
00190     joints_map_mutex.unlock();
00191     return 0;
00192 }
00193 
00194 JointData VirtualArm::getJointData( std::string joint_name )
00195 {
00196     joints_map_mutex.lock();
00197     JointsMap::iterator iter = joints_map.find(joint_name);
00198 
00199     //joint found
00200     if( iter != joints_map.end() )
00201     {
00202         //return the position
00203         iter->second.temperature = ((double)(rand() % 100) / 100.0);
00204         iter->second.current = ((double)(rand() % 100) / 100.0);
00205 #ifndef GAZEBO
00206         iter->second.force = ((double)(rand() % 100) / 100.0);
00207 #endif
00208 
00209         JointData tmpData = JointData(iter->second);
00210         joints_map_mutex.unlock();
00211         return tmpData;
00212     }
00213 
00214     ROS_ERROR("Joint %s not found.", joint_name.c_str());
00215     JointData noData;
00216     joints_map_mutex.unlock();
00217     return noData;
00218 }
00219 
00220 SRArticulatedRobot::JointsMap VirtualArm::getAllJointsData()
00221 {
00222     joints_map_mutex.lock();
00223     JointsMap tmpMap;
00224 
00225     for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00226     {
00227         JointData tmpData = it->second;
00228         tmpData.temperature = ((double)(rand() % 100) / 100.0);
00229         tmpData.current = ((double)(rand() % 100) / 100.0);
00230 #ifndef GAZEBO
00231         tmpData.force = ((double)(rand() % 100) / 100.0);
00232 #endif
00233         tmpData.jointIndex = 0;
00234         tmpData.flags = "";
00235 
00236         joints_map[it->first] = tmpData;
00237     }
00238 
00239     tmpMap = JointsMap(joints_map);
00240     joints_map_mutex.unlock();
00241     return tmpMap;
00242 }
00243 
00244 short VirtualArm::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00245 {
00246     ROS_WARN("The setContrl method is not yet implemented");
00247     return 0;
00248 }
00249 
00250 JointControllerData VirtualArm::getContrl( std::string contrlr_name )
00251 {
00252     ROS_WARN("The getContrl method is not yet implemented");
00253     JointControllerData no_result;
00254     return no_result;
00255 }
00256 
00257 short VirtualArm::setConfig( std::vector<std::string> myConfig )
00258 {
00259     ROS_WARN("The set config function is not implemented in the virtual arm.");
00260     return 0;
00261 }
00262 
00263 void VirtualArm::getConfig( std::string joint_name )
00264 {
00265     ROS_WARN("The get config function is not implemented in the virtual arm.");
00266 }
00267 
00268 std::vector<DiagnosticData> VirtualArm::getDiagnostics()
00269 {
00270     joints_map_mutex.lock();
00271     std::vector<DiagnosticData> returnVect;
00272 
00273     for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00274     {
00275         DiagnosticData tmpDiag;
00276         tmpDiag.joint_name = it->first;
00277         tmpDiag.level = 0;
00278         tmpDiag.flags = "";
00279         tmpDiag.target_sensor_num = 0;
00280         tmpDiag.position_sensor_num = 0;
00281         tmpDiag.target = it->second.target;
00282         tmpDiag.position = it-> second.position;
00283 
00284         returnVect.push_back(tmpDiag);
00285     }
00286 
00287     joints_map_mutex.unlock();
00288     return returnVect;
00289 }
00290 
00291 #ifdef GAZEBO
00292 void VirtualArm::gazeboCallback(const sensor_msgs::JointStateConstPtr& msg)
00293 {
00294     joints_map_mutex.lock();
00295     //loop on all the names in the joint_states message
00296     for(unsigned int index = 0; index < msg->name.size(); ++index)
00297     {
00298         std::string joint_name = msg->name[index];
00299         JointsMap::iterator iter = joints_map.find(joint_name);
00300 
00301         //not found => can be a joint from the arm / hand
00302         if(iter == joints_map.end())
00303         continue;
00304 
00305         //joint found
00306         JointData tmpData(iter->second);
00307 
00308         tmpData.position = toDegrees(msg->position[index]);
00309         tmpData.force = msg->effort[index];
00310 
00311         joints_map[joint_name] = tmpData;
00312     }
00313     joints_map_mutex.unlock();
00314 }
00315 #endif
00316 } //end namespace


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Fri Jan 3 2014 12:03:25