real_arm.cpp
Go to the documentation of this file.
00001 
00026 #include "sr_hand/hand/real_arm.h"
00027 //our robot code
00028 #include <robot/robot.h>
00029 #include <robot/hand.h>
00030 #include <robot/hand_protocol.h>
00031 
00032 #include <time.h>
00033 #include <ros/ros.h>
00034 
00035 namespace shadowrobot
00036 {
00037 RealArm::RealArm() :
00038   SRArticulatedRobot()
00039 {
00040 
00041   /* We need to attach the program to the robot, or fail if we cannot. */
00042   if (robot_init() < 0)
00043   {
00044     ROS_FATAL("Robot interface broken\n");
00045     ROS_BREAK();
00046   }
00047 
00048   /* We need to attach the program to the hand as well, or fail if we cannot. */
00049   if (hand_init() < 0)
00050   {
00051     ROS_FATAL("Arm interface broken\n");
00052     ROS_BREAK();
00053   }
00054 
00055   srand(time(NULL));
00056   initializeMap();
00057 }
00058 
00059 RealArm::~RealArm()
00060 {
00061 }
00062 
00063 void RealArm::initializeMap()
00064 {
00065   joints_map_mutex.lock();
00066   JointData tmpData;
00067 
00068   tmpData.position = 0.0;
00069   tmpData.target = 0.0;
00070   tmpData.temperature = 0.0;
00071   tmpData.current = 0.0;
00072   tmpData.force = 0.0;
00073   tmpData.flags = "";
00074 
00075   for (unsigned int i = START_OF_ARM; i < NUM_HAND_JOINTS; ++i)
00076   {
00077     std::string name = hand_joints[i].joint_name;
00078     tmpData.jointIndex = i;
00079 
00080     joints_map[name] = tmpData;
00081 
00082     ROS_INFO("NAME[%d]: %s ", i, name.c_str());
00083   }
00084 
00085   joints_map_mutex.unlock();
00086 }
00087 
00088 short RealArm::sendupdate(std::string joint_name, double target)
00089 {
00090   joints_map_mutex.lock();
00091 
00092   //search the sensor in the hash_map
00093   JointsMap::iterator iter = joints_map.find(joint_name);
00094 
00095   if (iter != joints_map.end())
00096   {
00097     JointData tmpData = joints_map.find(joint_name)->second;
00098     int index_arm_joints = tmpData.jointIndex;
00099 
00100     //trim to the correct range
00101     if (target < hand_joints[index_arm_joints].min_angle)
00102       target = hand_joints[index_arm_joints].min_angle;
00103     if (target > hand_joints[index_arm_joints].max_angle)
00104       target = hand_joints[index_arm_joints].max_angle;
00105 
00106     //here we update the actual hand angles
00107     robot_update_sensor(&hand_joints[index_arm_joints].joint_target, target);
00108     joints_map_mutex.unlock();
00109     return 0;
00110   }
00111 
00112   ROS_DEBUG("Joint %s not found", joint_name.c_str());
00113 
00114   joints_map_mutex.unlock();
00115   return -1;
00116 }
00117 
00118 JointData RealArm::getJointData(std::string joint_name)
00119 {
00120   joints_map_mutex.lock();
00121   JointsMap::iterator iter = joints_map.find(joint_name);
00122 
00123   //joint not found
00124   if (iter == joints_map.end())
00125   {
00126     ROS_ERROR("Joint %s not found.", joint_name.c_str());
00127     JointData noData;
00128     noData.position = 0.0;
00129     noData.target = 0.0;
00130     noData.temperature = 0.0;
00131     noData.current = 0.0;
00132     noData.force = 0.0;
00133     noData.flags = "";
00134     noData.jointIndex = 0;
00135 
00136     joints_map_mutex.unlock();
00137     return noData;
00138   }
00139 
00140   //joint found
00141   JointData tmpData = iter->second;
00142   int index = tmpData.jointIndex;
00143 
00144   tmpData.position = robot_read_sensor(&hand_joints[index].position);
00145   tmpData.target = robot_read_sensor(&hand_joints[index].joint_target);
00146 
00147   joints_map[joint_name] = tmpData;
00148 
00149   joints_map_mutex.unlock();
00150   return tmpData;
00151 }
00152 
00153 SRArticulatedRobot::JointsMap RealArm::getAllJointsData()
00154 {
00155   //update the map for each joints
00156   for (JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
00157     getJointData(it->first);
00158 
00159   JointsMap tmp = JointsMap(joints_map);
00160 
00161   //return the map
00162   return tmp;
00163 }
00164 
00165 short RealArm::setContrl(std::string contrlr_name, JointControllerData ctrlr_data)
00166 {
00167   ROS_WARN("The setContrl method is not yet implemented");
00168   return 0;
00169 }
00170 
00171 JointControllerData RealArm::getContrl(std::string contrlr_name)
00172 {
00173   ROS_WARN("The getContrl method is not yet implemented");
00174   JointControllerData no_result;
00175   return no_result;
00176 }
00177 
00178 short RealArm::setConfig(std::vector<std::string> myConfig)
00179 {
00180   ROS_WARN("The set config function is not implemented in the virtual arm.");
00181   return 0;
00182 }
00183 
00184 void RealArm::getConfig(std::string joint_name)
00185 {
00186   ROS_WARN("The get config function is not implemented in the virtual arm.");
00187 }
00188 
00189 std::vector<DiagnosticData> RealArm::getDiagnostics()
00190 {
00191   joints_map_mutex.lock();
00192   std::vector<DiagnosticData> returnVect;
00193 
00194   //TODO: read diagnostic data from the robot
00195 
00196   for (JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it)
00197   {
00198     DiagnosticData tmpDiag;
00199     tmpDiag.joint_name = it->first;
00200     tmpDiag.level = 0;
00201     tmpDiag.flags = "";
00202     tmpDiag.target_sensor_num = 0;
00203     tmpDiag.target = it->second.target;
00204     tmpDiag.position_sensor_num = 0;
00205     tmpDiag.position = it-> second.position;
00206 
00207     returnVect.push_back(tmpDiag);
00208   }
00209 
00210   joints_map_mutex.unlock();
00211   return returnVect;
00212 }
00213 
00214 } //end namespace


sr_hand
Author(s): Ugo Cupcic / ugo@shadowrobot.com, contact@shadowrobot.com
autogenerated on Mon Oct 6 2014 07:44:02