gazebo_rsv_balance.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Copyright (c) 2015 Robosavvy Ltd.
3  * Author: Vitor Matos
4  *
5  *********************************************************************/
6 
7 #ifndef GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H
8 #define GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H
9 
10 #include <string>
11 #include <vector>
12 #include <map>
13 
14 #include <ros/ros.h>
15 #include <ros/callback_queue.h>
16 
18 #include <tf/transform_listener.h>
19 
20 #include <gazebo/common/common.hh>
21 #include <gazebo/physics/physics.hh>
23 
24 #include <std_msgs/Float64.h>
25 #include <sensor_msgs/JointState.h>
26 #include <nav_msgs/Odometry.h>
27 #include <geometry_msgs/Twist.h>
28 
29 #include <std_srvs/Empty.h>
30 
31 #include <rsv_balance_msgs/State.h>
32 #include <rsv_balance_msgs/SetMode.h>
33 #include <rsv_balance_msgs/SetInput.h>
34 
36 
37 namespace gazebo
38 {
39 
40 class Joint;
41 class Entity;
45 class GazeboRsvBalance: public ModelPlugin
46 {
47  enum
48  {
51  };
53  {
54  ENCODER = 0,
55  WORLD = 1
56  };
57  enum Mode
58  {
59  PARK = rsv_balance_msgs::SetModeRequest::PARK,
60  TRACTOR = rsv_balance_msgs::SetModeRequest::TRACTOR,
61  BALANCE = rsv_balance_msgs::SetModeRequest::BALANCE
62  };
63  public:
66  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
67  void Reset();
68 
69  protected:
70  virtual void UpdateChild();
71  virtual void FiniChild();
72 
73  private:
74  // Gazebo information
76  physics::ModelPtr parent_;
77  sdf::ElementPtr sdf_;
78  event::ConnectionPtr update_connection_;
79 
80  // Plugin
81  bool alive_;
83  boost::thread callback_queue_thread_;
84  void QueueThread();
85 
86  // Publishers and subscribers
87  std::string command_topic_;
88  std::string odom_topic_;
99 
100  // Node callbacks
101  void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
102  void cmdTiltCallback(const std_msgs::Float64::ConstPtr& cmd_tilt);
103  bool setMode(rsv_balance_msgs::SetMode::Request &req);
104  bool setInput(rsv_balance_msgs::SetInput::Request &req);
105  bool resetOverride(std_srvs::Empty::Request &req);
106  bool resetOdom(std_srvs::Empty::Request &req);
107 
108  // Node configurations
111  std::string base_frame_id_;
112  std::string odom_frame_id_;
117 
120 
121  std::map<std::string, Mode> mode_map_;
123 
124  // Robot
125  std::vector<physics::JointPtr> joints_;
126  double x_desired_;
127  double rot_desired_;
129  double imu_pitch_;
130  double imu_dpitch_;
131  double feedback_v_;
132  double feedback_w_;
133  math::Vector3 odom_offset_pos_;
134  math::Vector3 odom_offset_rot_;
135 
136  void resetVariables();
137  nav_msgs::Odometry odom_;
138  void updateIMU();
139  void updateOdometry();
140  void resetOdometry();
141  void publishOdometry();
142  void publishWheelJointState();
143 
144  // Control
145  double update_rate_;
147  common::Time last_update_time_;
149  double *u_control_;
150 };
151 
152 } // namespace gazebo
153 
154 #endif // GAZEBO_RSV_BALANCE_GAZEBO_RSV_BALANCE_H
155 
std::map< std::string, Mode > mode_map_
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
bool setMode(rsv_balance_msgs::SetMode::Request &req)
Sets platform operating mode.
boost::thread callback_queue_thread_
ros::ServiceServer reset_odom_server_
void updateOdometry()
Updates odometry, from Gazebo world or from encoders.
void resetOdometry()
Resets odometry by adding offset to WORLD odometry, and resetting odometry values.
ros::ServiceServer set_input_server_
ros::Subscriber cmd_tilt_subscriber_
ros::ServiceServer reset_override_server_
ros::Subscriber cmd_vel_subscriber_
bool resetOverride(std_srvs::Empty::Request &req)
Just exposes service. Not used in simulation.
void Reset()
Called when Gazebo resets world.
void publishOdometry()
Publishes odometry and desired tfs.
ros::Publisher odometry_publisher_
event::ConnectionPtr update_connection_
ros::ServiceServer set_mode_server_
bool setInput(rsv_balance_msgs::SetInput::Request &req)
Just exposes service. Not used in simulation.
ros::CallbackQueue queue_
void publishWheelJointState()
Publishes wheel joint_states.
void resetVariables()
Resets simulation variables.
void cmdTiltCallback(const std_msgs::Float64::ConstPtr &cmd_tilt)
Callback to cmd_tilt.
ros::Publisher joint_state_publisher_
void updateIMU()
Gets pitch angle values directly from Gazebo world.
std::vector< physics::JointPtr > joints_
balance_control::BalanceControl state_control_
virtual void UpdateChild()
Gazebo step update.
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
Callback to cmd_vel.
bool resetOdom(std_srvs::Empty::Request &req)
Service to reset odometry.
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
virtual void FiniChild()
Called by gazebo upon exiting.


rsv_balance_gazebo
Author(s): Vitor Matos
autogenerated on Mon Jun 10 2019 15:06:42