joint_relay_handler.cpp
Go to the documentation of this file.
1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2011, Southwest Research Institute
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 are met:
9 *
10 * * Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * * Redistributions in binary form must reproduce the above copyright
13 * notice, this list of conditions and the following disclaimer in the
14 * documentation and/or other materials provided with the distribution.
15 * * Neither the name of the Southwest Research Institute, nor the names
16 * of its contributors may be used to endorse or promote products derived
17 * from this software without specific prior written permission.
18 *
19 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
20 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
21 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
22 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
23 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
24 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
25 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
26 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
27 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
28 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
29 * POSSIBILITY OF SUCH DAMAGE.
30 */
31 
32 #include <algorithm>
33 
36 
39 using namespace industrial::simple_message;
40 
42 {
43 namespace joint_relay_handler
44 {
45 
46 bool JointRelayHandler::init(SmplMsgConnection* connection, std::vector<std::string>& joint_names)
47 {
48  this->pub_joint_control_state_ =
49  this->node_.advertise<control_msgs::FollowJointTrajectoryFeedback>("feedback_states", 1);
50 
51  this->pub_joint_sensor_state_ = this->node_.advertise<sensor_msgs::JointState>("joint_states",1);
52 
53  // save "complete" joint-name list, preserving any blank entries for later use
54  this->all_joint_names_ = joint_names;
55 
56  return init((int)StandardMsgTypes::JOINT, connection);
57 }
58 
59 bool JointRelayHandler::internalCB(SimpleMessage& in)
60 {
61  JointMessage joint_msg;
62 
63  if (!joint_msg.init(in))
64  {
65  LOG_ERROR("Failed to initialize joint message");
66  return false;
67  }
68 
69  return internalCB(joint_msg);
70 }
71 
72 bool JointRelayHandler::internalCB(JointMessage& in)
73 {
74  control_msgs::FollowJointTrajectoryFeedback control_state;
75  sensor_msgs::JointState sensor_state;
76  bool rtn = true;
77 
78  if (create_messages(in, &control_state, &sensor_state))
79  {
80  this->pub_joint_control_state_.publish(control_state);
81  this->pub_joint_sensor_state_.publish(sensor_state);
82  }
83  else
84  rtn = false;
85 
86  // Reply back to the controller if the sender requested it.
88  {
89  SimpleMessage reply;
91  this->getConnection()->sendMsg(reply);
92  }
93 
94  return rtn;
95 }
96 
97 // TODO: Add support for other message fields (velocity, effort, desired pos)
98 bool JointRelayHandler::create_messages(JointMessage& msg_in,
99  control_msgs::FollowJointTrajectoryFeedback* control_state,
100  sensor_msgs::JointState* sensor_state)
101 {
102  // read joint positions from JointMessage
103  std::vector<double> all_joint_pos(all_joint_names_.size());
104  for (int i=0; i<all_joint_names_.size(); ++i)
105  {
106  shared_real value;
107  if (msg_in.getJoints().getJoint(i, value))
108  all_joint_pos[i] = value;
109  else
110  LOG_ERROR("Failed to parse #%d value from JointMessage", i);
111  }
112 
113  // apply transform to joint positions, if required
114  std::vector<double> xform_joint_pos;
115  if (!transform(all_joint_pos, &xform_joint_pos))
116  {
117  LOG_ERROR("Failed to transform joint positions");
118  return false;
119  }
120 
121  // select specific joints for publishing
122  std::vector<double> pub_joint_pos;
123  std::vector<std::string> pub_joint_names;
124  if (!select(xform_joint_pos, all_joint_names_, &pub_joint_pos, &pub_joint_names))
125  {
126  LOG_ERROR("Failed to select joints for publishing");
127  return false;
128  }
129 
130  // assign values to messages
131  control_msgs::FollowJointTrajectoryFeedback tmp_control_state; // always start with a "clean" message
132  tmp_control_state.header.stamp = ros::Time::now();
133  tmp_control_state.joint_names = pub_joint_names;
134  tmp_control_state.actual.positions = pub_joint_pos;
135  *control_state = tmp_control_state;
136 
137  sensor_msgs::JointState tmp_sensor_state;
138  tmp_sensor_state.header.stamp = ros::Time::now();
139  tmp_sensor_state.name = pub_joint_names;
140  tmp_sensor_state.position = pub_joint_pos;
141  *sensor_state = tmp_sensor_state;
142 
143  return true;
144 }
145 
146 bool JointRelayHandler::select(const std::vector<double>& all_joint_pos, const std::vector<std::string>& all_joint_names,
147  std::vector<double>* pub_joint_pos, std::vector<std::string>* pub_joint_names)
148 {
149  ROS_ASSERT(all_joint_pos.size() == all_joint_names.size());
150 
151  pub_joint_pos->clear();
152  pub_joint_names->clear();
153 
154  // skip over "blank" joint names
155  for (int i=0; i<all_joint_pos.size(); ++i)
156  {
157  if (all_joint_names[i].empty())
158  continue;
159 
160  pub_joint_pos->push_back(all_joint_pos[i]);
161  pub_joint_names->push_back(all_joint_names[i]);
162  }
163 
164  return true;
165 }
166 
167 }//namespace joint_relay_handler
168 }//namespace industrial_robot_client
169 
170 
171 
172 
industrial::joint_data::JointData::getJoint
industrial::shared_types::shared_real getJoint(industrial::shared_types::shared_int index) const
log_wrapper.h
industrial::simple_message::ReplyTypes::SUCCESS
SUCCESS
industrial::simple_message::StandardMsgTypes::JOINT
JOINT
LOG_ERROR
#define LOG_ERROR(format,...)
industrial::shared_types::shared_real
float shared_real
industrial::simple_message::SimpleMessage
industrial::typed_message::TypedMessage::toReply
virtual bool toReply(industrial::simple_message::SimpleMessage &msg, industrial::simple_message::ReplyType reply)
industrial::simple_message
industrial::joint_message::JointMessage
industrial::smpl_msg_connection::SmplMsgConnection
joint_relay_handler.h
industrial_robot_client
Definition: joint_relay_handler.h:46
industrial::simple_message::CommTypes::SERVICE_REQUEST
SERVICE_REQUEST
industrial::joint_message::JointMessage::getJoints
industrial::joint_data::JointData & getJoints()
init
void init(const M_string &remappings)
industrial::simple_message::ReplyTypes::FAILURE
FAILURE
ROS_ASSERT
#define ROS_ASSERT(cond)
industrial::joint_message::JointMessage::init
void init()
industrial::typed_message::TypedMessage::getMessageType
int getMessageType() const
ros::Time::now
static Time now()


industrial_robot_client
Author(s): Jeremy Zoss
autogenerated on Wed Mar 2 2022 00:24:59