void GazeboRosCreate::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
this->my_world_ = _parent->GetWorld();
this->my_parent_ = _parent;
if (!this->my_parent_)
{
ROS_FATAL("Gazebo_ROS_Create controller requires a Model as its parent");
return;
}
this->node_namespace_ = "";
if (_sdf->HasElement("node_namespace"))
this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
left_wheel_joint_name_ = "left_wheel_joint";
if (_sdf->HasElement("left_wheel_joint"))
left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();
right_wheel_joint_name_ = "right_wheel_joint";
if (_sdf->HasElement("right_wheel_joint"))
right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();
front_castor_joint_name_ = "front_castor_joint";
if (_sdf->HasElement("front_castor_joint"))
front_castor_joint_name_ = _sdf->GetElement("front_castor_joint")->GetValueString();
rear_castor_joint_name_ = "rear_castor_joint";
if (_sdf->HasElement("rear_castor_joint"))
rear_castor_joint_name_ = _sdf->GetElement("rear_castor_joint")->GetValueString();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_diam_ = 0.15;
if (_sdf->HasElement("wheel_diameter"))
wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();
torque_ = 10.0;
if (_sdf->HasElement("torque"))
torque_ = _sdf->GetElement("torque")->GetValueDouble();
//base_geom_name_ = "base_geom";
base_geom_name_ = "base_footprint_geom_base_link";
if (_sdf->HasElement("base_geom"))
base_geom_name_ = _sdf->GetElement("base_geom")->GetValueString();
base_geom_ = my_parent_->GetChildCollision(base_geom_name_);
if (!base_geom_)
{
// This is a hack for ROS Diamond back. E-turtle and future releases
// will not need this, because it will contain the fixed-joint reduction
// in urdf2gazebo
base_geom_ = my_parent_->GetChildCollision("base_footprint_geom");
if (!base_geom_)
{
ROS_ERROR("Unable to find geom[%s]",base_geom_name_.c_str());
return;
}
}
base_geom_->SetContactsEnabled(true);
contact_event_ = base_geom_->ConnectContact(boost::bind(&GazeboRosCreate::OnContact, this, _1, _2));
// Get then name of the parent model
std::string modelName = _sdf->GetParent()->GetValueString("name");
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&GazeboRosCreate::UpdateChild, this));
gzdbg << "plugin model name: " << modelName << "\n";
/*wall_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("wall_sensor"));
if (!wall_sensor_)
{
ROS_ERROR("Unable to find sensor[wall_sensor]");
return;
}
left_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("left_cliff_sensor"));
right_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("right_cliff_sensor"));
leftfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("leftfront_cliff_sensor"));
rightfront_cliff_sensor_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor("rightfront_cliff_sensor"));*/
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo_myrabot", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle( node_namespace_ );
cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosCreate::OnCmdVel, this );
sensor_state_pub_ = rosnode_->advertise<turtlebot_node::TurtlebotSensorState>("sensor_state", 1);
odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states_roomba", 1);
js_.name.push_back( left_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( right_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( front_castor_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( rear_castor_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
prev_update_time_ = 0;
last_cmd_vel_time_ = 0;
sensor_state_.bumps_wheeldrops = 0x0;
//TODO: fix this
joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
joints_[FRONT] = my_parent_->GetJoint(front_castor_joint_name_);
joints_[REAR] = my_parent_->GetJoint(rear_castor_joint_name_);
if (joints_[LEFT]) set_joints_[LEFT] = true;
if (joints_[RIGHT]) set_joints_[RIGHT] = true;
if (joints_[FRONT]) set_joints_[FRONT] = true;
if (joints_[REAR]) set_joints_[REAR] = true;
//initialize time and odometry position
prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
void GazeboRosEklavya::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
this->my_world_ = _parent->GetWorld();
this->my_parent_ = _parent;
if (!this->my_parent_)
{
ROS_FATAL("Gazebo_ROS_Eklavya controller requires a Model as its parent");
return;
}
this->node_namespace_ = "";
if (_sdf->HasElement("node_namespace"))
this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
left_wheel_joint_name_ = "left_wheel_joint";
if (_sdf->HasElement("left_wheel_joint"))
left_wheel_joint_name_ = _sdf->GetElement("left_wheel_joint")->GetValueString();
right_wheel_joint_name_ = "right_wheel_joint";
if (_sdf->HasElement("right_wheel_joint"))
right_wheel_joint_name_ = _sdf->GetElement("right_wheel_joint")->GetValueString();
front_castor_wheel_joint_name_ = "front_castor_wheel_joint";
if (_sdf->HasElement("front_castor_wheel_joint"))
front_castor_wheel_joint_name_ = _sdf->GetElement("front_castor_wheel_joint")->GetValueString();
front_castor_swivel_joint_name_ = "front_castor_swivel_joint";
if (_sdf->HasElement("front_castor_swivel_joint"))
front_castor_swivel_joint_name_ = _sdf->GetElement("front_castor_swivel_joint")->GetValueString();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_sep_ = 0.34;
if (_sdf->HasElement("wheel_separation"))
wheel_sep_ = _sdf->GetElement("wheel_separation")->GetValueDouble();
wheel_diam_ = 0.15;
if (_sdf->HasElement("wheel_diameter"))
wheel_diam_ = _sdf->GetElement("wheel_diameter")->GetValueDouble();
torque_ = 10.0;
if (_sdf->HasElement("torque"))
torque_ = _sdf->GetElement("torque")->GetValueDouble();
// Get then name of the parent model
std::string modelName = _sdf->GetParent()->GetValueString("name");
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&GazeboRosEklavya::UpdateChild, this));
gzdbg << "plugin model name: " << modelName << "\n";
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo_eklavya", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle( node_namespace_ );
cmd_vel_sub_ = rosnode_->subscribe("/cmd_vel", 1, &GazeboRosEklavya::OnCmdVel, this );
odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("/odom", 1);
joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("/joint_states", 1);
js_.name.push_back( left_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( right_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( front_castor_wheel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( front_castor_swivel_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
prev_update_time_ = 0;
last_cmd_vel_time_ = 0;
//TODO: fix this
joints_[LEFT] = my_parent_->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = my_parent_->GetJoint(right_wheel_joint_name_);
joints_[FRONT_CASTOR_WHEEL] = my_parent_->GetJoint(front_castor_wheel_joint_name_);
joints_[FRONT_CASTOR_SWIVEL] = my_parent_->GetJoint(front_castor_swivel_joint_name_);
if (joints_[LEFT]) set_joints_[LEFT] = true;
if (joints_[RIGHT]) set_joints_[RIGHT] = true;
if (joints_[FRONT_CASTOR_WHEEL]) set_joints_[FRONT_CASTOR_WHEEL] = true;
if (joints_[FRONT_CASTOR_SWIVEL]) set_joints_[FRONT_CASTOR_SWIVEL] = true;
//initialize time and odometry position
prev_update_time_ = last_cmd_vel_time_ = this->my_world_->GetSimTime();
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}
void GazeboRosKobuki::Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
{
model_ = parent;
if (!model_)
{
ROS_ERROR_STREAM("Invalid model pointer! [" << node_name_ << "]");
return;
}
// Get then name of the parent model and use it as node name
std::string model_name = sdf->GetParent()->GetValueString("name");
gzdbg << "Plugin model name: " << model_name << "\n";
nh_ = ros::NodeHandle("");
// creating a private name pace until Gazebo implements topic remappings
nh_priv_ = ros::NodeHandle("/" + model_name);
node_name_ = model_name;
world_ = parent->GetWorld();
// TODO: use when implementing subs
// ros_spinner_thread_ = new boost::thread(boost::bind(&GazeboRosKobuki::spin, this));
//
// this->node_namespace_ = "";
// if (_sdf->HasElement("node_namespace"))
// this->node_namespace_ = _sdf->GetElement("node_namespace")->GetValueString() + "/";
/*
* Prepare receiving motor power commands
*/
motor_power_sub_ = nh_priv_.subscribe("commands/motor_power", 10, &GazeboRosKobuki::motorPowerCB, this);
motors_enabled_ = true;
/*
* Prepare joint state publishing
*/
if (sdf->HasElement("left_wheel_joint_name"))
{
left_wheel_joint_name_ = sdf->GetElement("left_wheel_joint_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find left wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("right_wheel_joint_name"))
{
right_wheel_joint_name_ = sdf->GetElement("right_wheel_joint_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find right wheel joint in the model description!"
<< " Did you specify the correct joint name?" << " [" << node_name_ <<"]");
return;
}
joints_[LEFT] = parent->GetJoint(left_wheel_joint_name_);
joints_[RIGHT] = parent->GetJoint(right_wheel_joint_name_);
if (!joints_[LEFT] || !joints_[RIGHT])
{
ROS_ERROR_STREAM("Couldn't find specified wheel joints in the model! [" << node_name_ <<"]");
return;
}
joint_state_.header.frame_id = "Joint States";
joint_state_.name.push_back(left_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_.name.push_back(right_wheel_joint_name_);
joint_state_.position.push_back(0);
joint_state_.velocity.push_back(0);
joint_state_.effort.push_back(0);
joint_state_pub_ = nh_.advertise<sensor_msgs::JointState>("joint_states", 1);
/*
* Prepare publishing odometry data
*/
if (sdf->HasElement("wheel_separation"))
{
wheel_sep_ = sdf->GetElement("wheel_separation")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("wheel_diameter"))
{
wheel_diam_ = sdf->GetElement("wheel_diameter")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel diameter parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("torque"))
{
torque_ = sdf->GetElement("torque")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the torque parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
odom_pub_ = nh_.advertise<nav_msgs::Odometry>("odom", 1);
/*
* Prepare receiving velocity commands
*/
if (sdf->HasElement("velocity_command_timeout"))
{
cmd_vel_timeout_ = sdf->GetElement("velocity_command_timeout")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the wheel separation parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
last_cmd_vel_time_ = world_->GetSimTime();
cmd_vel_sub_ = nh_priv_.subscribe("commands/velocity", 100, &GazeboRosKobuki::cmdVelCB, this);
/*
* Prepare cliff sensors
*/
std::string cliff_sensor_left_name, cliff_sensor_front_name, cliff_sensor_right_name;
if (sdf->HasElement("cliff_sensor_left_name"))
{
cliff_sensor_left_name = sdf->GetElement("cliff_sensor_left_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of left cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_front_name"))
{
cliff_sensor_front_name = sdf->GetElement("cliff_sensor_front_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of frontal cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_sensor_right_name"))
{
cliff_sensor_right_name = sdf->GetElement("cliff_sensor_right_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of right cliff sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_left_name));
cliff_sensor_front_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_front_name));
cliff_sensor_right_ = boost::shared_dynamic_cast<sensors::RaySensor>(
sensors::SensorManager::Instance()->GetSensor(cliff_sensor_right_name));
if (!cliff_sensor_left_)
{
ROS_ERROR_STREAM("Couldn't find the left cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_front_)
{
ROS_ERROR_STREAM("Couldn't find the frontal cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (!cliff_sensor_right_)
{
ROS_ERROR_STREAM("Couldn't find the right cliff sensor in the model! [" << node_name_ <<"]");
return;
}
if (sdf->HasElement("cliff_detection_threshold"))
{
cliff_detection_threshold_ = sdf->GetElement("cliff_detection_threshold")->GetValueDouble();
}
else
{
ROS_ERROR_STREAM("Couldn't find the cliff detection threshold parameter in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
cliff_sensor_left_->SetActive(true);
cliff_sensor_front_->SetActive(true);
cliff_sensor_right_->SetActive(true);
cliff_event_pub_ = nh_priv_.advertise<kobuki_msgs::CliffEvent>("events/cliff", 1);
/*
* Prepare bumper
*/
std::string bumper_name;
if (sdf->HasElement("bumper_name"))
{
bumper_name = sdf->GetElement("bumper_name")->GetValueString();
}
else
{
ROS_ERROR_STREAM("Couldn't find the name of bumper sensor in the model description!"
<< " Did you specify it?" << " [" << node_name_ <<"]");
return;
}
bumper_ = boost::shared_dynamic_cast<sensors::ContactSensor>(
sensors::SensorManager::Instance()->GetSensor(bumper_name));
bumper_->SetActive(true);
bumper_event_pub_ = nh_priv_.advertise<kobuki_msgs::BumperEvent>("events/bumper", 1);
prev_update_time_ = world_->GetSimTime();
ROS_INFO_STREAM("GazeboRosKobuki plugin ready to go! [" << node_name_ << "]");
update_connection_ = event::Events::ConnectWorldUpdateStart(boost::bind(&GazeboRosKobuki::OnUpdate, this));
}
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
}
void HuskyPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
{
this->model_ = _parent;
this->world_ = this->model_->GetWorld();
this->node_namespace_ = "";
if (_sdf->HasElement("robotNamespace"))
this->node_namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
bl_joint_name_ = "backLeftJoint";
if (_sdf->HasElement("backLeftJoint"))
bl_joint_name_ = _sdf->GetElement("backLeftJoint")->GetValueString();
br_joint_name_ = "backRightJoint";
if (_sdf->HasElement("backRightJoint"))
br_joint_name_ = _sdf->GetElement("backRightJoint")->GetValueString();
fl_joint_name_ = "frontLeftJoint";
if (_sdf->HasElement("frontLeftJoint"))
fl_joint_name_ = _sdf->GetElement("frontLeftJoint")->GetValueString();
fr_joint_name_ = "frontRightJoint";
if (_sdf->HasElement("frontRightJoint"))
fr_joint_name_ = _sdf->GetElement("frontRightJoint")->GetValueString();
wheel_sep_ = 0.44;
if (_sdf->HasElement("wheelSeparation"))
wheel_sep_ = _sdf->GetElement("wheelSeparation")->GetValueDouble();
wheel_diam_ = 0.25;
if (_sdf->HasElement("wheelDiameter"))
wheel_diam_ = _sdf->GetElement("wheelDiameter")->GetValueDouble();
torque_ = 24.0; // realer Wert 6
if (_sdf->HasElement("torque"))
torque_ = _sdf->GetElement("torque")->GetValueDouble();
base_geom_name_ = "base_link";
if (_sdf->HasElement("baseGeom"))
base_geom_name_ = _sdf->GetElement("baseGeom")->GetValueString();
base_geom_ = model_->GetChildCollision(base_geom_name_);
//base_geom_->SetContactsEnabled(true);
// Get the name of the parent model
std::string modelName = _sdf->GetParent()->GetValueString("name");
// Listen to the update event. This event is broadcast every
// simulation iteration.
this->updateConnection = event::Events::ConnectWorldUpdateStart(
boost::bind(&HuskyPlugin::UpdateChild, this));
gzdbg << "plugin model name: " << modelName << "\n";
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc, argv, "gazebo_husky", ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
rosnode_ = new ros::NodeHandle( node_namespace_ );
cmd_vel_sub_ = rosnode_->subscribe("cmd_vel", 1, &HuskyPlugin::OnCmdVel, this );
odom_pub_ = rosnode_->advertise<nav_msgs::Odometry>("odom", 1);
joint_state_pub_ = rosnode_->advertise<sensor_msgs::JointState>("joint_states", 1);
js_.name.push_back( bl_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( br_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( fl_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
js_.name.push_back( fr_joint_name_ );
js_.position.push_back(0);
js_.velocity.push_back(0);
js_.effort.push_back(0);
prev_update_time_ = 0;
last_cmd_vel_time_ = 0;
//TODO: fix this
joints_[BL] = model_->GetJoint(bl_joint_name_);
joints_[BR] = model_->GetJoint(br_joint_name_);
joints_[FL] = model_->GetJoint(fl_joint_name_);
joints_[FR] = model_->GetJoint(fr_joint_name_);
if (joints_[BL]) set_joints_[BL] = true;
if (joints_[BR]) set_joints_[BR] = true;
if (joints_[FL]) set_joints_[FL] = true;
if (joints_[FR]) set_joints_[FR] = true;
//initialize time and odometry position
prev_update_time_ = last_cmd_vel_time_ = this->world_->GetSimTime();
odom_pose_[0] = 0.0;
odom_pose_[1] = 0.0;
odom_pose_[2] = 0.0;
}