Robotics StackExchange | Archived questions

Gazebo plugin

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

Error [parser.cc:688] Error reading element

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

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

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::scopedlock scopedlock(lock); math::Pose pose = parent_->GetWorldPose(); float phi = pose.rot.GetRoll(); float theta = pose.rot.GetPitch(); float psi = pose.rot.GetYaw();

parent_->SetLinearVel(math::Vector3(x_*cosf(theta)*cosf(psi) + y_*(sinf(phi)*sinf(theta)*cosf(psi)-cosf(phi)*sinf(psi)) + z_*(cosf(phi)*sinf(theta)*cosf(psi)+sinf(phi)*sinf(psi)), 
      x_*cosf(theta)*sinf(psi) + y_*(sinf(phi)*sinf(theta)*sinf(psi)+cosf(phi)*cosf(psi)) + z_*(cosf(phi)*sinf(theta)*sinf(psi)-sinf(phi)*cosf(psi)), 
      x_*(-sinf(theta)) + y_*sinf(phi)*cosf(theta) + z_*cosf(phi)*cosf(theta)));


parent_->SetAngularVel(math::Vector3(
        //  rot_x_ + rot_y_*sinf(phi)*tanf(theta) + rot_z_*cosf(phi)*tanf(theta),
        //  rot_y_ * cosf(phi) + rot_z_*(-sinf(theta)),
        //  rot_y_ * sinf(phi)/cosf(theta) + rot_z_*cosf(phi)/cosf(theta)));
            rot_x_,
            rot_y_,
            rot_z_));


if (odometry_rate_ > 0.0) {
  common::Time current_time = parent_->GetWorld()->GetSimTime();
  double seconds_since_last_update = 
    (current_time - last_odom_publish_time_).Double();
  if (seconds_since_last_update > (1.0 / odometry_rate_)) {
    publishOdometry(seconds_since_last_update);
    last_odom_publish_time_ = current_time;
  }
}

}

// Finalize the controller void GazeboRos3DMove::FiniChild() { alive_ = false; queue.clear(); queue.disable(); rosnode->shutdown(); callbackqueuethread.join(); }

void GazeboRos3DMove::cmdVelCallback( const geometrymsgs::Twist::ConstPtr& cmdmsg) { boost::mutex::scopedlock scopedlock(lock); x_ = cmdmsg->linear.x; y = cmdmsg->linear.y; z = cmdmsg->linear.z; rotx_ = cmdmsg->angular.x; roty_ = cmdmsg->angular.y; rotz_ = cmd_msg->angular.z; }

void GazeboRos3DMove::QueueThread() { static const double timeout = 0.01; while (alive_ && rosnode->ok()) { queue.callAvailable(ros::WallDuration(timeout)); } }

void GazeboRos3DMove::publishOdometry(double step_time) {

ros::Time current_time = ros::Time::now();
std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
std::string base_footprint_frame = 
  tf::resolve(tf_prefix_, robot_base_frame_);

// getting data for base_footprint to odom transform
math::Pose pose = this->parent_->GetWorldPose();

tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);

tf::Transform base_footprint_to_odom(qt, vt);
transform_broadcaster_->sendTransform(
    tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame,
        base_footprint_frame));

// publish odom topic
odom_.pose.pose.position.x = pose.pos.x;
odom_.pose.pose.position.y = pose.pos.y;

odom_.pose.pose.orientation.x = pose.rot.x;
odom_.pose.pose.orientation.y = pose.rot.y;
odom_.pose.pose.orientation.z = pose.rot.z;
odom_.pose.pose.orientation.w = pose.rot.w;
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.001;

// get velocity in /odom frame
math::Vector3 linear;
linear.x = (pose.pos.x - last_odom_pose_.pos.x) / step_time;
linear.y = (pose.pos.y - last_odom_pose_.pos.y) / step_time;
linear.z = (pose.pos.z - last_odom_pose_.pos.z) / step_time;
if (rot_x_ > M_PI / step_time) 
{ 
  // we cannot calculate the angular velocity correctly
  odom_.twist.twist.angular.x = rot_x_;

} 
else 
{
  float last_roll = last_odom_pose_.rot.GetRoll();
  float current_roll = pose.rot.GetRoll();
  while (current_roll < last_roll - M_PI) current_roll += 2 * M_PI;
  while (current_roll > last_roll + M_PI) current_roll -= 2 * M_PI;
  float angular_diff_roll = current_roll - last_roll;
  odom_.twist.twist.angular.x = angular_diff_roll / step_time;
 }

 if (rot_y_ > M_PI / step_time) 
{ 
  odom_.twist.twist.angular.y = rot_y_;
} 
else 
{
  float last_pitch = last_odom_pose_.rot.GetPitch();
  float current_pitch = pose.rot.GetPitch();
  while (current_pitch < last_pitch - M_PI) current_pitch += 2 * M_PI;
  while (current_pitch > last_pitch + M_PI) current_pitch -= 2 * M_PI;
  float angular_diff_pitch = current_pitch - last_pitch;
  odom_.twist.twist.angular.y = angular_diff_pitch / step_time;
}

 if (rot_z_ > M_PI / step_time) 
{ 
  odom_.twist.twist.angular.z = rot_z_;
} 
else 
{
  float last_yaw = last_odom_pose_.rot.GetYaw();
  float current_yaw = pose.rot.GetYaw();
  while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
  while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
  float angular_diff_yaw = current_yaw - last_yaw;
  odom_.twist.twist.angular.z = angular_diff_yaw / step_time;
}

last_odom_pose_ = pose;

// convert velocity to child_frame_id (aka base_footprint)
float phi = pose.rot.GetRoll();
float theta = pose.rot.GetPitch();
float psi = pose.rot.GetYaw();


odom_.twist.twist.linear.x = cosf(theta)*cosf(psi) * linear.x + cosf(theta)*sinf(psi) * linear.y - sinf(theta)*linear.z;
odom_.twist.twist.linear.y = (sinf(phi)*sinf(theta)*cosf(psi)-cosf(phi)*sinf(psi)) * linear.x + (sinf(phi)*sinf(theta)*sinf(psi)+cosf(phi)*cosf(psi))* linear.y + sinf(phi)*cosf(theta)*linear.z;
odom_.twist.twist.linear.z = (cosf(phi)*sinf(theta)*cosf(psi)+sinf(phi)*sinf(psi))*linear.x + (cosf(phi)*sinf(theta)*sinf(psi)-sinf(phi)*cosf(psi)) * linear.y + cosf(phi)*cosf(theta)*linear.z;

odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;

odometry_pub_.publish(odom_);

}

GZREGISTERMODEL_PLUGIN(GazeboRos3DMove) }

There might be some stupid error overlooked. Any help is highly appreciated. Thanks

Asked by espee on 2013-10-04 09:20:29 UTC

Comments

Answers