servo_state_publisher.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2015 Vladimir Ermakov
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <ros/ros.h>
15 #include <ros/console.h>
16 
17 #include <urdf/model.h>
18 #include <mavros_msgs/RCOut.h>
19 #include <sensor_msgs/JointState.h>
20 
22 public:
23  std::string joint_name;
24  float joint_lower;
25  float joint_upper;
26 
27  size_t rc_channel;
28 
29  uint16_t rc_min;
30  uint16_t rc_max;
31  uint16_t rc_trim;
32  uint16_t rc_dz;
33  bool rc_rev;
34 
36  joint_name{},
37  joint_lower(-M_PI/4),
38  joint_upper(M_PI/4),
39  rc_channel(0),
40  rc_min(1000),
41  rc_max(2000),
42  rc_trim(1500),
43  rc_dz(0),
44  rc_rev(false)
45  { };
46 
47  ServoDescription(std::string joint_name_, double lower_, double upper_,
48  int channel_,
49  int min_, int max_, int trim_, int dz_,
50  bool rev_) :
51  joint_name(joint_name_),
52  joint_lower(lower_),
53  joint_upper(upper_),
54  rc_channel(channel_),
55  rc_min(min_),
56  rc_max(max_),
57  rc_trim(trim_),
58  rc_dz(dz_),
59  rc_rev(rev_)
60  { };
61 
66  inline float normalize(uint16_t pwm) {
67  // 1) fix bounds
68  pwm = std::max(pwm, rc_min);
69  pwm = std::min(pwm, rc_max);
70 
71  // 2) scale around mid point
72  float chan;
73  if (pwm > (rc_trim + rc_dz)) {
74  chan = (pwm - rc_trim - rc_dz) / (float)(rc_max - rc_trim - rc_dz);
75  }
76  else if (pwm < (rc_trim - rc_dz)) {
77  chan = (pwm - rc_trim + rc_dz) / (float)(rc_trim - rc_min - rc_dz);
78  }
79  else {
80  chan = 0.0;
81  }
82 
83  if (rc_rev)
84  chan *= -1;
85 
86  if (!std::isfinite(chan)) {
87  ROS_DEBUG("SSP: not finite result in RC%zu channel normalization!", rc_channel);
88  chan = 0.0;
89  }
90 
91  return chan;
92  }
93 
94  float calculate_position(uint16_t pwm) {
95  float channel = normalize(pwm);
96 
97  // not sure should i differently map -1..0 and 0..1
98  // for now there arduino map() (explicit)
99  float position = (channel + 1.0) * (joint_upper - joint_lower) / (1.0 + 1.0) + joint_lower;
100 
101  return position;
102  }
103 };
104 
106 public:
108  nh()
109  {
110  ros::NodeHandle priv_nh("~");
111 
112  XmlRpc::XmlRpcValue param_dict;
113  priv_nh.getParam("", param_dict);
114 
116 
117  urdf::Model model;
118  model.initParam("robot_description");
119  ROS_INFO("SSP: URDF robot: %s", model.getName().c_str());
120 
121  for (auto &pair : param_dict) {
122  ROS_DEBUG("SSP: Loading joint: %s", pair.first.c_str());
123 
124  // inefficient, but easier to program
125  ros::NodeHandle pnh(priv_nh, pair.first);
126 
127  bool rc_rev;
129 
130  if (!pnh.getParam("rc_channel", rc_channel)) {
131  ROS_ERROR("SSP: '%s' should provice rc_channel", pair.first.c_str());
132  continue;
133  }
134 
135  pnh.param("rc_min", rc_min, 1000);
136  pnh.param("rc_max", rc_max, 2000);
137  if (!pnh.getParam("rc_trim", rc_trim)) {
138  rc_trim = rc_min + (rc_max - rc_min) / 2;
139  }
140 
141  pnh.param("rc_dz", rc_dz, 0);
142  pnh.param("rc_rev", rc_rev, false);
143 
144  auto joint = model.getJoint(pair.first);
145  if (!joint) {
146  ROS_ERROR("SSP: URDF: there no joint '%s'", pair.first.c_str());
147  continue;
148  }
149  if (!joint->limits) {
150  ROS_ERROR("SSP: URDF: joint '%s' should provide <limit>", pair.first.c_str());
151  continue;
152  }
153 
154  double lower = joint->limits->lower;
155  double upper = joint->limits->upper;
156 
157  servos.emplace_back(pair.first, lower, upper, rc_channel, rc_min, rc_max, rc_trim, rc_dz, rc_rev);
158  ROS_INFO("SSP: joint '%s' (RC%d) loaded", pair.first.c_str(), rc_channel);
159  }
160 
161  rc_out_sub = nh.subscribe("rc_out", 10, &ServoStatePublisher::rc_out_cb, this);
162  joint_states_pub = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
163  }
164 
165  void spin() {
166  if (servos.empty()) {
167  ROS_WARN("SSP: there nothing to do, exiting");
168  return;
169  }
170 
171  ROS_INFO("SSP: Initialization done. %zu joints served", servos.size());
172  ros::spin();
173  }
174 
175 private:
179 
180  std::list<ServoDescription> servos;
181 
183  if (msg->channels.empty())
184  return; // nothing to do
185 
186  auto states = boost::make_shared<sensor_msgs::JointState>();
187  states->header.stamp = msg->header.stamp;
188 
189  for (auto &desc : servos) {
190  if (!(desc.rc_channel != 0 && desc.rc_channel <= msg->channels.size()))
191  continue; // prevent crash on servos not in that message
192 
193  uint16_t pwm = msg->channels[desc.rc_channel - 1];
194  if (pwm == 0 || pwm == UINT16_MAX)
195  continue; // exclude unset channels
196 
197  states->name.emplace_back(desc.joint_name);
198  states->position.emplace_back(desc.calculate_position(pwm));
199  }
200 
201  joint_states_pub.publish(states);
202  }
203 };
204 
205 int main(int argc, char *argv[])
206 {
207  ros::init(argc, argv, "servo_state_publisher");
208 
209  ServoStatePublisher state_publisher;
210  state_publisher.spin();
211  return 0;
212 }
213 
std::shared_ptr< MAVConnInterface const > ConstPtr
int main(int argc, char *argv[])
upper
ros::NodeHandle nh
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
float normalize(uint16_t pwm)
ServoDescription(std::string joint_name_, double lower_, double upper_, int channel_, int min_, int max_, int trim_, int dz_, bool rev_)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
float calculate_position(uint16_t pwm)
Type const & getType() const
#define ROS_WARN(...)
ROSCPP_DECL void spin(Spinner &spinner)
#define ROS_INFO(...)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
URDF_EXPORT bool initParam(const std::string &param)
lower
bool getParam(const std::string &key, std::string &s) const
std::list< ServoDescription > servos
void rc_out_cb(const mavros_msgs::RCOut::ConstPtr &msg)
#define ROS_ASSERT(cond)
#define ROS_ERROR(...)
#define ROS_DEBUG(...)


mavros_extras
Author(s): Vladimir Ermakov , Amilcar Lucas
autogenerated on Tue Jun 1 2021 02:36:36