in simulation_ws/src/rover/hector_gazebo_plugins/src/diffdrive_plugin_multi_wheel.cpp [114:300]
void GazeboRosDiffDriveMultiWheel::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
this->parent = _parent;
this->world = _parent->GetWorld();
this->robot_namespace_ = "";
if (!_sdf->HasElement("robotNamespace")) {
ROS_INFO("GazeboRosDiffDriveMultiWheel Plugin missing <robotNamespace>, defaults to \"%s\"",
this->robot_namespace_.c_str());
} else {
this->robot_namespace_ =
_sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
}
//this->left_joint_names_ = "left_joint";
if (!_sdf->HasElement("leftJoints")) {
gzthrow("Have to specify space separated left side joint names via <leftJoints> tag!");
} else {
std::string joint_string = _sdf->GetElement("leftJoints")->Get<std::string>();
boost::split( joint_names_[LEFT], joint_string, boost::is_any_of(" ") );
}
//this->right_joint_names_ = "right_joint";
if (!_sdf->HasElement("rightJoints")) {
gzthrow("Have to specify space separated right side joint names via <rightJoints> tag!");
} else {
std::string joint_string = _sdf->GetElement("rightJoints")->Get<std::string>();
boost::split( joint_names_[RIGHT], joint_string, boost::is_any_of(" ") );
}
this->wheel_separation_ = 0.34;
if (!_sdf->HasElement("wheelSeparation")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <wheelSeparation>, defaults to %f",
this->robot_namespace_.c_str(), this->wheel_separation_);
} else {
this->wheel_separation_ =
_sdf->GetElement("wheelSeparation")->Get<double>();
}
this->wheel_diameter_ = 0.15;
if (!_sdf->HasElement("wheelDiameter")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <wheelDiameter>, defaults to %f",
this->robot_namespace_.c_str(), this->wheel_diameter_);
} else {
this->wheel_diameter_ = _sdf->GetElement("wheelDiameter")->Get<double>();
}
this->torque = 5.0;
if (!_sdf->HasElement("torque")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <torque>, defaults to %f",
this->robot_namespace_.c_str(), this->torque);
} else {
this->torque = _sdf->GetElement("torque")->Get<double>();
}
this->command_topic_ = "cmd_vel";
if (!_sdf->HasElement("commandTopic")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <commandTopic>, defaults to \"%s\"",
this->robot_namespace_.c_str(), this->command_topic_.c_str());
} else {
this->command_topic_ = _sdf->GetElement("commandTopic")->Get<std::string>();
}
this->odometry_topic_ = "odom";
if (!_sdf->HasElement("odometryTopic")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <odometryTopic>, defaults to \"%s\"",
this->robot_namespace_.c_str(), this->odometry_topic_.c_str());
} else {
this->odometry_topic_ = _sdf->GetElement("odometryTopic")->Get<std::string>();
}
this->odometry_frame_ = "odom";
if (!_sdf->HasElement("odometryFrame")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <odometryFrame>, defaults to \"%s\"",
this->robot_namespace_.c_str(), this->odometry_frame_.c_str());
} else {
this->odometry_frame_ = _sdf->GetElement("odometryFrame")->Get<std::string>();
}
this->robot_base_frame_ = "base_footprint";
if (!_sdf->HasElement("robotBaseFrame")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <robotBaseFrame>, defaults to \"%s\"",
this->robot_namespace_.c_str(), this->robot_base_frame_.c_str());
} else {
this->robot_base_frame_ = _sdf->GetElement("robotBaseFrame")->Get<std::string>();
}
this->update_rate_ = 100.0;
if (!_sdf->HasElement("updateRate")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <updateRate>, defaults to %f",
this->robot_namespace_.c_str(), this->update_rate_);
} else {
this->update_rate_ = _sdf->GetElement("updateRate")->Get<double>();
}
this->publish_odometry_tf_ = true;
if (!_sdf->HasElement("publishOdometryTf")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <publishOdometryTf>, defaults to %s",
this->robot_namespace_.c_str(), this->publish_odometry_tf_ ? "true" : "false");
} else {
this->publish_odometry_tf_ = _sdf->GetElement("publishOdometryTf")->Get<bool>();
}
this->publish_odometry_msg_ = true;
if (!_sdf->HasElement("publishOdometryMsg")) {
ROS_WARN("GazeboRosDiffDriveMultiWheel Plugin (ns = %s) missing <publishOdometryMsg>, defaults to %s",
this->robot_namespace_.c_str(), this->publish_odometry_msg_ ? "true" : "false");
} else {
this->publish_odometry_msg_ = _sdf->GetElement("publishOdometryMsg")->Get<bool>();
}
// Initialize update rate stuff
if (this->update_rate_ > 0.0) {
this->update_period_ = 1.0 / this->update_rate_;
} else {
this->update_period_ = 0.0;
}
#if (GAZEBO_MAJOR_VERSION >= 8)
last_update_time_ = this->world->SimTime();
#else
last_update_time_ = this->world->GetSimTime();
#endif
// Initialize velocity stuff
wheel_speed_[RIGHT] = 0;
wheel_speed_[LEFT] = 0;
x_ = 0;
rot_ = 0;
alive_ = true;
for (size_t side = 0; side < 2; ++side){
for (size_t i = 0; i < joint_names_[side].size(); ++i){
joints_[side].push_back(this->parent->GetJoint(joint_names_[side][i]));
if (!joints_[side][i]){
char error[200];
snprintf(error, 200,
"GazeboRosDiffDriveMultiWheel Plugin (ns = %s) couldn't get hinge joint named \"%s\"",
this->robot_namespace_.c_str(), joint_names_[side][i].c_str());
gzthrow(error);
}
#if (GAZEBO_MAJOR_VERSION > 4)
joints_[side][i]->SetEffortLimit(0, torque);
#else
joints_[side][i]->SetMaxForce(0, torque);
#endif
}
}
// Make sure the ROS node for Gazebo has already been initialized
if (!ros::isInitialized())
{
ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
<< "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
return;
}
rosnode_ = new ros::NodeHandle(this->robot_namespace_);
ROS_INFO("Starting GazeboRosDiffDriveMultiWheel Plugin (ns = %s)!", this->robot_namespace_.c_str());
tf_prefix_ = tf::getPrefixParam(*rosnode_);
transform_broadcaster_ = new tf::TransformBroadcaster();
// ROS: Subscribe to the velocity command topic (usually "cmd_vel")
ros::SubscribeOptions so =
ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
boost::bind(&GazeboRosDiffDriveMultiWheel::cmdVelCallback, this, _1),
ros::VoidPtr(), &queue_);
cmd_vel_subscriber_ = rosnode_->subscribe(so);
odometry_publisher_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);
// start custom queue for diff drive
this->callback_queue_thread_ =
boost::thread(boost::bind(&GazeboRosDiffDriveMultiWheel::QueueThread, this));
// listen to the update event (broadcast every simulation iteration)
this->update_connection_ =
event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosDiffDriveMultiWheel::UpdateChild, this));
}