00001
00026 #include "sr_hand/hand/real_arm.h"
00027
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
00042 if (robot_init() < 0)
00043 {
00044 ROS_FATAL("Robot interface broken\n");
00045 ROS_BREAK();
00046 }
00047
00048
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
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
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
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
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
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
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
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
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 }