////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboQuadrotorStateController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (!_sdf->HasElement("robotNamespace"))
//namespace_.clear();
robot_namespace_.clear();
else
//namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
robot_namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
namespace_ = robot_namespace_;
if (!_sdf->HasElement("node_namespace"))
{
node_namespace_.clear();
}
else
{
this->node_namespace_ = this->robot_namespace_ + _sdf->GetElement("node_namespace")->GetValueString() + "/";
namespace_ = namespace_ + node_namespace_;
}
//////////////////////////
if (!_sdf->HasElement("topicName"))
velocity_topic_ = "cmd_vel";
else
velocity_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
if (!_sdf->HasElement("takeoffTopic"))
//takeoff_topic_ = "/ardrone/takeoff";
takeoff_topic_ = "takeoff";
else
takeoff_topic_ = _sdf->GetElement("takeoffTopic")->Get<std::string>();
//if (!_sdf->HasElement("/ardrone/land"))
// land_topic_ = "/ardrone/land";
if (!_sdf->HasElement("landTopic"))
land_topic_ = "land";
else
land_topic_ = _sdf->GetElement("landTopic")->Get<std::string>();
if (!_sdf->HasElement("resetTopic"))
//reset_topic_ = "/ardrone/reset";
reset_topic_ = "reset";
else
reset_topic_ = _sdf->GetElement("resetTopic")->Get<std::string>();
if (!_sdf->HasElement("navdataTopic"))
{
//navdata_topic_ = "/ardrone/navdata"; ORIGINAL
navdata_topic_ = "navdata";
//required from tum_ardrone
navdata_topic_tum = "/ardrone/navdata";
}
else
{
navdata_topic_ = _sdf->GetElement("navdataTopic")->Get<std::string>();
//required frim tum_ardrone
navdata_topic_tum = "/ardrone/navdata";
}
if (!_sdf->HasElement("imuTopic"))
imu_topic_.clear();
else
imu_topic_ = _sdf->GetElement("imuTopic")->Get<std::string>();
if (!_sdf->HasElement("sonarTopic"))
sonar_topic_.clear();
else
sonar_topic_ = _sdf->GetElement("sonarTopic")->Get<std::string>();
if (!_sdf->HasElement("stateTopic"))
state_topic_.clear();
else
state_topic_ = _sdf->GetElement("stateTopic")->Get<std::string>();
/*
if (!_sdf->HasElement("bodyName"))
{
link = _model->GetLink();
link_name_ = link->GetName();
}
else {
link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
link = boost::dynamic_pointer_cast<physics::Link>(world->GetEntity(link_name_));
}*/
link = _model->GetChildLink("base_link");
if (!link)
{
ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
node_handle_ = new ros::NodeHandle(namespace_);
// subscribe command: velocity control command
if (!velocity_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
velocity_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::VelocityCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
velocity_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!takeoff_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
takeoff_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::TakeoffCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
takeoff_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!land_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
land_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::LandCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
land_subscriber_ = node_handle_->subscribe(ops);
}
// subscribe command: take off command
if (!reset_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<std_msgs::Empty>(
reset_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ResetCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
reset_subscriber_ = node_handle_->subscribe(ops);
}
m_navdataPub = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_ , 25 );
//required by tum_ardrone
m_navdataPub_tum = node_handle_->advertise< ardrone_autonomy::Navdata >( navdata_topic_tum, 25);
// subscribe imu
if (!imu_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
imu_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::ImuCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
imu_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
}
// subscribe sonar senor info
if (!sonar_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Range>(
sonar_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::SonarCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
sonar_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using sonar information on topic %s as source of altitude.", sonar_topic_.c_str());
}
// subscribe state
if (!state_topic_.empty())
{
ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
state_topic_, 1,
boost::bind(&GazeboQuadrotorStateController::StateCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
state_subscriber_ = node_handle_->subscribe(ops);
ROS_INFO_NAMED("quadrotor_state_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
}
// for camera control
// switch camera server
//std::string toggleCam_topic = "ardrone/togglecam";
std::string toggleCam_topic = "togglecam";
ros::AdvertiseServiceOptions toggleCam_ops = ros::AdvertiseServiceOptions::create<std_srvs::Empty>(
toggleCam_topic,
boost::bind(&GazeboQuadrotorStateController::toggleCamCallback, this, _1,_2),
ros::VoidPtr(),
&callback_queue_);
toggleCam_service = node_handle_->advertiseService(toggleCam_ops);
// camera image data
/*
// Original code
std::string cam_out_topic = "/ardrone/image_raw";
std::string cam_front_in_topic = "/ardrone/front/image_raw";
std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
*/
// CODE FOR MAKING AR_POSE WORKING: it needs /camera/etc. as topic
// ABSOLUTE PATH REQUIRED BY AR_POSE: should be changed accordingly to match proper video topic
std::string cam_out_topic = "/camera/image_raw";
std::string cam_front_in_topic = "/ardrone/front/image_raw";
std::string cam_bottom_in_topic = "/ardrone/bottom/image_raw";
std::string in_transport = "raw";
camera_it_ = new image_transport::ImageTransport(*node_handle_);
camera_publisher_ = camera_it_->advertise(cam_out_topic, 1);
camera_front_subscriber_ = camera_it_->subscribe(
cam_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraFrontCallback, this, _1),
ros::VoidPtr(), in_transport);
camera_bottom_subscriber_ = camera_it_->subscribe(
cam_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraBottomCallback, this, _1),
ros::VoidPtr(), in_transport);
// camera image data
/*
// Original code
std::string cam_info_out_topic = "/ardrone/camera_info";
std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
*/
// CODE FOR MAKING AR_POSE WORKING: it needs /camera/etc. as topic
// ABSOLUTE PATH REQUIRED BY AR_POSE: should be changed accordingly to match proper video topic
std::string cam_info_out_topic = "/camera/camera_info";
std::string cam_info_front_in_topic = "/ardrone/front/camera_info";
std::string cam_info_bottom_in_topic = "/ardrone/bottom/camera_info";
camera_info_publisher_ = node_handle_->advertise<sensor_msgs::CameraInfo>(cam_info_out_topic,1);
ros::SubscribeOptions cam_info_front_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_front_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoFrontCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_front_subscriber_ = node_handle_->subscribe(cam_info_front_ops);
ros::SubscribeOptions cam_info_bottom_ops = ros::SubscribeOptions::create<sensor_msgs::CameraInfo>(
cam_info_bottom_in_topic, 1,
boost::bind(&GazeboQuadrotorStateController::CameraInfoBottomCallback, this, _1),
ros::VoidPtr(), &callback_queue_);
camera_info_bottom_subscriber_ = node_handle_->subscribe(cam_info_bottom_ops);
// callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorStateController::CallbackQueueThread,this ) );
Reset();
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboQuadrotorStateController::Update, this));
robot_current_state = LANDED_MODEL;
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
{
world = _model->GetWorld();
// load parameters
if (!_sdf->HasElement("robotNamespace"))
namespace_.clear();
else
namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>() + "/";
if (!_sdf->HasElement("topicName"))
topic_ = "magnetic";
else
topic_ = _sdf->GetElement("topicName")->Get<std::string>();
link = _model->GetChildLink("base_link");
if (!link)
{
ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
return;
}
double update_rate = 0.0;
if (_sdf->HasElement("updateRate")) update_rate = _sdf->GetElement("updateRate")->Get<double>();
update_period = update_rate > 0.0 ? 1.0/update_rate : 0.0;
if (!_sdf->HasElement("frameId"))
frame_id_ = link_name_;
else
frame_id_ = _sdf->GetElement("frameId")->Get<std::string>();
if (!_sdf->HasElement("magnitude"))
magnitude_ = DEFAULT_MAGNITUDE;
else
magnitude_ = _sdf->GetElement("magnitude")->Get<double>();
if (!_sdf->HasElement("referenceHeading"))
reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
else
reference_heading_ = _sdf->GetElement("referenceHeading")->Get<double>() * M_PI/180.0;
if (!_sdf->HasElement("declination"))
declination_ = DEFAULT_DECLINATION * M_PI/180.0;
else
declination_ = _sdf->GetElement("declination")->Get<double>() * M_PI/180.0;
if (!_sdf->HasElement("inclination"))
inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
else
inclination_ = _sdf->GetElement("inclination")->Get<double>() * M_PI/180.0;
// Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
magnetic_field_.header.frame_id = frame_id_;
magnetic_field_world_.x = magnitude_ * cos(inclination_) * cos(reference_heading_ - declination_);
magnetic_field_world_.y = magnitude_ * sin(reference_heading_ - declination_);
magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);
sensor_model_.Load(_sdf);
// start ros node
if (!ros::isInitialized())
{
int argc = 0;
char** argv = NULL;
ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
}
node_handle_ = new ros::NodeHandle(namespace_);
publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);
Reset();
// New Mechanism for Updating every World Cycle
// Listen to the update event. This event is broadcast every
// simulation iteration.
updateConnection = event::Events::ConnectWorldUpdateBegin(
boost::bind(&GazeboRosMagnetic::Update, this));
}