00001 #include "GazeboInterface.h"
00002 #include "physics/Model.hh"
00003 #include "physics/Joint.hh"
00004 #include "physics/World.hh"
00005
00006 #include "r2_msgs/JointStatusArray.h"
00007
00008 using namespace gazebo;
00009
00010 GazeboInterface::GazeboInterface()
00011 : ModelPlugin()
00012 , advancedMode(false)
00013 {
00014 }
00015
00016 GazeboInterface::~GazeboInterface()
00017 {
00018
00019 rosNodePtr->shutdown();
00020 }
00021
00022 void GazeboInterface::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00023 {
00024
00025 modelPtr = _model;
00026
00027 prevStatesUpdateTime = _model->GetWorld()->GetSimTime();
00028 prevStatusUpdateTime = _model->GetWorld()->GetSimTime();
00029
00030
00031 robotControllerPtr.reset(new RobotController(modelPtr));
00032
00033
00034 std::string robotNamespace = "";
00035 if (_sdf->HasElement("robotNamespace"))
00036 {
00037 robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00038 }
00039
00040 std::string paramsNamespace = "";
00041 if (_sdf->HasElement("paramsNamespace"))
00042 {
00043 paramsNamespace = _sdf->GetElement("paramsNamespace")->GetValueString() + "/";
00044 }
00045
00046 if (!_sdf->HasElement("jointCommandsTopic"))
00047 {
00048 ROS_FATAL("GazeboInterface plugin missing <jointCommandsTopic>, cannot proceed");
00049 return;
00050 }
00051 else
00052 jointCommandsTopic = _sdf->GetElement("jointCommandsTopic")->GetValueString();
00053
00054 if (!_sdf->HasElement("jointStatesTopic"))
00055 {
00056 ROS_FATAL("GazeboInterface plugin missing <jointStatesTopic>, cannot proceed");
00057 return;
00058 }
00059 else
00060 jointStatesTopic = _sdf->GetElement("jointStatesTopic")->GetValueString();
00061
00062 if (!_sdf->HasElement("jointStatesRate"))
00063 {
00064 jointStatesStepTime = 0.;
00065 }
00066 else
00067 jointStatesStepTime = 1./_sdf->GetElement("jointStatesRate")->GetValueDouble();
00068
00069 if (!_sdf->HasElement("advancedMode"))
00070 {
00071 ROS_INFO("GazeboInterface plugin missing <advancedMode>, defaults to false");
00072 advancedMode = false;
00073 }
00074 else
00075 {
00076 std::string strVal = _sdf->GetElement("advancedMode")->GetValueString();
00077 if (strVal == "1")
00078 advancedMode = true;
00079 else if (strVal == "0")
00080 advancedMode = false;
00081 else
00082 {
00083 ROS_WARN("GazeboInterface plugin <advancedMode> should be type bool (true/false), defaults to false");
00084 advancedMode = false;
00085 }
00086 }
00087
00088 ROS_INFO("GazeboInterface plugin advancedMode: %s", advancedMode ? "true" : "false");
00089
00090 if (advancedMode)
00091 {
00092 if (!_sdf->HasElement("jointControlTopic"))
00093 {
00094 ROS_FATAL("GazeboInterface plugin missing <jointControlTopic>, cannot proceed");
00095 return;
00096 }
00097 else
00098 jointControlTopic = _sdf->GetElement("jointControlTopic")->GetValueString();
00099
00100 if (!_sdf->HasElement("jointStatusTopic"))
00101 {
00102 ROS_FATAL("GazeboInterface plugin missing <jointStatusTopic>, cannot proceed");
00103 return;
00104 }
00105 else
00106 jointStatusTopic = _sdf->GetElement("jointStatusTopic")->GetValueString();
00107
00108 if (!_sdf->HasElement("jointStatusRate"))
00109 {
00110 jointStatusStepTime = 0.;
00111 }
00112 else
00113 jointStatusStepTime = 1./_sdf->GetElement("jointStatusRate")->GetValueDouble();
00114 }
00115
00116
00117 rosNodePtr.reset(new ros::NodeHandle(robotNamespace));
00118 paramsNodePtr.reset(new ros::NodeHandle(paramsNamespace));
00119
00120
00121
00122 updateConnectionPtr = event::Events::ConnectWorldUpdateStart(boost::bind(&GazeboInterface::update, this));
00123
00124 ROS_INFO("Gazebo Interface plugin loaded");
00125 }
00126
00127 void GazeboInterface::traverseParams(XmlRpc::XmlRpcValue param, std::map<std::string, XmlRpc::XmlRpcValue>& valMap, std::string searchKey,
00128 std::string ns, std::string name)
00129 {
00130 std::string fullName;
00131 if (name.empty())
00132 {
00133 fullName = ns;
00134 }
00135 else
00136 {
00137 fullName = ns + "/" + name;
00138 }
00139
00140 if (param.getType() == XmlRpc::XmlRpcValue::TypeStruct)
00141 {
00142
00143 for (XmlRpc::XmlRpcValue::iterator paramIt = param.begin(); paramIt != param.end(); ++paramIt)
00144 {
00145 traverseParams(paramIt->second, valMap, searchKey, fullName, paramIt->first);
00146 }
00147 }
00148 else if (searchKey == "")
00149 {
00150
00151 valMap.insert(std::make_pair(fullName, param));
00152 }
00153 else if (searchKey == name)
00154 {
00155
00156 valMap.insert(std::make_pair(ns, param));
00157 }
00158
00159
00160 }
00161
00162 bool GazeboInterface::getDoubleVal(XmlRpc::XmlRpcValue& val, double& doubleVal)
00163 {
00164 if (val.getType() == XmlRpc::XmlRpcValue::TypeInt)
00165 {
00166 doubleVal = (double)(int)val;
00167 }
00168 else if (val.getType() == XmlRpc::XmlRpcValue::TypeDouble)
00169 {
00170 doubleVal = (double)val;
00171 }
00172 else
00173 {
00174 return false;
00175 }
00176
00177 return true;
00178 }
00179
00180 void GazeboInterface::Init()
00181 {
00182 ROS_DEBUG("add joints");
00183
00184 for (unsigned int i = 0; i < modelPtr->GetJointCount(); ++i)
00185 {
00186 physics::JointPtr jPtr = modelPtr->GetJoint(i);
00187 robotControllerPtr->addJoint(jPtr, advancedMode);
00188 ROS_DEBUG("Add %s to controller", jPtr->GetName().c_str());
00189 }
00190
00191 XmlRpc::XmlRpcValue param;
00192
00193 if (paramsNodePtr->getParam("dependency", param))
00194 {
00195 ROS_DEBUG("set dependencies");
00196
00197 std::map<std::string, XmlRpc::XmlRpcValue> dependentChildrenMap;
00198 traverseParams(param, dependentChildrenMap, "children");
00199
00200 std::map<std::string, XmlRpc::XmlRpcValue> dependentFactorsMap;
00201 traverseParams(param, dependentFactorsMap, "factors");
00202
00203
00204 for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator depIt = dependentChildrenMap.begin();
00205 depIt != dependentChildrenMap.end(); ++depIt)
00206 {
00207 XmlRpc::XmlRpcValue factorVal = dependentFactorsMap[depIt->first];
00208
00209 if (depIt->second.getType() == XmlRpc::XmlRpcValue::TypeString)
00210 {
00211 double val;
00212 if (getDoubleVal(factorVal, val))
00213 {
00214
00215 ROS_DEBUG("Add joint dependency: %s, %s, %f", ((std::string)depIt->second).c_str(), depIt->first.c_str(), (double)factorVal);
00216 robotControllerPtr->addJointDependency((std::string)depIt->second, depIt->first, (double)factorVal);
00217 }
00218 else
00219 {
00220 ROS_WARN("Add dependency for %s (child) to %s (parent) not completed because factor not an appropriate type (double or int)",
00221 ((std::string)depIt->second).c_str(), depIt->first.c_str());
00222 }
00223 }
00224 else if (depIt->second.getType() == XmlRpc::XmlRpcValue::TypeArray)
00225 {
00226 if (factorVal.getType() == XmlRpc::XmlRpcValue::TypeArray && factorVal.size() == depIt->second.size())
00227 {
00228 for (int i = 0; i < depIt->second.size(); ++i)
00229 {
00230 if (depIt->second[i].getType() == XmlRpc::XmlRpcValue::TypeString)
00231 {
00232 double val;
00233 if (getDoubleVal(factorVal[i], val))
00234 {
00235
00236 ROS_DEBUG("Add joint dependency: %s, %s, %f", ((std::string)depIt->second[i]).c_str(), depIt->first.c_str(), val);
00237 robotControllerPtr->addJointDependency((std::string)depIt->second[i], depIt->first, val);
00238 }
00239 else
00240 {
00241 ROS_WARN("Add dependency for %s (child) to %s (parent) not completed because factor not an appropriate type (double or int)",
00242 ((std::string)depIt->second[i]).c_str(), depIt->first.c_str());
00243 }
00244 }
00245 else
00246 {
00247 ROS_WARN("A dependency to %s (parent) not added because type is invalid", depIt->first.c_str());
00248 }
00249 }
00250 }
00251 else
00252 {
00253 ROS_WARN("Add dependency to %s (parent) not completed because children/factors array size not the same",
00254 depIt->first.c_str());
00255 }
00256 }
00257 }
00258 }
00259
00260
00261 if (paramsNodePtr->getParam("initial_position", param))
00262 {
00263 ROS_DEBUG("set initial positions");
00264 bool radians = true;
00265 if (param.hasMember("radians") && param["radians"].getType() == XmlRpc::XmlRpcValue::TypeBoolean)
00266 {
00267 radians = (bool)param["radians"];
00268 ROS_DEBUG("radians: %d", radians);
00269 }
00270 else
00271 {
00272 ROS_WARN("No 'radians' member provided (or invalid type) in 'initial_position'; assumed true");
00273 }
00274
00275 std::map<std::string, XmlRpc::XmlRpcValue> initialPoseMapParams;
00276 traverseParams(param, initialPoseMapParams);
00277
00278 initialPoseMapParams.erase("/radians");
00279
00280
00281 std::map<std::string, double> initialPoseMap;
00282 for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator poseIt = initialPoseMapParams.begin();
00283 poseIt != initialPoseMapParams.end(); ++poseIt)
00284 {
00285 double val;
00286 if (getDoubleVal(poseIt->second, val))
00287 {
00288 if (radians == false)
00289 {
00290 val = GZ_DTOR(val);
00291 }
00292
00293 if (modelPtr->GetJoint(poseIt->first))
00294 {
00295 initialPoseMap.insert(std::make_pair(poseIt->first, val));
00296 ROS_DEBUG("initial position of %s set to %f", poseIt->first.c_str(), val);
00297 }
00298 else
00299 {
00300 ROS_INFO("initial position of %s ignored because model doesn't contain joint", poseIt->first.c_str());
00301 }
00302 }
00303 else
00304 {
00305 ROS_WARN("Sim initial position value (%s) ignored because it is not a valid type (double or int)", poseIt->first.c_str());
00306 }
00307 }
00308
00309
00310 robotControllerPtr->setJointPositions(initialPoseMap);
00311 }
00312
00313
00314 if (paramsNodePtr->getParam("position_pid", param))
00315 {
00316 ROS_DEBUG("set position PIDs");
00317 std::map<std::string, XmlRpc::XmlRpcValue> positionPidMap;
00318 traverseParams(param, positionPidMap);
00319
00320
00321 for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator pidIt = positionPidMap.begin();
00322 pidIt != positionPidMap.end(); ++pidIt)
00323 {
00324 if (pidIt->second.getType() == XmlRpc::XmlRpcValue::TypeArray && pidIt->second.size() >= 3)
00325 {
00326 std::vector<double> gains(7, 0.0);
00327 bool valid = true;
00328 for (int i = 0; i < pidIt->second.size(); ++ i)
00329 {
00330 valid &= getDoubleVal(pidIt->second[i], gains[i]);
00331 }
00332
00333 if (valid)
00334 {
00335 ROS_DEBUG("setPosPid: %s, %f, %f, %f, %f, %f, %f, %f", pidIt->first.c_str(), gains[0], gains[1], gains[2], gains[3], gains[4], gains[5], gains[6]);
00336 robotControllerPtr->setPosPid(pidIt->first, gains[0], gains[1], gains[2], gains[3], gains[4], gains[5], gains[6]);
00337 }
00338 else
00339 {
00340 ROS_WARN("position PID for %s not set because the values are not valid (double or int)", pidIt->first.c_str());
00341 }
00342 }
00343 else
00344 {
00345 ROS_WARN("position PID values for %s not set because it is not an array of size 3 or greater", pidIt->first.c_str());
00346 }
00347 }
00348 }
00349
00350
00351 if (paramsNodePtr->getParam("velocity_pid", param))
00352 {
00353 ROS_DEBUG("set velocity PIDs");
00354 std::map<std::string, XmlRpc::XmlRpcValue> velocityPidMap;
00355 traverseParams(param, velocityPidMap);
00356
00357
00358 for (std::map<std::string, XmlRpc::XmlRpcValue>::iterator pidIt = velocityPidMap.begin();
00359 pidIt != velocityPidMap.end(); ++pidIt)
00360 {
00361 if (pidIt->second.getType() == XmlRpc::XmlRpcValue::TypeArray && pidIt->second.size() >= 3)
00362 {
00363 std::vector<double> gains(7, 0.0);
00364 bool valid = true;
00365 for (int i = 0; i < pidIt->second.size(); ++ i)
00366 {
00367 valid &= getDoubleVal(pidIt->second[i], gains[i]);
00368 }
00369
00370 if (valid)
00371 {
00372 ROS_DEBUG("setVelPid: %s, %f, %f, %f, %f, %f, %f, %f", pidIt->first.c_str(), gains[0], gains[1], gains[2], gains[3], gains[4], gains[5], gains[6]);
00373 robotControllerPtr->setVelPid(pidIt->first, gains[0], gains[1], gains[2], gains[3], gains[4], gains[5], gains[6]);
00374 }
00375 else
00376 {
00377 ROS_WARN("velocity PID for %s not set because the values are not valid (double or int)", pidIt->first.c_str());
00378 }
00379 }
00380 else
00381 {
00382 ROS_WARN("velocity PID values for %s not set because it is not an array of size 3 or greater", pidIt->first.c_str());
00383 }
00384 }
00385 }
00386
00387
00388 if (!advancedMode)
00389 {
00390 for (unsigned int i = 0; i < modelPtr->GetJointCount(); ++i)
00391 {
00392 physics::JointPtr jPtr = modelPtr->GetJoint(i);
00393 robotControllerPtr->setJointPosTarget(jPtr->GetName(), jPtr->GetAngle(0).GetAsRadian());
00394 }
00395 }
00396
00397
00398 jointCommandsSub = rosNodePtr->subscribe(jointCommandsTopic, 5, &GazeboInterface::commandJoints, this);
00399
00400
00401 jointStatePub = rosNodePtr->advertise<sensor_msgs::JointState>(jointStatesTopic, true);
00402
00403 if (advancedMode)
00404 {
00405
00406 jointControlSub = rosNodePtr->subscribe(jointControlTopic, 5, &GazeboInterface::controlJoints, this);
00407
00408
00409 jointStatusPub = rosNodePtr->advertise<r2_msgs::JointStatusArray>(jointStatusTopic, true);
00410 }
00411
00412 ROS_INFO("Gazebo Interface plugin initialized");
00413 }
00414
00415 void GazeboInterface::update()
00416 {
00417 common::Time currTime = modelPtr->GetWorld()->GetSimTime();
00418
00419
00420 robotControllerPtr->update();
00421
00422
00423 if ((currTime - prevStatesUpdateTime).Double() >= jointStatesStepTime)
00424 {
00425 prevStatesUpdateTime = currTime;
00426 sensor_msgs::JointStatePtr msgPtr(new sensor_msgs::JointState);
00427 msgPtr->header.stamp = ros::Time::now();
00428
00429 for (unsigned int i = 0; i < modelPtr->GetJointCount(); ++i)
00430 {
00431 physics::JointPtr jPtr = modelPtr->GetJoint(i);
00432
00433
00434 std::string name = jPtr->GetName();
00435 msgPtr->name.push_back(name);
00436 msgPtr->position.push_back(jPtr->GetAngle(0).GetAsRadian());
00437 msgPtr->velocity.push_back(jPtr->GetVelocity(0));
00438 msgPtr->effort.push_back(jPtr->GetForce(0));
00439
00440
00441 std::string::size_type index = name.find("/joint");
00442 if (index != std::string::npos)
00443 {
00444 msgPtr->name.push_back(name.replace(index, 6, "/motor"));
00445 msgPtr->position.push_back(jPtr->GetAngle(0).GetAsRadian());
00446 msgPtr->velocity.push_back(jPtr->GetVelocity(0));
00447 msgPtr->effort.push_back(jPtr->GetForce(0));
00448
00449
00450 msgPtr->name.push_back(name.replace(index, 6, "/encoder"));
00451 msgPtr->position.push_back(jPtr->GetAngle(0).GetAsRadian());
00452 msgPtr->velocity.push_back(jPtr->GetVelocity(0));
00453 msgPtr->effort.push_back(jPtr->GetForce(0));
00454 }
00455 }
00456 jointStatePub.publish(msgPtr);
00457 }
00458
00459
00460 if (advancedMode && (currTime - prevStatusUpdateTime).Double() >= jointStatusStepTime)
00461 {
00462 prevStatusUpdateTime = currTime;
00463 robotControllerPtr->publishJointStatuses(jointStatusPub);
00464 }
00465 }
00466
00467 void GazeboInterface::commandJoints(const sensor_msgs::JointState::ConstPtr& msg)
00468 {
00469 bool setPos = msg->position.size() >= msg->name.size();
00470 bool setVel = msg->velocity.size() >= msg->name.size();
00471 bool setEffort = msg->effort.size() >= msg->name.size();
00472
00473 ROS_DEBUG("GazeboInterface received joint command");
00474 for (unsigned int i = 0; i < msg->name.size(); ++i)
00475 {
00476 if (setPos)
00477 {
00478 robotControllerPtr->setJointPosTarget(msg->name[i], msg->position[i]);
00479 }
00480 if (setVel)
00481 {
00482 robotControllerPtr->setJointVelTarget(msg->name[i], msg->velocity[i]);
00483 }
00484 if (setEffort)
00485 {
00486 robotControllerPtr->setJointEffortTarget(msg->name[i], msg->effort[i]);
00487 }
00488 }
00489 }
00490
00491
00492 void GazeboInterface::controlJoints(const r2_msgs::JointControl::ConstPtr& msg)
00493 {
00494 ROS_DEBUG("GazeboInterface received joint control");
00495 robotControllerPtr->setJointControl(msg);
00496 }
00497
00498 GZ_REGISTER_MODEL_PLUGIN(GazeboInterface)