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 #endif
00034
00035 namespace shadowrobot
00036 {
00037 VirtualArm::VirtualArm() :
00038 SRArticulatedRobot()
00039 {
00040 #ifdef GAZEBO
00041 ROS_INFO("This ROS interface is built for Gazebo.");
00042
00043 std::string prefix;
00044 std::string searched_param;
00045 n_tilde = ros::NodeHandle("~");
00046
00047 n_tilde.searchParam("gazebo_joint_states_prefix", searched_param);
00048 n_tilde.param(searched_param, prefix, std::string());
00049 std::string full_topic = prefix + "joint_states";
00050 gazebo_subscriber = node.subscribe(full_topic, 2, &VirtualArm::gazeboCallback, this);
00051 #else
00052 ROS_INFO("This ROS interface is not built for Gazebo.");
00053 #endif
00054
00055 srand(time(NULL));
00056 initializeMap();
00057 }
00058
00059 VirtualArm::~VirtualArm()
00060 {
00061 }
00062
00063 void VirtualArm::initializeMap()
00064 {
00065 joints_map_mutex.lock();
00066 JointData tmpData;
00067
00068 #ifdef GAZEBO
00069 std::string topic_prefix = "/";
00070 std::string topic_suffix = "/command";
00071 std::string full_topic = "";
00072 #endif
00073
00074 tmpData.min = -45.0;
00075 tmpData.max = 90.0;
00076 #ifdef GAZEBO
00077 full_topic = topic_prefix + "trunk_rotation_controller" + topic_suffix;
00078 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00079 int tmp_index = 0;
00080 tmpData.publisher_index = tmp_index;
00081 #endif
00082 joints_map["ShoulderJRotate"] = tmpData;
00083 tmpData.min = 0.0;
00084 tmpData.max = 90.0;
00085 #ifdef GAZEBO
00086 full_topic = topic_prefix + "shoulder_rotation_controller" + topic_suffix;
00087 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00088 tmp_index ++;
00089 tmpData.publisher_index = tmp_index;
00090 #endif
00091 joints_map["ShoulderJSwing"] = tmpData;
00092 tmpData.min = 0.0;
00093 tmpData.max = 120.0;
00094 #ifdef GAZEBO
00095 full_topic = topic_prefix + "elbow_abduction_controller" + topic_suffix;
00096 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00097 tmp_index ++;
00098 tmpData.publisher_index = tmp_index;
00099 #endif
00100 joints_map["ElbowJSwing"] = tmpData;
00101 tmpData.min = -90.0;
00102 tmpData.max = 90.0;
00103 #ifdef GAZEBO
00104 full_topic = topic_prefix + "forearm_rotation_controller" + topic_suffix;
00105 gazebo_publishers.push_back(node.advertise<std_msgs::Float64>(full_topic, 2));
00106 tmp_index ++;
00107 tmpData.publisher_index = tmp_index;
00108 #endif
00109 joints_map["ElbowJRotate"] = tmpData;
00110
00111 joints_map_mutex.unlock();
00112 }
00113
00114 short VirtualArm::sendupdate( std::string joint_name, double target )
00115 {
00116 joints_map_mutex.lock();
00117
00118 JointsMap::iterator iter = joints_map.find(joint_name);
00119
00120 #ifdef GAZEBO
00121 std_msgs::Float64 target_msg;
00122 #endif
00123
00124
00125 if( iter == joints_map.end() )
00126 {
00127 ROS_DEBUG("Joint %s not found.", joint_name.c_str());
00128 joints_map_mutex.unlock();
00129 return -1;
00130 }
00131
00132
00133 JointData tmpData(iter->second);
00134 if( target < tmpData.min )
00135 target = tmpData.min;
00136 if( target > tmpData.max )
00137 target = tmpData.max;
00138
00139 #ifdef GAZEBO
00140
00141 target_msg.data = toRad(target);
00142 gazebo_publishers[tmpData.publisher_index].publish(target_msg);
00143 ros::spinOnce();
00144 #else
00145 tmpData.position = target;
00146 #endif
00147 tmpData.target = target;
00148
00149 joints_map[joint_name] = tmpData;
00150
00151 joints_map_mutex.unlock();
00152 return 0;
00153 }
00154
00155 JointData VirtualArm::getJointData( std::string joint_name )
00156 {
00157 joints_map_mutex.lock();
00158 JointsMap::iterator iter = joints_map.find(joint_name);
00159
00160
00161 if( iter != joints_map.end() )
00162 {
00163
00164 iter->second.temperature = ((double)(rand() % 100) / 100.0);
00165 iter->second.current = ((double)(rand() % 100) / 100.0);
00166 iter->second.force = ((double)(rand() % 100) / 100.0);
00167
00168 JointData tmpData = JointData(iter->second);
00169 joints_map_mutex.unlock();
00170 return tmpData;
00171 }
00172
00173 ROS_ERROR("Joint %s not found.", joint_name.c_str());
00174 JointData noData;
00175 joints_map_mutex.unlock();
00176 return noData;
00177 }
00178
00179 SRArticulatedRobot::JointsMap VirtualArm::getAllJointsData()
00180 {
00181 joints_map_mutex.lock();
00182 JointsMap tmpMap;
00183
00184 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00185 {
00186 JointData tmpData = it->second;
00187 tmpData.temperature = ((double)(rand() % 100) / 100.0);
00188 tmpData.current = ((double)(rand() % 100) / 100.0);
00189 tmpData.force = ((double)(rand() % 100) / 100.0);
00190 tmpData.jointIndex = 0;
00191 tmpData.flags = "";
00192
00193 joints_map[it->first] = tmpData;
00194 }
00195
00196 tmpMap = JointsMap(joints_map);
00197 joints_map_mutex.unlock();
00198 return tmpMap;
00199 }
00200
00201 short VirtualArm::setContrl( std::string contrlr_name, JointControllerData ctrlr_data )
00202 {
00203 ROS_WARN("The setContrl method is not yet implemented");
00204 return 0;
00205 }
00206
00207 JointControllerData VirtualArm::getContrl( std::string contrlr_name )
00208 {
00209 ROS_WARN("The getContrl method is not yet implemented");
00210 JointControllerData no_result;
00211 return no_result;
00212 }
00213
00214 short VirtualArm::setConfig( std::vector<std::string> myConfig )
00215 {
00216 ROS_WARN("The set config function is not implemented in the virtual arm.");
00217 return 0;
00218 }
00219
00220 void VirtualArm::getConfig( std::string joint_name )
00221 {
00222 ROS_WARN("The get config function is not implemented in the virtual arm.");
00223 }
00224
00225 std::vector<DiagnosticData> VirtualArm::getDiagnostics()
00226 {
00227 joints_map_mutex.lock();
00228 std::vector<DiagnosticData> returnVect;
00229
00230 for( JointsMap::const_iterator it = joints_map.begin(); it != joints_map.end(); ++it )
00231 {
00232 DiagnosticData tmpDiag;
00233 tmpDiag.joint_name = it->first;
00234 tmpDiag.level = 0;
00235 tmpDiag.flags = "";
00236 tmpDiag.target_sensor_num = 0;
00237 tmpDiag.target = it->second.target;
00238 tmpDiag.position_sensor_num = 0;
00239 tmpDiag.position = it-> second.position;
00240
00241 returnVect.push_back(tmpDiag);
00242 }
00243
00244 joints_map_mutex.unlock();
00245 return returnVect;
00246 }
00247
00248 #ifdef GAZEBO
00249 void VirtualArm::gazeboCallback(const sensor_msgs::JointStateConstPtr& msg)
00250 {
00251 joints_map_mutex.lock();
00252
00253 for(unsigned int index = 0; index < msg->name.size(); ++index)
00254 {
00255 std::string joint_name = msg->name[index];
00256 JointsMap::iterator iter = joints_map.find(joint_name);
00257
00258
00259 if(iter == joints_map.end())
00260 continue;
00261
00262
00263 JointData tmpData(iter->second);
00264
00265 tmpData.position = toDegrees(msg->position[index]);
00266 tmpData.force = msg->effort[index];
00267
00268 joints_map[joint_name] = tmpData;
00269 }
00270 joints_map_mutex.unlock();
00271 }
00272 #endif
00273 }