diff --git a/tetris_description/urdf/tetris.gazebo.xacro b/tetris_description/urdf/tetris.gazebo.xacro index 0a0efbc..2e8631f 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 5f44f7f..6914af2 100644 --- a/tetris_gazebo/CMakeLists.txt +++ b/tetris_gazebo/CMakeLists.txt @@ -1,35 +1,10 @@ cmake_minimum_required(VERSION 2.8.3) project(tetris_gazebo) -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} -) - +find_package(catkin REQUIRED COMPONENTS) 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/src/gazebo_ros_diff_drive.cpp b/tetris_gazebo/src/gazebo_ros_diff_drive.cpp deleted file mode 100644 index 9d8178d..0000000 --- a/tetris_gazebo/src/gazebo_ros_diff_drive.cpp +++ /dev/null @@ -1,425 +0,0 @@ -/* - 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 deleted file mode 100644 index fece866..0000000 --- a/tetris_gazebo/src/gazebo_ros_diff_drive.h +++ /dev/null @@ -1,158 +0,0 @@ -/* - * 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 -