$search
00001 /* 00002 * Gazebo - Outdoor Multi-Robot Simulator 00003 * Copyright (C) 2003 00004 * Nate Koenig & Andrew Howard 00005 * 00006 * This program is free software; you can redistribute it and/or modify 00007 * it under the terms of the GNU General Public License as published by 00008 * the Free Software Foundation; either version 2 of the License, or 00009 * (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 /* 00022 * Desc: 3D position interface for ground truth. 00023 * Author: Sachin Chitta and John Hsu 00024 * Date: 1 June 2008 00025 * SVN info: $Id$ 00026 */ 00027 00028 #include <gazebo_plugins/gazebo_ros_p3d.h> 00029 00030 #include <gazebo/Global.hh> 00031 #include <gazebo/XMLConfig.hh> 00032 #include <gazebo/HingeJoint.hh> 00033 #include <gazebo/SliderJoint.hh> 00034 #include <gazebo/Simulator.hh> 00035 #include <gazebo/gazebo.h> 00036 #include <gazebo/GazeboError.hh> 00037 #include <gazebo/ControllerFactory.hh> 00038 #include <gazebo/World.hh> 00039 00040 using namespace gazebo; 00041 00042 GZ_REGISTER_DYNAMIC_CONTROLLER("gazebo_ros_p3d", GazeboRosP3D); 00043 00044 00046 // Constructor 00047 GazeboRosP3D::GazeboRosP3D(Entity *parent ) 00048 : Controller(parent) 00049 { 00050 this->myParent = dynamic_cast<Model*>(this->parent); 00051 00052 if (!this->myParent) 00053 gzthrow("GazeboRosP3D controller requires a Model as its parent"); 00054 00055 Param::Begin(&this->parameters); 00056 this->robotNamespaceP = new ParamT<std::string>("robotNamespace", "/", 0); 00057 this->bodyNameP = new ParamT<std::string>("bodyName", "", 0); 00058 this->topicNameP = new ParamT<std::string>("topicName", "", 1); 00059 this->frameNameP = new ParamT<std::string>("frameName", "world", 0); 00060 this->xyzOffsetsP = new ParamT<Vector3>("xyzOffsets", Vector3(0,0,0),0); 00061 this->rpyOffsetsP = new ParamT<Vector3>("rpyOffsets", Vector3(0,0,0),0); 00062 this->gaussianNoiseP = new ParamT<double>("gaussianNoise",0.0,0); 00063 Param::End(); 00064 00065 this->p3dConnectCount = 0; 00066 } 00067 00069 // Destructor 00070 GazeboRosP3D::~GazeboRosP3D() 00071 { 00072 delete this->robotNamespaceP; 00073 delete this->bodyNameP; 00074 delete this->topicNameP; 00075 delete this->frameNameP; 00076 delete this->xyzOffsetsP; 00077 delete this->rpyOffsetsP; 00078 delete this->gaussianNoiseP; 00079 delete this->rosnode_; 00080 } 00081 00083 // Load the controller 00084 void GazeboRosP3D::LoadChild(XMLConfigNode *node) 00085 { 00086 this->robotNamespaceP->Load(node); 00087 this->robotNamespace = this->robotNamespaceP->GetValue(); 00088 if (!ros::isInitialized()) 00089 { 00090 int argc = 0; 00091 char** argv = NULL; 00092 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName); 00093 } 00094 00095 this->rosnode_ = new ros::NodeHandle(this->robotNamespace); 00096 00097 this->bodyNameP->Load(node); 00098 this->bodyName = this->bodyNameP->GetValue(); 00099 00100 // assert that the body by bodyName exists 00101 this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(this->bodyName)); 00102 if (this->myBody == NULL) 00103 { 00104 ROS_WARN("gazebo_ros_p3d plugin error: bodyName: %s does not exist\n",this->bodyName.c_str()); 00105 return; 00106 } 00107 00108 this->topicNameP->Load(node); 00109 this->topicName = this->topicNameP->GetValue(); 00110 00111 this->frameNameP->Load(node); 00112 this->frameName = this->frameNameP->GetValue(); 00113 00114 this->xyzOffsetsP->Load(node); 00115 this->xyzOffsets = this->xyzOffsetsP->GetValue(); 00116 this->rpyOffsetsP->Load(node); 00117 this->rpyOffsets = this->rpyOffsetsP->GetValue(); 00118 this->gaussianNoiseP->Load(node); 00119 this->gaussianNoise = this->gaussianNoiseP->GetValue(); 00120 00121 if (this->topicName != "") 00122 { 00123 #ifdef USE_CBQ 00124 ros::AdvertiseOptions p3d_ao = ros::AdvertiseOptions::create<nav_msgs::Odometry>( 00125 this->topicName,1, 00126 boost::bind( &GazeboRosP3D::P3DConnect,this), 00127 boost::bind( &GazeboRosP3D::P3DDisconnect,this), ros::VoidPtr(), &this->p3d_queue_); 00128 this->pub_ = this->rosnode_->advertise(p3d_ao); 00129 #else 00130 this->pub_ = this->rosnode_->advertise<nav_msgs::Odometry>(this->topicName,1, 00131 boost::bind( &GazeboRosP3D::P3DConnect, this), 00132 boost::bind( &GazeboRosP3D::P3DDisconnect, this)); 00133 #endif 00134 } 00135 } 00136 00138 // Increment count 00139 void GazeboRosP3D::P3DConnect() 00140 { 00141 this->p3dConnectCount++; 00142 } 00144 // Decrement count 00145 void GazeboRosP3D::P3DDisconnect() 00146 { 00147 this->p3dConnectCount--; 00148 } 00149 00151 // Initialize the controller 00152 void GazeboRosP3D::InitChild() 00153 { 00154 if (this->myBody == NULL) 00155 return; 00156 00157 this->last_time = Simulator::Instance()->GetSimTime(); 00158 // initialize body 00159 this->last_vpos = this->myBody->GetWorldLinearVel(); // get velocity in gazebo frame 00160 this->last_veul = this->myBody->GetWorldAngularVel(); // get velocity in gazebo frame 00161 this->apos = 0; 00162 this->aeul = 0; 00163 00164 // preset myFrame to NULL, will search for the body with matching name in UpdateChild() 00165 // since most bodies are constructed on the fly 00166 this->myFrame = NULL; 00167 this->frame_apos = 0; 00168 this->frame_aeul = 0; 00169 #ifdef USE_CBQ 00170 // start custom queue for p3d 00171 this->callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosP3D::P3DQueueThread,this ) ); 00172 #endif 00173 } 00174 00176 // Update the controller 00177 void GazeboRosP3D::UpdateChild() 00178 { 00179 if (this->myBody == NULL) 00180 return; 00181 00182 if (this->p3dConnectCount > 0) 00183 { 00185 if (this->myFrame == NULL && this->frameName != "world" && this->frameName != "/map" && this->frameName != "map") 00186 { 00187 // lock in case a model is being spawned 00188 boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMRMutex()); 00189 // look through all models in the world, search for body name that matches frameName 00190 std::vector<Model*> all_models = World::Instance()->GetModels(); 00191 for (std::vector<Model*>::iterator iter = all_models.begin(); iter != all_models.end(); iter++) 00192 { 00193 //ROS_ERROR("checking model %s for link %s",(*iter)->GetName().c_str(),this->frameName.c_str()); 00194 if (*iter) this->myFrame = dynamic_cast<Body*>((*iter)->GetBody(this->frameName)); 00195 if (this->myFrame) break; 00196 } 00197 00198 // not found 00199 if (this->myFrame == NULL) 00200 { 00201 ROS_DEBUG("gazebo_ros_p3d plugin: frameName: %s does not exist yet, will not publish\n",this->frameName.c_str()); 00202 return; 00203 } 00204 else 00205 { 00206 //ROS_ERROR("got body %s",this->myFrame->GetName().c_str()); 00207 // found! initialize frame 00208 this->last_frame_vpos = this->myFrame->GetWorldLinearVel(); // get velocity in gazebo frame 00209 this->last_frame_veul = this->myFrame->GetWorldAngularVel(); // get velocity in gazebo frame 00210 this->initial_frame_pose = this->myFrame->GetWorldPose(); // get velocity in gazebo frame 00211 } 00212 } 00213 00214 Pose3d pose, frame_pose; 00215 Quatern rot, frame_rot; 00216 Vector3 pos, frame_pos; 00217 00218 // Get Pose/Orientation ///@todo: verify correctness 00219 pose = this->myBody->GetWorldPose(); // - this->myBody->GetCoMPose(); 00220 // apply xyz offsets and get position and rotation components 00221 pos = pose.pos + this->xyzOffsets; 00222 rot = pose.rot; 00223 // std::cout << " --------- GazeboRosP3D rot " << rot.x << ", " << rot.y << ", " << rot.z << ", " << rot.u << std::endl; 00224 00225 if (this->myFrame) 00226 { 00227 frame_pose = this->myFrame->GetWorldPose(); // - this->myBody->GetCoMPose(); 00228 frame_pos = frame_pose.pos; 00229 frame_rot = frame_pose.rot; 00230 } 00231 00232 // apply rpy offsets 00233 Quatern qOffsets; 00234 qOffsets.SetFromEuler(this->rpyOffsets); 00235 rot = qOffsets*rot; 00236 rot.Normalize(); 00237 00238 gazebo::Time cur_time = Simulator::Instance()->GetSimTime(); 00239 00240 // get inertial Rates 00241 Vector3 vpos = this->myBody->GetWorldLinearVel(); // get velocity in gazebo frame 00242 Vector3 veul = this->myBody->GetWorldAngularVel(); // get velocity in gazebo frame 00243 Vector3 frame_vpos; 00244 Vector3 frame_veul; 00245 if (this->myFrame) 00246 { 00247 frame_vpos = this->myFrame->GetWorldLinearVel(); // get velocity in gazebo frame 00248 frame_veul = this->myFrame->GetWorldAngularVel(); // get velocity in gazebo frame 00249 } 00250 00251 // differentiate to get accelerations 00252 double tmp_dt = cur_time.Double() - this->last_time.Double(); 00253 if (tmp_dt != 0) 00254 { 00255 this->apos = (this->last_vpos - vpos) / tmp_dt; 00256 this->aeul = (this->last_veul - veul) / tmp_dt; 00257 this->last_vpos = vpos; 00258 this->last_veul = veul; 00259 00260 this->frame_apos = (this->last_frame_vpos - frame_vpos) / tmp_dt; 00261 this->frame_aeul = (this->last_frame_veul - frame_veul) / tmp_dt; 00262 this->last_frame_vpos = frame_vpos; 00263 this->last_frame_veul = frame_veul; 00264 00265 this->lock.lock(); 00266 00267 if (this->topicName != "") 00268 { 00269 // copy data into pose message 00270 this->poseMsg.header.frame_id = this->frameName; // @todo: should this be changeable? 00271 this->poseMsg.header.stamp.sec = cur_time.sec; 00272 this->poseMsg.header.stamp.nsec = cur_time.nsec; 00273 00274 // pose is given in inertial frame for Gazebo, transform to the designated frame name 00275 // get relative pose in specified frame 00276 // @todo: account for offsets 00277 if (this->myFrame) 00278 { 00279 // get the relative transform from myFrame to myBody 00280 pos = (pos - frame_pos); // linear offset from myFrame to myBody 00281 rot *= frame_rot.GetInverse(); // rotation between myFrame and myBody 00282 } 00283 this->poseMsg.pose.pose.position.x = pos.x; 00284 this->poseMsg.pose.pose.position.y = pos.y; 00285 this->poseMsg.pose.pose.position.z = pos.z; 00286 00287 this->poseMsg.pose.pose.orientation.x = rot.x; 00288 this->poseMsg.pose.pose.orientation.y = rot.y; 00289 this->poseMsg.pose.pose.orientation.z = rot.z; 00290 this->poseMsg.pose.pose.orientation.w = rot.u; 00291 00292 // get relative rates then rotate into specified frame 00293 // @todo: account for offsets 00294 if (this->myFrame) 00295 { 00296 vpos = frame_rot.RotateVector(vpos - frame_vpos); 00297 veul = frame_rot.RotateVector(veul - frame_veul); 00298 } 00299 this->poseMsg.twist.twist.linear.x = vpos.x + this->GaussianKernel(0,this->gaussianNoise) ; 00300 this->poseMsg.twist.twist.linear.y = vpos.y + this->GaussianKernel(0,this->gaussianNoise) ; 00301 this->poseMsg.twist.twist.linear.z = vpos.z + this->GaussianKernel(0,this->gaussianNoise) ; 00302 // pass euler angular rates 00303 this->poseMsg.twist.twist.angular.x = veul.x + this->GaussianKernel(0,this->gaussianNoise) ; 00304 this->poseMsg.twist.twist.angular.y = veul.y + this->GaussianKernel(0,this->gaussianNoise) ; 00305 this->poseMsg.twist.twist.angular.z = veul.z + this->GaussianKernel(0,this->gaussianNoise) ; 00306 00307 // fill in covariance matrix 00309 this->poseMsg.pose.covariance[0] = this->gaussianNoise*this->gaussianNoise; 00310 this->poseMsg.pose.covariance[7] = this->gaussianNoise*this->gaussianNoise; 00311 this->poseMsg.pose.covariance[14] = this->gaussianNoise*this->gaussianNoise; 00312 this->poseMsg.pose.covariance[21] = this->gaussianNoise*this->gaussianNoise; 00313 this->poseMsg.pose.covariance[28] = this->gaussianNoise*this->gaussianNoise; 00314 this->poseMsg.pose.covariance[35] = this->gaussianNoise*this->gaussianNoise; 00315 00316 this->poseMsg.twist.covariance[0] = this->gaussianNoise*this->gaussianNoise; 00317 this->poseMsg.twist.covariance[7] = this->gaussianNoise*this->gaussianNoise; 00318 this->poseMsg.twist.covariance[14] = this->gaussianNoise*this->gaussianNoise; 00319 this->poseMsg.twist.covariance[21] = this->gaussianNoise*this->gaussianNoise; 00320 this->poseMsg.twist.covariance[28] = this->gaussianNoise*this->gaussianNoise; 00321 this->poseMsg.twist.covariance[35] = this->gaussianNoise*this->gaussianNoise; 00322 00323 // publish to ros 00324 this->pub_.publish(this->poseMsg); 00325 } 00326 00327 this->lock.unlock(); 00328 00329 // save last time stamp 00330 this->last_time = cur_time; 00331 } 00332 } 00333 } 00334 00336 // Finalize the controller 00337 void GazeboRosP3D::FiniChild() 00338 { 00339 if (this->myBody == NULL) 00340 return; 00341 this->rosnode_->shutdown(); 00342 #ifdef USE_CBQ 00343 this->p3d_queue_.clear(); 00344 this->p3d_queue_.disable(); 00345 this->callback_queue_thread_.join(); 00346 #endif 00347 } 00348 00350 // Utility for adding noise 00351 double GazeboRosP3D::GaussianKernel(double mu,double sigma) 00352 { 00353 // using Box-Muller transform to generate two independent standard normally disbributed normal variables 00354 // see wikipedia 00355 double U = (double)rand()/(double)RAND_MAX; // normalized uniform random variable 00356 double V = (double)rand()/(double)RAND_MAX; // normalized uniform random variable 00357 double X = sqrt(-2.0 * ::log(U)) * cos( 2.0*M_PI * V); 00358 //double Y = sqrt(-2.0 * ::log(U)) * sin( 2.0*M_PI * V); // the other indep. normal variable 00359 // we'll just use X 00360 // scale to our mu and sigma 00361 X = sigma * X + mu; 00362 return X; 00363 } 00364 00365 #ifdef USE_CBQ 00366 00367 // Put laser data to the interface 00368 void GazeboRosP3D::P3DQueueThread() 00369 { 00370 static const double timeout = 0.01; 00371 00372 while (this->rosnode_->ok()) 00373 { 00374 this->p3d_queue_.callAvailable(ros::WallDuration(timeout)); 00375 } 00376 } 00377 #endif