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 */