four_wheel_steering.h
Go to the documentation of this file.
1 // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials
2 
3 // ROS
4 #include <ros/ros.h>
5 #include <std_msgs/Float64.h>
6 #include <std_srvs/Empty.h>
7 
8 // ros_control
9 #include <controller_manager/controller_manager.h>
14 
15 // NaN
16 #include <limits>
17 
18 // ostringstream
19 #include <sstream>
20 
22 {
23 public:
25  : running_(true)
28  {
29  std::vector<std::string> velocity_joints_name = {"front_left_wheel", "front_right_wheel",
30  "rear_left_wheel", "rear_right_wheel"};
31  // Connect and register the joint state and velocity interface
32  for (unsigned int i = 0; i < velocity_joints_name.size(); ++i)
33  {
34 
35  hardware_interface::JointStateHandle state_handle(velocity_joints_name[i], &joints_[i].position, &joints_[i].velocity, &joints_[i].effort);
37 
38  hardware_interface::JointHandle vel_handle(state_handle, &joints_[i].velocity_command);
40  }
41 
42  std::vector<std::string> position_joints_name = {"front_left_steering_joint", "front_right_steering_joint",
43  "rear_left_steering_joint", "rear_right_steering_joint"};
44  // Connect and register the joint state and position interface
45  for (unsigned int i = 0; i < position_joints_name.size(); ++i)
46  {
47  hardware_interface::JointStateHandle state_handle(position_joints_name[i], &steering_joints_[i].position, &steering_joints_[i].velocity, &steering_joints_[i].effort);
49 
50  hardware_interface::JointHandle pos_handle(state_handle, &steering_joints_[i].position_command);
52  }
53 
57  }
58 
59  ros::Time getTime() const {return ros::Time::now();}
60  ros::Duration getPeriod() const {return ros::Duration(0.01);}
61 
62  void read()
63  {
64  std::ostringstream os;
65  for (unsigned int i = 0; i < 3; ++i)
66  {
67  os << joints_[i].velocity_command << ", ";
68  }
69  os << joints_[3].velocity_command;
70 
71  ROS_DEBUG_STREAM("Commands for joints: " << os.str());
72 
73  os.str("");
74  for (unsigned int i = 0; i < 3; ++i)
75  {
76  os << steering_joints_[i].position_command << ", ";
77  }
79  ROS_DEBUG_STREAM("Commands for steering joints: " << os.str());
80  }
81 
82  void write()
83  {
84  if (running_)
85  {
86  for (unsigned int i = 0; i < 4; ++i)
87  {
88  // Note that joints_[i].position will be NaN for one more cycle after we start(),
89  // but that is consistent with the knowledge we have about the state
90  // of the robot.
91  joints_[i].position += joints_[i].velocity*getPeriod().toSec(); // update position
92  joints_[i].velocity = joints_[i].velocity_command; // might add smoothing here later
93  }
94  for (unsigned int i = 0; i < 4; ++i)
95  {
96  steering_joints_[i].position = steering_joints_[i].position_command; // might add smoothing here later
97  }
98  }
99  else
100  {
101  for (unsigned int i = 0; i < 4; ++i)
102  {
103  joints_[i].position = std::numeric_limits<double>::quiet_NaN();
104  joints_[i].velocity = std::numeric_limits<double>::quiet_NaN();
105  }
106  for (unsigned int i = 0; i < 4; ++i)
107  {
108  steering_joints_[i].position = std::numeric_limits<double>::quiet_NaN();
109  steering_joints_[i].velocity = std::numeric_limits<double>::quiet_NaN();
110  }
111  }
112  }
113 
114  bool start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
115  {
116  running_ = true;
117  return true;
118  }
119 
120  bool stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
121  {
122  running_ = false;
123  return true;
124  }
125 
126 private:
130 
131  struct Joint
132  {
133  double position;
134  double velocity;
135  double effort;
137 
138  Joint() : position(0), velocity(0), effort(0), velocity_command(0) { }
139  } joints_[4];
140 
142  {
143  double position;
144  double velocity;
145  double effort;
147 
148  SteeringJoint() : position(0), velocity(0), effort(0), position_command(0) { }
149  } steering_joints_[4];
150  bool running_;
151 
155 };
struct FourWheelSteering::Joint joints_[4]
ros::ServiceServer stop_srv_
ros::Time getTime() const
ros::Duration getPeriod() const
ServiceServer advertiseService(ros::NodeHandle n, std::string name, boost::function< bool(const typename ActionSpec::_action_goal_type::_goal_type &, typename ActionSpec::_action_result_type::_result_type &result)> service_cb)
bool start_callback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
bool stop_callback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
struct FourWheelSteering::SteeringJoint steering_joints_[4]
#define ROS_DEBUG_STREAM(args)
hardware_interface::VelocityJointInterface jnt_vel_interface_
ros::ServiceServer start_srv_
void registerHandle(const JointStateHandle &handle)
hardware_interface::PositionJointInterface jnt_pos_interface_
static Time now()
hardware_interface::JointStateInterface jnt_state_interface_


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