void GazeboMechanismControl::LoadChild(XMLConfigNode *node)
{
// read pr2.xml (pr2_gazebo_mechanism_control.xml)
// setup actuators, then setup mechanism control node
ReadPr2Xml(node);
// Initializes the fake state (for running the transmissions backwards).
fake_state_ = new mechanism::RobotState(&mc_.model_, &hw_);
// Get gazebo joint properties such as explicit damping coefficient, simulation specific.
// Currently constructs a map of joint-name/damping-value pairs.
ReadGazeboPhysics(node);
// The gazebo joints and mechanism joints should match up.
for (unsigned int i = 0; i < mc_.model_.joints_.size(); ++i)
{
std::string joint_name = mc_.model_.joints_[i]->name_;
// fill in gazebo joints pointer
gazebo::Joint *joint = parent_model_->GetJoint(joint_name);
if (joint)
{
joints_.push_back(joint);
}
else
{
fprintf(stderr, "WARNING (gazebo_mechanism_control): A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
joints_.push_back(NULL);
}
// fill in gazebo joints / damping value pairs
std::map<std::string,double>::iterator jt = joints_damping_map_.find(joint_name);
if (jt!=joints_damping_map_.end())
{
joints_damping_.push_back(jt->second);
//std::cout << "adding gazebo joint name (" << joint_name << ") with damping=" << jt->second << std::endl;
}
else
{
joints_damping_.push_back(0); // no damping
//std::cout << "adding gazebo joint name (" << joint_name << ") with no damping." << std::endl;
}
}
hw_.current_time_ = Simulator::Instance()->GetSimTime();
}
void GazeboRosControllerManager::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
{
// Get then name of the parent model
std::string modelName = _sdf->GetParent()->GetValueString("name");
// Get the world name.
this->world = _parent->GetWorld();
// Get a pointer to the model
this->parent_model_ = _parent;
// Error message if the model couldn't be found
if (!this->parent_model_)
gzerr << "Unable to get parent model\n";
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&GazeboRosControllerManager::UpdateChild, this));
gzdbg << "plugin model name: " << modelName << "\n";
physics_ = false;
this->world->EnablePhysicsEngine(physics_); //TODO should be false
if (getenv("CHECK_SPEEDUP"))
{
wall_start_ = this->world->GetRealTime().Double();
sim_start_ = this->world->GetSimTime().Double();
}
// check update rate against world physics update rate
// should be equal or higher to guarantee the wrench applied is not "diluted"
//if (this->updatePeriod > 0 &&
// (this->world->GetPhysicsEngine()->GetUpdateRate() > 1.0/this->updatePeriod))
// ROS_ERROR("gazebo_ros_force controller update rate is less than physics update rate, wrench applied will be diluted (applied intermittently)");
// get parameter name
this->robotNamespace = "";
if (_sdf->HasElement("robotNamespace"))
this->robotNamespace = _sdf->GetElement("robotNamespace")->GetValueString();
this->robotParam = "robot_description";
if (_sdf->HasElement("robotParam"))
this->robotParam = _sdf->GetElement("robotParam")->GetValueString();
this->robotParam = this->robotNamespace+"/" + this->robotParam;
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
this->rosnode_ = new ros::NodeHandle(this->robotNamespace);
ROS_INFO("starting gazebo_ros_controller_manager plugin in ns: %s",this->robotNamespace.c_str());
// pr2_etherCAT calls ros::spin(), we'll thread out one spinner here to mimic that
this->ros_spinner_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerROSThread,this ) );
// load a controller manager
this->cm_ = new pr2_controller_manager::ControllerManager(&hw_,*this->rosnode_);
this->hw_.current_time_ = ros::Time(this->world->GetSimTime().Double());
if (this->hw_.current_time_ < ros::Time(0.001)) this->hw_.current_time_ == ros::Time(0.001); // hardcoded to minimum of 1ms on start up
this->rosnode_->param("gazebo/start_robot_calibrated",this->fake_calibration_,true);
// read pr2 urdf
// setup actuators, then setup mechanism control node
ReadPr2Xml();
// Initializes the fake state (for running the transmissions backwards).
this->fake_state_ = new pr2_mechanism_model::RobotState(&this->cm_->model_);
// The gazebo joints and mechanism joints should match up.
if (this->cm_->state_ != NULL) // could be NULL if ReadPr2Xml is unsuccessful
{
for (unsigned int i = 0; i < this->cm_->state_->joint_states_.size(); ++i)
{
std::string joint_name = this->cm_->state_->joint_states_[i].joint_->name;
// fill in gazebo joints pointer
gazebo::physics::JointPtr joint = this->parent_model_->GetJoint(joint_name);
if (joint)
{
this->joints_.push_back(joint);
}
else
{
//ROS_WARN("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
//this->joints_.push_back(NULL); // FIXME: cannot be null, must be an empty boost shared pointer
this->joints_.push_back(gazebo::physics::JointPtr()); // FIXME: cannot be null, must be an empty boost shared pointer
ROS_ERROR("A joint named \"%s\" is not part of Mechanism Controlled joints.\n", joint_name.c_str());
}
}
}
#ifdef USE_CBQ
// start custom queue for controller manager
this->controller_manager_callback_queue_thread_ = boost::thread( boost::bind( &GazeboRosControllerManager::ControllerManagerQueueThread,this ) );
#endif
}