Gazebo plugin

edit

I got the following error suddenly in otherwise working code. the error doesnt show when a plugin is not included however thats not the solution.

Error [parser.cc:697] XML Element[plugin], child of element[link] not defined in SDF. Ignoring.[link]

Error [parser.cc:688] Error reading element <link>

Error [parser.cc:688] Error reading element <model>

Error [parser.cc:348] Unable to read element <sdf>

Error [parser.cc:299] parse as old deprecated model file failed.

Error [World.cc:1469] Unable to read sdf string

It appears at the section where I include the plugin for 6D velocity command. It looks like:

<gazebo reference="base_footprint">
  <plugin name="3d_vel_controller" filename="libgazebo_ros3d.so">
    <robotNamespace>quad</robotNamespace>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>map</odometryFrame>
    <robotBaseFrame>base_footprint</robotBaseFrame>
    <odometryRate>20</odometryRate>
  </plugin>
</gazebo>

and my plugin is

#include "/home/saroj/gazebo_plugin/gazebo_ros3d.h"

include <cmath>

namespace gazebo {

GazeboRos3DMove::GazeboRos3DMove() {}

GazeboRos3DMove::~GazeboRos3DMove() {}

// Load the controller void GazeboRos3DMove::Load(physics::ModelPtr parent, sdf::ElementPtr sdf) {

parent_ = parent;

/* Parse parameters */

robot_namespace_ = "";
if (!sdf->HasElement("robotNamespace")) 
{
  ROS_INFO("3DMovePlugin missing <robotNamespace>, "
      "defaults to \"%s\"", robot_namespace_.c_str());
}
else 
{
  robot_namespace_ = 
    sdf->GetElement("robotNamespace")->Get<std::string>();
}

command_topic_ = "cmd_vel";
if (!sdf->HasElement("commandTopic")) 
{
  ROS_WARN("3DMovePlugin (ns = %s) missing <commandTopic>, "
      "defaults to \"%s\"", 
      robot_namespace_.c_str(), command_topic_.c_str());
} 
else 
{
  command_topic_ = sdf->GetElement("commandTopic")->Get<std::string>();
}

odometry_topic_ = "odom";
if (!sdf->HasElement("odometryTopic")) 
{
  ROS_WARN("3DMovePlugin (ns = %s) missing <odometryTopic>, "
      "defaults to \"%s\"", 
      robot_namespace_.c_str(), odometry_topic_.c_str());
} 
else 
{
  odometry_topic_ = sdf->GetElement("odometryTopic")->Get<std::string>();
}

odometry_frame_ = "odom";
if (!sdf->HasElement("odometryFrame")) 
{
  ROS_WARN("3DMovePlugin (ns = %s) missing <odometryFrame>, "
      "defaults to \"%s\"",
      robot_namespace_.c_str(), odometry_frame_.c_str());
}
else 
{
  odometry_frame_ = sdf->GetElement("odometryFrame")->Get<std::string>();
}

robot_base_frame_ = "base_footprint";
if (!sdf->HasElement("robotBaseFrame")) 
{
  ROS_WARN("3DMovePlugin (ns = %s) missing <robotBaseFrame>, "
      "defaults to \"%s\"",
      robot_namespace_.c_str(), robot_base_frame_.c_str());
} 
else 
{
  robot_base_frame_ = sdf->GetElement("robotBaseFrame")->Get<std::string>();
} 

odometry_rate_ = 20.0;
if (!sdf->HasElement("odometryRate")) 
{
  ROS_WARN("3DMovePlugin (ns = %s) missing <odometryRate>, "
      "defaults to %f",
      robot_namespace_.c_str(), odometry_rate_);
} 
else 
{
  odometry_rate_ = sdf->GetElement("odometryRate")->Get<double>();
} 

last_odom_publish_time_ = parent_->GetWorld()->GetSimTime();
last_odom_pose_ = parent_->GetWorldPose();
x_ = 0;
y_ = 0;
z_ = 0;
rot_x_ = 0;
rot_y_ = 0;
rot_z_ = 0;
alive_ = true;

// Ensure that ROS has been initialized and subscribe to cmd_vel
if (!ros::isInitialized()) 
{
  ROS_FATAL_STREAM("3DMovePlugin (ns = " << robot_namespace_
    << "). 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_.reset(new ros::NodeHandle(robot_namespace_));

ROS_DEBUG("OCPlugin (%s) has started!", 
    robot_namespace_.c_str());

tf_prefix_ = tf::getPrefixParam(*rosnode_);
transform_broadcaster_.reset(new tf::TransformBroadcaster());

// subscribe to the odometry topic
ros::SubscribeOptions so =
  ros::SubscribeOptions::create<geometry_msgs::Twist>(command_topic_, 1,
      boost::bind(&GazeboRos3DMove::cmdVelCallback, this, _1),
      ros::VoidPtr(), &queue_);

vel_sub_ = rosnode_->subscribe(so);
odometry_pub_ = rosnode_->advertise<nav_msgs::Odometry>(odometry_topic_, 1);

// start custom queue for diff drive
callback_queue_thread_ = 
  boost::thread(boost::bind(&GazeboRos3DMove::QueueThread, this));

// listen to the update event (broadcast every simulation iteration)
update_connection_ = 
  event::Events::ConnectWorldUpdateBegin(
      boost::bind(&GazeboRos3DMove::UpdateChild, this));

}

// Update the controller void GazeboRos3DMove::UpdateChild() { boost::mutex::scoped_lock scoped_lock(lock); math::Pose pose = parent_->GetWorldPose(); float phi = pose.rot.GetRoll(); float theta = pose.rot ... (more)