four_wheel_steering_controller.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2017, Irstea
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Irstea nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
38 
39 #include <nav_msgs/Odometry.h>
40 #include <four_wheel_steering_msgs/FourWheelSteeringStamped.h>
41 #include <tf/tfMessage.h>
42 
45 
48 
50 
61  : public controller_interface::MultiInterfaceController<hardware_interface::VelocityJointInterface,
62  hardware_interface::PositionJointInterface>
63  {
64  public:
66 
73  bool init(hardware_interface::RobotHW* robot_hw,
74  ros::NodeHandle& root_nh,
75  ros::NodeHandle &controller_nh);
76 
82  void update(const ros::Time& time, const ros::Duration& period);
83 
88  void starting(const ros::Time& time);
89 
94  void stopping(const ros::Time& /*time*/);
95 
96  private:
97  std::string name_;
98 
103 
105  std::vector<hardware_interface::JointHandle> front_wheel_joints_;
106  std::vector<hardware_interface::JointHandle> rear_wheel_joints_;
107  std::vector<hardware_interface::JointHandle> front_steering_joints_;
108  std::vector<hardware_interface::JointHandle> rear_steering_joints_;
109 
111  struct Command
112  {
114 
115  Command() : stamp(0.0) {}
116  };
118  {
119  double lin_x;
120  double lin_y;
121  double ang;
122 
123  CommandTwist() : lin_x(0.0), lin_y(0.0), ang(0.0) {}
124  };
126  {
127  double lin;
130 
131  Command4ws() : lin(0.0), front_steering(0.0), rear_steering(0.0) {}
132  };
136 
141 
147 
149  double track_;
153 
156 
158  double wheel_base_;
159 
162 
164  std::string base_frame_id_;
165 
168 
171 
177 
178  private:
179 
184  void updateOdometry(const ros::Time &time);
190  void updateCommand(const ros::Time& time, const ros::Duration& period);
191 
195  void brake();
196 
201  void cmdVelCallback(const geometry_msgs::Twist& command);
202 
207  void cmdFourWheelSteeringCallback(const four_wheel_steering_msgs::FourWheelSteering &command);
208 
217  bool getWheelNames(ros::NodeHandle& controller_nh,
218  const std::string& wheel_param,
219  std::vector<std::string>& wheel_names);
220 
226  void setOdomPubFields(ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
227 
228  };
229 
231 } // namespace four_wheel_steering_controller
double cmd_vel_timeout_
Timeout to consider cmd_vel commands old:
void cmdVelCallback(const geometry_msgs::Twist &command)
Velocity command callback.
void updateCommand(const ros::Time &time, const ros::Duration &period)
Compute and publish command.
std::vector< hardware_interface::JointHandle > front_steering_joints_
std::vector< hardware_interface::JointHandle > rear_wheel_joints_
void update(const ros::Time &time, const ros::Duration &period)
Updates controller, i.e. computes the odometry and sets the new velocity commands.
void updateOdometry(const ros::Time &time)
Update and publish odometry.
bool init(hardware_interface::RobotHW *robot_hw, ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Initialize controller.
std::vector< hardware_interface::JointHandle > rear_steering_joints_
void cmdFourWheelSteeringCallback(const four_wheel_steering_msgs::FourWheelSteering &command)
Velocity and steering command callback.
The Odometry class handles odometry readings (2D pose and velocity with related timestamp) ...
Definition: odometry.h:52
bool enable_twist_cmd_
Whether the control is make with four_wheel_steering msg or twist msg:
double wheel_radius_
Wheel radius (assuming it&#39;s the same for the left and right wheels):
std::vector< hardware_interface::JointHandle > front_wheel_joints_
Hardware handles:
bool getWheelNames(ros::NodeHandle &controller_nh, const std::string &wheel_param, std::vector< std::string > &wheel_names)
Get the wheel names from a wheel param.
boost::shared_ptr< realtime_tools::RealtimePublisher< tf::tfMessage > > tf_odom_pub_
void setOdomPubFields(ros::NodeHandle &root_nh, ros::NodeHandle &controller_nh)
Sets the odometry publishing fields.
PLUGINLIB_EXPORT_CLASS(four_wheel_steering_controller::FourWheelSteeringController, controller_interface::ControllerBase)
void brake()
Brakes the wheels, i.e. sets the velocity to 0.
boost::shared_ptr< realtime_tools::RealtimePublisher< nav_msgs::Odometry > > odom_pub_
Odometry related:
boost::shared_ptr< realtime_tools::RealtimePublisher< four_wheel_steering_msgs::FourWheelSteeringStamped > > odom_4ws_pub_
double track_
Wheel separation (or track), distance between left and right wheels (from the midpoint of the wheel w...
double wheel_base_
Wheel base (distance between front and rear wheel):
realtime_tools::RealtimeBuffer< Command4ws > command_four_wheel_steering_
FourWheelSteering command related:


four_wheel_steering_controller
Author(s): Vincent Rousseau
autogenerated on Sat Apr 18 2020 03:58:13