joint_position_to_joint_trajectory.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2015-2017, the ypspur_ros authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <ros/ros.h>
31 
32 #include <sensor_msgs/JointState.h>
33 #include <trajectory_msgs/JointTrajectory.h>
34 #include <ypspur_ros/JointPositionControl.h>
35 
36 #include <map>
37 #include <string>
38 
39 #include <compatibility.h>
40 
42 {
43 private:
49 
50  std::map<std::string, double> state_;
51 
52  double accel_;
53  bool skip_same_;
54 
55  void cbJointState(const sensor_msgs::JointState::ConstPtr& msg)
56  {
57  for (size_t i = 0; i < msg->name.size(); i++)
58  {
59  state_[msg->name[i]] = msg->position[i];
60  }
61  }
62  ypspur_ros::JointPositionControl cmd_prev;
63  void cbJointPosition(const ypspur_ros::JointPositionControl::ConstPtr& msg)
64  {
65  while (true)
66  {
67  if (msg->joint_names.size() != cmd_prev.joint_names.size())
68  break;
69  {
70  bool eq = true;
71  for (unsigned int i = 0; i < msg->joint_names.size(); i++)
72  {
73  if (msg->joint_names[i].compare(cmd_prev.joint_names[i]) != 0)
74  eq = false;
75  }
76  if (!eq)
77  break;
78  }
79  if (msg->positions.size() != cmd_prev.positions.size())
80  break;
81  {
82  bool eq = true;
83  for (unsigned int i = 0; i < msg->positions.size(); i++)
84  {
85  if (msg->positions[i] != cmd_prev.positions[i])
86  eq = false;
87  }
88  if (!eq)
89  break;
90  }
91  if (msg->velocities.size() != cmd_prev.velocities.size())
92  break;
93  {
94  bool eq = true;
95  for (unsigned int i = 0; i < msg->velocities.size(); i++)
96  {
97  if (msg->velocities[i] != cmd_prev.velocities[i])
98  eq = false;
99  }
100  if (!eq)
101  break;
102  }
103  if (msg->accelerations.size() != cmd_prev.accelerations.size())
104  break;
105  {
106  bool eq = true;
107  for (unsigned int i = 0; i < msg->accelerations.size(); i++)
108  {
109  if (msg->accelerations[i] != cmd_prev.accelerations[i])
110  eq = false;
111  }
112  if (!eq)
113  break;
114  }
115  return;
116  }
117  cmd_prev = *msg;
118 
119  trajectory_msgs::JointTrajectory cmd;
120  cmd.header = msg->header;
121  cmd.joint_names = msg->joint_names;
122  cmd.points.resize(1);
123  cmd.points[0].velocities.resize(msg->positions.size());
124  cmd.points[0].positions = msg->positions;
125  cmd.points[0].accelerations.resize(msg->positions.size());
126 
127  float t_max = 0;
128  int i = 0;
129  for (auto& p : msg->positions)
130  {
131  float t = fabs(p - state_[msg->joint_names[i]]) / msg->velocities[i];
132  if (t_max < t)
133  t_max = t;
134 
135  i++;
136  }
137  cmd.points[0].time_from_start = ros::Duration(t_max);
138 
139  pub_joint_.publish(cmd);
140  }
141 
142 public:
144  : nh_()
145  , pnh_("~")
146  {
147  sub_joint_ = compat::subscribe(
148  nh_, "joint_position",
149  pnh_, "joint_position", 5, &ConvertNode::cbJointPosition, this);
150  sub_joint_state_ = compat::subscribe(
151  nh_, "joint_states",
152  pnh_, "joint", 5, &ConvertNode::cbJointState, this);
153  pub_joint_ = compat::advertise<trajectory_msgs::JointTrajectory>(
154  nh_, "joint_trajectory",
155  pnh_, "joint_trajectory", 5, false);
156 
157  pnh_.param("accel", accel_, 0.3);
158  pnh_.param("skip_same", skip_same_, true);
159  }
160 };
161 
162 int main(int argc, char* argv[])
163 {
164  ros::init(argc, argv, "joint_position_to_joint_trajectory");
165 
166  ConvertNode conv;
167  ros::spin();
168 
169  return 0;
170 }
std::map< std::string, double > state_
int main(int argc, char *argv[])
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
void cbJointState(const sensor_msgs::JointState::ConstPtr &msg)
ROSCPP_DECL void spin(Spinner &spinner)
void cbJointPosition(const ypspur_ros::JointPositionControl::ConstPtr &msg)
ypspur_ros::JointPositionControl cmd_prev
double p(YPSpur_param id, enum motor_id motor)
ros::Subscriber subscribe(ros::NodeHandle &nh_new, const std::string &topic_new, ros::NodeHandle &nh_old, const std::string &topic_old, uint32_t queue_size, void(T::*fp)(M) const, T *obj, const ros::TransportHints &transport_hints=ros::TransportHints())
Definition: compatibility.h:98


ypspur_ros
Author(s): Atsushi Watanabe
autogenerated on Thu Jun 6 2019 19:41:30