diff --git a/tetris_description/urdf/tetris.gazebo.xacro b/tetris_description/urdf/tetris.gazebo.xacro index 96bc3b8..088e7d5 100644 --- a/tetris_description/urdf/tetris.gazebo.xacro +++ b/tetris_description/urdf/tetris.gazebo.xacro @@ -9,7 +9,7 @@ - + true 100 right_wheel_joint diff --git a/tetris_gazebo/CMakeLists.txt b/tetris_gazebo/CMakeLists.txt index 6914af2..5f44f7f 100644 --- a/tetris_gazebo/CMakeLists.txt +++ b/tetris_gazebo/CMakeLists.txt @@ -1,10 +1,35 @@ cmake_minimum_required(VERSION 2.8.3) project(tetris_gazebo) -find_package(catkin REQUIRED COMPONENTS) +find_package(catkin REQUIRED COMPONENTS gazebo_plugins) + +# Depend on system install of Gazebo and SDFormat +find_package(gazebo REQUIRED) +find_package(Boost REQUIRED COMPONENTS thread) + +include_directories( + ${Boost_INCLUDE_DIRS} + ${catkin_INCLUDE_DIRS} + ${GAZEBO_INCLUDE_DIRS} +) + +link_directories( + ${catkin_LIBRARY_DIRS} + ${GAZEBO_LIBRARY_DIRS} +) + catkin_package() +add_library(gazebo_ros_diff_drive_fixed src/gazebo_ros_diff_drive.cpp) +target_link_libraries(gazebo_ros_diff_drive_fixed gazebo_ros_utils ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +install(TARGETS + gazebo_ros_diff_drive_fixed + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) + install(DIRECTORY launch worlds models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) diff --git a/tetris_gazebo/package.xml b/tetris_gazebo/package.xml index c84caed..cb16b98 100644 --- a/tetris_gazebo/package.xml +++ b/tetris_gazebo/package.xml @@ -14,6 +14,8 @@ catkin + gazebo_plugins + tetris_description rosbash xacro diff --git a/tetris_gazebo/src/gazebo_ros_diff_drive.cpp b/tetris_gazebo/src/gazebo_ros_diff_drive.cpp new file mode 100644 index 0000000..9d8178d --- /dev/null +++ b/tetris_gazebo/src/gazebo_ros_diff_drive.cpp @@ -0,0 +1,425 @@ +/* + Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions are met: + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above copyright + notice, this list of conditions and the following disclaimer in the + documentation and/or other materials provided with the distribution. + * Neither the name of the nor the + names of its contributors may be used to endorse or promote products + derived from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY + EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY + DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +*/ + +/* + * \file gazebo_ros_diff_drive.cpp + * + * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin + * developed for the erratic robot (see copyright notice above). The original + * plugin can be found in the ROS package gazebo_erratic_plugins. + * + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * $ Id: 06/21/2013 11:23:40 AM piyushk $ + */ + + +/* + * + * The support of acceleration limit was added by + * \author George Todoran + * \author Markus Bader + * \date 22th of May 2014 + */ + +#include +#include + +//#include +#include "gazebo_ros_diff_drive.h" + +#include +#include + +#include + +namespace gazebo +{ + +enum { + RIGHT, + LEFT, +}; + +GazeboRosDiffDrive::GazeboRosDiffDrive() {} + +// Destructor +GazeboRosDiffDrive::~GazeboRosDiffDrive() {} + +// Load the controller +void GazeboRosDiffDrive::Load ( physics::ModelPtr _parent, sdf::ElementPtr _sdf ) +{ + + this->parent = _parent; + gazebo_ros_ = GazeboRosPtr ( new GazeboRos ( _parent, _sdf, "DiffDrive" ) ); + // Make sure the ROS node for Gazebo has already been initialized + gazebo_ros_->isInitialized(); + + gazebo_ros_->getParameter ( command_topic_, "commandTopic", "cmd_vel" ); + gazebo_ros_->getParameter ( odometry_topic_, "odometryTopic", "odom" ); + gazebo_ros_->getParameter ( odometry_frame_, "odometryFrame", "odom" ); + gazebo_ros_->getParameter ( robot_base_frame_, "robotBaseFrame", "base_footprint" ); + gazebo_ros_->getParameterBoolean ( publishWheelTF_, "publishWheelTF", false ); + gazebo_ros_->getParameterBoolean ( publishWheelJointState_, "publishWheelJointState", false ); + + gazebo_ros_->getParameter ( wheel_separation_, "wheelSeparation", 0.34 ); + gazebo_ros_->getParameter ( wheel_diameter_, "wheelDiameter", 0.15 ); + gazebo_ros_->getParameter ( wheel_accel, "wheelAcceleration", 0.0 ); + gazebo_ros_->getParameter ( wheel_torque, "wheelTorque", 5.0 ); + gazebo_ros_->getParameter ( update_rate_, "updateRate", 100.0 ); + std::map odomOptions; + odomOptions["encoder"] = ENCODER; + odomOptions["world"] = WORLD; + gazebo_ros_->getParameter ( odom_source_, "odometrySource", odomOptions, WORLD ); + + + joints_.resize ( 2 ); + joints_[LEFT] = gazebo_ros_->getJoint ( parent, "leftJoint", "left_joint" ); + joints_[RIGHT] = gazebo_ros_->getJoint ( parent, "rightJoint", "right_joint" ); + joints_[LEFT]->SetMaxForce ( 0, wheel_torque ); + joints_[RIGHT]->SetMaxForce ( 0, wheel_torque ); + + + + this->publish_tf_ = true; + if (!_sdf->HasElement("publishTf")) { + ROS_WARN("GazeboRosDiffDrive Plugin (ns = %s) missing , defaults to %d", + this->robot_namespace_.c_str(), this->publish_tf_); + } else { + this->publish_tf_ = _sdf->GetElement("publishTf")->Get(); + } + + // Initialize update rate stuff + if ( this->update_rate_ > 0.0 ) this->update_period_ = 1.0 / this->update_rate_; + else this->update_period_ = 0.0; + last_update_time_ = parent->GetWorld()->GetSimTime(); + + // Initialize velocity stuff + wheel_speed_[RIGHT] = 0; + wheel_speed_[LEFT] = 0; + + x_ = 0; + rot_ = 0; + alive_ = true; + + + if (this->publishWheelJointState_) + { + joint_state_publisher_ = gazebo_ros_->node()->advertise("joint_states", 1000); + ROS_INFO("%s: Advertise joint_states!", gazebo_ros_->info()); + } + + transform_broadcaster_ = boost::shared_ptr(new tf::TransformBroadcaster()); + + // ROS: Subscribe to the velocity command topic (usually "cmd_vel") + ROS_INFO("%s: Try to subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + ros::SubscribeOptions so = + ros::SubscribeOptions::create(command_topic_, 1, + boost::bind(&GazeboRosDiffDrive::cmdVelCallback, this, _1), + ros::VoidPtr(), &queue_); + + cmd_vel_subscriber_ = gazebo_ros_->node()->subscribe(so); + ROS_INFO("%s: Subscribe to %s!", gazebo_ros_->info(), command_topic_.c_str()); + + if (this->publish_tf_) + { + odometry_publisher_ = gazebo_ros_->node()->advertise(odometry_topic_, 1); + ROS_INFO("%s: Advertise odom on %s !", gazebo_ros_->info(), odometry_topic_.c_str()); + } + + // start custom queue for diff drive + this->callback_queue_thread_ = + boost::thread ( boost::bind ( &GazeboRosDiffDrive::QueueThread, this ) ); + + // listen to the update event (broadcast every simulation iteration) + this->update_connection_ = + event::Events::ConnectWorldUpdateBegin ( boost::bind ( &GazeboRosDiffDrive::UpdateChild, this ) ); + +} + +void GazeboRosDiffDrive::Reset() +{ + last_update_time_ = parent->GetWorld()->GetSimTime(); + pose_encoder_.x = 0; + pose_encoder_.y = 0; + pose_encoder_.theta = 0; + x_ = 0; + rot_ = 0; + joints_[LEFT]->SetMaxForce ( 0, wheel_torque ); + joints_[RIGHT]->SetMaxForce ( 0, wheel_torque ); +} + +void GazeboRosDiffDrive::publishWheelJointState() +{ + ros::Time current_time = ros::Time::now(); + + joint_state_.header.stamp = current_time; + joint_state_.name.resize ( joints_.size() ); + joint_state_.position.resize ( joints_.size() ); + + for ( int i = 0; i < 2; i++ ) { + physics::JointPtr joint = joints_[i]; + math::Angle angle = joint->GetAngle ( 0 ); + joint_state_.name[i] = joint->GetName(); + joint_state_.position[i] = angle.Radian () ; + } + joint_state_publisher_.publish ( joint_state_ ); +} + +void GazeboRosDiffDrive::publishWheelTF() +{ + ros::Time current_time = ros::Time::now(); + for ( int i = 0; i < 2; i++ ) { + + std::string wheel_frame = gazebo_ros_->resolveTF(joints_[i]->GetChild()->GetName ()); + std::string wheel_parent_frame = gazebo_ros_->resolveTF(joints_[i]->GetParent()->GetName ()); + + math::Pose poseWheel = joints_[i]->GetChild()->GetRelativePose(); + + tf::Quaternion qt ( poseWheel.rot.x, poseWheel.rot.y, poseWheel.rot.z, poseWheel.rot.w ); + tf::Vector3 vt ( poseWheel.pos.x, poseWheel.pos.y, poseWheel.pos.z ); + + tf::Transform tfWheel ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( tfWheel, current_time, wheel_parent_frame, wheel_frame ) ); + } +} + +// Update the controller +void GazeboRosDiffDrive::UpdateChild() +{ + + /* force reset SetMaxForce since Joint::Reset reset MaxForce to zero at + https://bitbucket.org/osrf/gazebo/src/8091da8b3c529a362f39b042095e12c94656a5d1/gazebo/physics/Joint.cc?at=gazebo2_2.2.5#cl-331 + (this has been solved in https://bitbucket.org/osrf/gazebo/diff/gazebo/physics/Joint.cc?diff2=b64ff1b7b6ff&at=issue_964 ) + and Joint::Reset is called after ModelPlugin::Reset, so we need to set maxForce to wheel_torque other than GazeboRosDiffDrive::Reset + (this seems to be solved in https://bitbucket.org/osrf/gazebo/commits/ec8801d8683160eccae22c74bf865d59fac81f1e) + */ + for ( int i = 0; i < 2; i++ ) { + if ( fabs(wheel_torque -joints_[i]->GetMaxForce ( 0 )) > 1e-6 ) { + joints_[i]->SetMaxForce ( 0, wheel_torque ); + } + } + + + if ( odom_source_ == ENCODER ) UpdateOdometryEncoder(); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = ( current_time - last_update_time_ ).Double(); + + if ( seconds_since_last_update > update_period_ ) { + if (this->publish_tf_) publishOdometry ( seconds_since_last_update ); + if ( publishWheelTF_ ) publishWheelTF(); + if ( publishWheelJointState_ ) publishWheelJointState(); + + // Update robot in case new velocities have been requested + getWheelVelocities(); + + double current_speed[2]; + + current_speed[LEFT] = joints_[LEFT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + current_speed[RIGHT] = joints_[RIGHT]->GetVelocity ( 0 ) * ( wheel_diameter_ / 2.0 ); + + if ( wheel_accel == 0 || + ( fabs ( wheel_speed_[LEFT] - current_speed[LEFT] ) < 0.01 ) || + ( fabs ( wheel_speed_[RIGHT] - current_speed[RIGHT] ) < 0.01 ) ) { + //if max_accel == 0, or target speed is reached + joints_[LEFT]->SetVelocity ( 0, wheel_speed_[LEFT]/ ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetVelocity ( 0, wheel_speed_[RIGHT]/ ( wheel_diameter_ / 2.0 ) ); + } else { + if ( wheel_speed_[LEFT]>=current_speed[LEFT] ) + wheel_speed_instr_[LEFT]+=fmin ( wheel_speed_[LEFT]-current_speed[LEFT], wheel_accel * seconds_since_last_update ); + else + wheel_speed_instr_[LEFT]+=fmax ( wheel_speed_[LEFT]-current_speed[LEFT], -wheel_accel * seconds_since_last_update ); + + if ( wheel_speed_[RIGHT]>current_speed[RIGHT] ) + wheel_speed_instr_[RIGHT]+=fmin ( wheel_speed_[RIGHT]-current_speed[RIGHT], wheel_accel * seconds_since_last_update ); + else + wheel_speed_instr_[RIGHT]+=fmax ( wheel_speed_[RIGHT]-current_speed[RIGHT], -wheel_accel * seconds_since_last_update ); + + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[LEFT], wheel_speed_[LEFT]); + // ROS_INFO("actual wheel speed = %lf, issued wheel speed= %lf", current_speed[RIGHT],wheel_speed_[RIGHT]); + + joints_[LEFT]->SetVelocity ( 0,wheel_speed_instr_[LEFT] / ( wheel_diameter_ / 2.0 ) ); + joints_[RIGHT]->SetVelocity ( 0,wheel_speed_instr_[RIGHT] / ( wheel_diameter_ / 2.0 ) ); + } + last_update_time_+= common::Time ( update_period_ ); + } +} + +// Finalize the controller +void GazeboRosDiffDrive::FiniChild() +{ + alive_ = false; + queue_.clear(); + queue_.disable(); + gazebo_ros_->node()->shutdown(); + callback_queue_thread_.join(); +} + +void GazeboRosDiffDrive::getWheelVelocities() +{ + boost::mutex::scoped_lock scoped_lock ( lock ); + + double vr = x_; + double va = rot_; + + wheel_speed_[LEFT] = vr + va * wheel_separation_ / 2.0; + wheel_speed_[RIGHT] = vr - va * wheel_separation_ / 2.0; +} + +void GazeboRosDiffDrive::cmdVelCallback ( const geometry_msgs::Twist::ConstPtr& cmd_msg ) +{ + boost::mutex::scoped_lock scoped_lock ( lock ); + x_ = cmd_msg->linear.x; + rot_ = cmd_msg->angular.z; +} + +void GazeboRosDiffDrive::QueueThread() +{ + static const double timeout = 0.01; + + while ( alive_ && gazebo_ros_->node()->ok() ) { + queue_.callAvailable ( ros::WallDuration ( timeout ) ); + } +} + +void GazeboRosDiffDrive::UpdateOdometryEncoder() +{ + double vl = joints_[LEFT]->GetVelocity ( 0 ); + double vr = joints_[RIGHT]->GetVelocity ( 0 ); + common::Time current_time = parent->GetWorld()->GetSimTime(); + double seconds_since_last_update = ( current_time - last_odom_update_ ).Double(); + last_odom_update_ = current_time; + + double b = wheel_separation_; + + // Book: Sigwart 2011 Autonompus Mobile Robots page:337 + double sl = vl * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double sr = vr * ( wheel_diameter_ / 2.0 ) * seconds_since_last_update; + double theta = ( sl - sr ) /b; + + + double dx = ( sl + sr ) /2.0 * cos ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dy = ( sl + sr ) /2.0 * sin ( pose_encoder_.theta + ( sl - sr ) / ( 2.0*b ) ); + double dtheta = ( sl - sr ) /b; + + pose_encoder_.x += dx; + pose_encoder_.y += dy; + pose_encoder_.theta += dtheta; + + double w = dtheta/seconds_since_last_update; + double v = sqrt ( dx*dx+dy*dy ) /seconds_since_last_update; + + tf::Quaternion qt; + tf::Vector3 vt; + qt.setRPY ( 0,0,pose_encoder_.theta ); + vt = tf::Vector3 ( pose_encoder_.x, pose_encoder_.y, 0 ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + odom_.twist.twist.angular.z = w; + odom_.twist.twist.linear.x = dx/seconds_since_last_update; + odom_.twist.twist.linear.y = dy/seconds_since_last_update; +} + +void GazeboRosDiffDrive::publishOdometry ( double step_time ) +{ + + ros::Time current_time = ros::Time::now(); + std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ ); + std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ ); + + tf::Quaternion qt; + tf::Vector3 vt; + + if ( odom_source_ == ENCODER ) { + // getting data form encoder integration + qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w ); + vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z ); + + } + if ( odom_source_ == WORLD ) { + // getting data form gazebo world + math::Pose pose = parent->GetWorldPose(); + qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w ); + vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z ); + + odom_.pose.pose.position.x = vt.x(); + odom_.pose.pose.position.y = vt.y(); + odom_.pose.pose.position.z = vt.z(); + + odom_.pose.pose.orientation.x = qt.x(); + odom_.pose.pose.orientation.y = qt.y(); + odom_.pose.pose.orientation.z = qt.z(); + odom_.pose.pose.orientation.w = qt.w(); + + // get velocity in /odom frame + math::Vector3 linear; + linear = parent->GetWorldLinearVel(); + odom_.twist.twist.angular.z = parent->GetWorldAngularVel().z; + + // convert velocity to child_frame_id (aka base_footprint) + float yaw = pose.rot.GetYaw(); + odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y; + odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x; + } + + tf::Transform base_footprint_to_odom ( qt, vt ); + transform_broadcaster_->sendTransform ( + tf::StampedTransform ( base_footprint_to_odom, current_time, + odom_frame, base_footprint_frame ) ); + + + // set covariance + 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; + + + // set header + odom_.header.stamp = current_time; + odom_.header.frame_id = odom_frame; + odom_.child_frame_id = base_footprint_frame; + + odometry_publisher_.publish ( odom_ ); +} + +GZ_REGISTER_MODEL_PLUGIN ( GazeboRosDiffDrive ) +} + diff --git a/tetris_gazebo/src/gazebo_ros_diff_drive.h b/tetris_gazebo/src/gazebo_ros_diff_drive.h new file mode 100644 index 0000000..fece866 --- /dev/null +++ b/tetris_gazebo/src/gazebo_ros_diff_drive.h @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the nor the + * names of its contributors may be used to endorse or promote products + * derived from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY Antons Rebguns ''AS IS'' AND ANY + * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns BE LIABLE FOR ANY + * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND + * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS + * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + **/ + +/* + * \file gazebo_ros_diff_drive.h + * + * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin + * developed for the erratic robot (see copyright notice above). The original + * plugin can be found in the ROS package gazebo_erratic_plugins. + * + * \author Piyush Khandelwal (piyushk@gmail.com) + * + * $ Id: 06/21/2013 11:23:40 AM piyushk $ + */ + +#ifndef DIFFDRIVE_PLUGIN_HH +#define DIFFDRIVE_PLUGIN_HH + +#include + +// Gazebo +#include +#include +#include + +// ROS +#include +#include +#include +#include +#include +#include +#include + +// Custom Callback Queue +#include +#include + +// Boost +#include +#include + +namespace gazebo { + + class Joint; + class Entity; + + class GazeboRosDiffDrive : public ModelPlugin { + + enum OdomSource + { + ENCODER = 0, + WORLD = 1, + }; + public: + GazeboRosDiffDrive(); + ~GazeboRosDiffDrive(); + void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); + void Reset(); + + protected: + virtual void UpdateChild(); + virtual void FiniChild(); + + private: + void publishOdometry(double step_time); + void getWheelVelocities(); + void publishWheelTF(); /// publishes the wheel tf's + void publishWheelJointState(); + void UpdateOdometryEncoder(); + + + GazeboRosPtr gazebo_ros_; + physics::ModelPtr parent; + event::ConnectionPtr update_connection_; + + double wheel_separation_; + double wheel_diameter_; + double wheel_torque; + double wheel_speed_[2]; + double wheel_accel; + double wheel_speed_instr_[2]; + + std::vector joints_; + + // ROS STUFF + ros::Publisher odometry_publisher_; + ros::Subscriber cmd_vel_subscriber_; + boost::shared_ptr transform_broadcaster_; + sensor_msgs::JointState joint_state_; + ros::Publisher joint_state_publisher_; + nav_msgs::Odometry odom_; + std::string tf_prefix_; + + boost::mutex lock; + + std::string robot_namespace_; + std::string command_topic_; + std::string odometry_topic_; + std::string odometry_frame_; + std::string robot_base_frame_; + bool publish_tf_; + // Custom Callback Queue + ros::CallbackQueue queue_; + boost::thread callback_queue_thread_; + void QueueThread(); + + // DiffDrive stuff + void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); + + double x_; + double rot_; + bool alive_; + + // Update Rate + double update_rate_; + double update_period_; + common::Time last_update_time_; + + OdomSource odom_source_; + geometry_msgs::Pose2D pose_encoder_; + common::Time last_odom_update_; + + // Flags + bool publishWheelTF_; + bool publishWheelJointState_; + + }; + +} + +#endif +