00001 /********************************************************************* 00002 * Copyright (c) 2015 Robosavvy Ltd. 00003 * Author: Vitor Matos 00004 * 00005 *********************************************************************/ 00006 00007 #ifndef GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H 00008 #define GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H 00009 00010 #include <string> 00011 #include <vector> 00012 #include <map> 00013 00014 #include <ros/ros.h> 00015 #include <ros/callback_queue.h> 00016 00017 #include <tf/transform_broadcaster.h> 00018 #include <tf/transform_listener.h> 00019 00020 #include <gazebo/common/common.hh> 00021 #include <gazebo/physics/physics.hh> 00022 #include <gazebo_plugins/gazebo_ros_utils.h> 00023 00024 #include <std_msgs/Float64.h> 00025 #include <sensor_msgs/JointState.h> 00026 #include <nav_msgs/Odometry.h> 00027 #include <geometry_msgs/Twist.h> 00028 00029 #include <std_srvs/Empty.h> 00030 00031 #include <rsv_balance_msgs/State.h> 00032 #include <rsv_balance_msgs/SetMode.h> 00033 #include <rsv_balance_msgs/SetInput.h> 00034 00035 #include "rsv_balance_gazebo_control/balance_gazebo_control.h" 00036 00037 namespace gazebo 00038 { 00039 00040 class Joint; 00041 class Entity; 00045 class GazeboRsvBalance: public ModelPlugin 00046 { 00047 enum 00048 { 00049 RIGHT, 00050 LEFT 00051 }; 00052 enum OdomSource 00053 { 00054 ENCODER = 0, 00055 WORLD = 1 00056 }; 00057 enum Mode 00058 { 00059 PARK = rsv_balance_msgs::SetModeRequest::PARK, 00060 TRACTOR = rsv_balance_msgs::SetModeRequest::TRACTOR, 00061 BALANCE = rsv_balance_msgs::SetModeRequest::BALANCE 00062 }; 00063 public: 00064 GazeboRsvBalance(); 00065 ~GazeboRsvBalance(); 00066 void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 00067 void Reset(); 00068 00069 protected: 00070 virtual void UpdateChild(); 00071 virtual void FiniChild(); 00072 00073 private: 00074 // Gazebo information 00075 GazeboRosPtr gazebo_ros_; 00076 physics::ModelPtr parent_; 00077 sdf::ElementPtr sdf_; 00078 event::ConnectionPtr update_connection_; 00079 00080 // Plugin 00081 bool alive_; 00082 ros::CallbackQueue queue_; 00083 boost::thread callback_queue_thread_; 00084 void QueueThread(); 00085 00086 // Publishers and subscribers 00087 std::string command_topic_; 00088 std::string odom_topic_; 00089 ros::Publisher odometry_publisher_; 00090 ros::Publisher state_publisher_; 00091 ros::Publisher joint_state_publisher_; 00092 ros::Subscriber cmd_vel_subscriber_; 00093 ros::Subscriber cmd_tilt_subscriber_; 00094 ros::ServiceServer set_mode_server_; 00095 ros::ServiceServer set_input_server_; 00096 ros::ServiceServer reset_override_server_; 00097 ros::ServiceServer reset_odom_server_; 00098 boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_; 00099 00100 // Node callbacks 00101 void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); 00102 void cmdTiltCallback(const std_msgs::Float64::ConstPtr& cmd_tilt); 00103 bool setMode(rsv_balance_msgs::SetMode::Request &req); 00104 bool setInput(rsv_balance_msgs::SetInput::Request &req); 00105 bool resetOverride(std_srvs::Empty::Request &req); 00106 bool resetOdom(std_srvs::Empty::Request &req); 00107 00108 // Node configurations 00109 bool publish_odom_tf_; 00110 bool publish_wheel_joint_; 00111 std::string base_frame_id_; 00112 std::string odom_frame_id_; 00113 OdomSource odom_source_; 00114 bool publish_state_; 00115 double publish_state_rate_; 00116 double publish_diagnostics_rate_; 00117 00118 double wheel_separation_; 00119 double wheel_radius_; 00120 00121 std::map<std::string, Mode> mode_map_; 00122 Mode current_mode_; 00123 00124 // Robot 00125 std::vector<physics::JointPtr> joints_; 00126 double x_desired_; 00127 double rot_desired_; 00128 double tilt_desired_; 00129 double imu_pitch_; 00130 double imu_dpitch_; 00131 double feedback_v_; 00132 double feedback_w_; 00133 math::Vector3 odom_offset_pos_; 00134 math::Vector3 odom_offset_rot_; 00135 00136 void resetVariables(); 00137 nav_msgs::Odometry odom_; 00138 void updateIMU(); 00139 void updateOdometry(); 00140 void resetOdometry(); 00141 void publishOdometry(); 00142 void publishWheelJointState(); 00143 00144 // Control 00145 double update_rate_; 00146 double update_period_; 00147 common::Time last_update_time_; 00148 balance_control::BalanceControl state_control_; 00149 double *u_control_; 00150 }; 00151 00152 } // namespace gazebo 00153 00154 #endif // GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H 00155