gazebo_ros_force_based_move.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Stefan Kohlbrecher, TU Darmstadt
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 /*
00019  * Desc: Simple model controller that uses a twist message to exert
00020  *       forces on a robot, resulting in motion. Based on the
00021  *       planar_move plugin by Piyush Khandelwal.
00022  * Author: Stefan Kohlbrecher
00023  * Date: 06 August 2015
00024  */
00025 
00026 #ifndef GAZEBO_ROS_FORCE_BASED_MOVE_HH
00027 #define GAZEBO_ROS_FORCE_BASED_MOVE_HH
00028 
00029 #include <boost/bind.hpp>
00030 #include <boost/thread.hpp>
00031 #include <map>
00032 
00033 #include <gazebo/common/common.hh>
00034 #include <gazebo/physics/physics.hh>
00035 #include <sdf/sdf.hh>
00036 
00037 #include <geometry_msgs/Twist.h>
00038 #include <nav_msgs/OccupancyGrid.h>
00039 #include <nav_msgs/Odometry.h>
00040 #include <ros/advertise_options.h>
00041 #include <ros/callback_queue.h>
00042 #include <ros/ros.h>
00043 #include <tf/transform_broadcaster.h>
00044 #include <tf/transform_listener.h>
00045 
00046 namespace gazebo {
00047 
00048   class GazeboRosForceBasedMove : public ModelPlugin {
00049 
00050     public: 
00051       GazeboRosForceBasedMove();
00052       ~GazeboRosForceBasedMove();
00053       void Load(physics::ModelPtr parent, sdf::ElementPtr sdf);
00054 
00055     protected: 
00056       virtual void UpdateChild();
00057       virtual void FiniChild();
00058 
00059     private:
00060       void publishOdometry(double step_time);
00061 
00062       tf::Transform getTransformForMotion(double linear_vel_x, double angular_vel, double timeSeconds) const;
00063 
00064       physics::ModelPtr parent_;
00065       event::ConnectionPtr update_connection_;
00066 
00068       physics::LinkPtr link_;
00069 
00071       private: std::string link_name_;
00072 
00073       boost::shared_ptr<ros::NodeHandle> rosnode_;
00074       ros::Publisher odometry_pub_;
00075       ros::Subscriber vel_sub_;
00076       boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
00077       nav_msgs::Odometry odom_;
00078       std::string tf_prefix_;
00079 
00080       tf::Transform odom_transform_;
00081 
00082       boost::mutex lock;
00083 
00084       std::string robot_namespace_;
00085       std::string command_topic_;
00086       std::string odometry_topic_;
00087       std::string odometry_frame_;
00088       std::string robot_base_frame_;
00089       double odometry_rate_;
00090       bool publish_odometry_tf_;
00091 
00092       // Custom Callback Queue
00093       ros::CallbackQueue queue_;
00094       boost::thread callback_queue_thread_;
00095       void QueueThread();
00096 
00097       // command velocity callback
00098       void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00099 
00100       double x_;
00101       double y_;
00102       double rot_;
00103       bool alive_;
00104       common::Time last_odom_publish_time_;
00105       math::Pose last_odom_pose_;
00106       
00107       double torque_yaw_velocity_p_gain_;
00108       double force_x_velocity_p_gain_;
00109       double force_y_velocity_p_gain_;
00110 
00111   };
00112 
00113 }
00114 
00115 #endif /* end of include guard: GAZEBO_ROS_PLANAR_MOVE_HH */


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Mon Jun 27 2016 04:58:09