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