48 namespace joint_relay_handler
53 this->robot_groups_ = robot_groups;
54 for (it_type iterator = robot_groups.begin(); iterator != robot_groups.end(); iterator++)
56 std::string name_str, ns_str;
57 int robot_id = iterator->first;
58 name_str = iterator->second.get_name();
59 ns_str = iterator->second.get_ns();
62 ns_str +
"/" + name_str +
"/feedback_states", 1);
65 this->
node_.
advertise<sensor_msgs::JointState>(ns_str +
"/" + name_str +
"/joint_states", 1);
74 return MessageHandler::init(msg_type, connection);
80 this->
node_.
advertise<control_msgs::FollowJointTrajectoryFeedback>(
"feedback_states", 1);
87 return MessageHandler::init(msg_type, connection);
94 control_msgs::FollowJointTrajectoryFeedback control_state;
95 sensor_msgs::JointState sensor_state;
106 if (CommTypes::SERVICE_REQUEST == msg_in.getMessageType())
109 reply.init(msg_in.getMessageType(),
120 control_msgs::FollowJointTrajectoryFeedback* control_state,
121 sensor_msgs::JointState* sensor_state)
124 JointTrajectoryPoint all_joint_state;
125 if (!convert_message(msg_in, &all_joint_state))
127 LOG_ERROR(
"Failed to convert SimpleMessage");
131 JointTrajectoryPoint xform_joint_state;
132 if (!
transform(all_joint_state, &xform_joint_state))
134 LOG_ERROR(
"Failed to transform joint state");
139 JointTrajectoryPoint pub_joint_state;
140 std::vector<std::string> pub_joint_names;
143 LOG_ERROR(
"Failed to select joints for publishing");
148 *control_state = control_msgs::FollowJointTrajectoryFeedback();
150 control_state->joint_names = pub_joint_names;
151 control_state->actual.positions = pub_joint_state.positions;
152 control_state->actual.velocities = pub_joint_state.velocities;
153 control_state->actual.accelerations = pub_joint_state.accelerations;
154 control_state->actual.time_from_start = pub_joint_state.time_from_start;
156 *sensor_state = sensor_msgs::JointState();
158 sensor_state->name = pub_joint_names;
159 sensor_state->position = pub_joint_state.positions;
160 sensor_state->velocity = pub_joint_state.velocities;
170 control_msgs::FollowJointTrajectoryFeedback* control_state,
171 sensor_msgs::JointState* sensor_state,
int robot_id)
173 DynamicJointsGroup all_joint_state;
174 if (!convert_message(msg_in, &all_joint_state, robot_id))
176 LOG_ERROR(
"Failed to convert SimpleMessage");
180 DynamicJointsGroup xform_joint_state;
181 if (!
transform(all_joint_state, &xform_joint_state))
183 LOG_ERROR(
"Failed to transform joint state");
188 DynamicJointsGroup pub_joint_state;
189 std::vector<std::string> pub_joint_names;
190 if (!
select(xform_joint_state, robot_groups_[robot_id].get_joint_names(), &pub_joint_state, &pub_joint_names))
192 LOG_ERROR(
"Failed to select joints for publishing");
196 *control_state = control_msgs::FollowJointTrajectoryFeedback();
198 control_state->joint_names = pub_joint_names;
199 control_state->actual.positions = pub_joint_state.positions;
200 control_state->actual.velocities = pub_joint_state.velocities;
201 control_state->actual.accelerations = pub_joint_state.accelerations;
202 control_state->actual.time_from_start = pub_joint_state.time_from_start;
204 *sensor_state = sensor_msgs::JointState();
206 sensor_state->name = pub_joint_names;
207 sensor_state->position = pub_joint_state.positions;
208 sensor_state->velocity = pub_joint_state.velocities;
210 this->pub_controls_[robot_id].publish(*control_state);
211 this->pub_states_[robot_id].publish(*sensor_state);
216 bool JointRelayHandler::convert_message(SimpleMessage& msg_in, JointTrajectoryPoint* joint_state)
218 JointMessage joint_msg;
220 if (!joint_msg.init(msg_in))
222 LOG_ERROR(
"Failed to initialize joint message");
226 return convert_message(joint_msg, joint_state);
229 bool JointRelayHandler::convert_message(SimpleMessage& msg_in, DynamicJointsGroup* joint_state,
int robot_id)
231 JointMessage joint_msg;
233 if (!joint_msg.init(msg_in))
235 LOG_ERROR(
"Failed to initialize joint message");
239 return convert_message(joint_msg, joint_state, robot_id);
242 bool JointRelayHandler::convert_message(JointMessage& msg_in, JointTrajectoryPoint* joint_state)
246 joint_state->positions.resize(num_jnts);
247 for (
int i = 0; i < num_jnts; ++i)
250 if (msg_in.getJoints().getJoint(i, value))
252 joint_state->positions[i] =
value;
255 LOG_ERROR(
"Failed to parse position #%d from JointMessage", i);
259 joint_state->velocities.clear();
260 joint_state->accelerations.clear();
266 bool JointRelayHandler::convert_message(JointMessage& msg_in, DynamicJointsGroup* joint_state,
int robot_id)
269 int num_jnts = robot_groups_[robot_id].get_joint_names().size();
270 joint_state->positions.resize(num_jnts);
271 for (
int i = 0; i < num_jnts; ++i)
274 if (msg_in.getJoints().getJoint(i, value))
276 joint_state->positions[i] =
value;
283 joint_state->velocities.clear();
284 joint_state->accelerations.clear();
291 const std::vector<std::string>& all_joint_names, JointTrajectoryPoint* pub_joint_state,
292 std::vector<std::string>* pub_joint_names)
294 ROS_ASSERT(all_joint_state.positions.size() == all_joint_names.size());
296 *pub_joint_state = JointTrajectoryPoint();
297 pub_joint_names->clear();
300 for (
size_t i = 0; i < all_joint_names.size(); ++i)
302 if (all_joint_names[i].empty())
305 pub_joint_names->push_back(all_joint_names[i]);
306 if (!all_joint_state.positions.empty())
307 pub_joint_state->positions.push_back(all_joint_state.positions[i]);
308 if (!all_joint_state.velocities.empty())
309 pub_joint_state->velocities.push_back(all_joint_state.velocities[i]);
310 if (!all_joint_state.accelerations.empty())
311 pub_joint_state->accelerations.push_back(all_joint_state.accelerations[i]);
313 pub_joint_state->time_from_start = all_joint_state.time_from_start;
319 const std::vector<std::string>& all_joint_names, DynamicJointsGroup* pub_joint_state,
320 std::vector<std::string>* pub_joint_names)
322 ROS_ASSERT(all_joint_state.positions.size() == all_joint_names.size());
324 *pub_joint_state = DynamicJointsGroup();
325 pub_joint_names->clear();
328 for (
size_t i = 0; i < all_joint_names.size(); ++i)
330 if (all_joint_names[i].empty())
332 pub_joint_names->push_back(all_joint_names[i]);
333 if (!all_joint_state.positions.empty())
334 pub_joint_state->positions.push_back(all_joint_state.positions[i]);
335 if (!all_joint_state.velocities.empty())
336 pub_joint_state->velocities.push_back(all_joint_state.velocities[i]);
337 if (!all_joint_state.accelerations.empty())
338 pub_joint_state->accelerations.push_back(all_joint_state.accelerations[i]);
340 pub_joint_state->time_from_start = all_joint_state.time_from_start;
virtual bool sendMsg(industrial::simple_message::SimpleMessage &message)
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection, std::vector< std::string > &joint_names)
void publish(const boost::shared_ptr< M > &message) const
virtual bool create_messages(JointMessage &msg_in, control_msgs::FollowJointTrajectoryFeedback *control_state, sensor_msgs::JointState *sensor_state)
virtual bool transform(const std::vector< double > &pos_in, std::vector< double > *pos_out)
virtual bool select(const std::vector< double > &all_joint_pos, const std::vector< std::string > &all_joint_names, std::vector< double > *pub_joint_pos, std::vector< std::string > *pub_joint_names)
#define LOG_ERROR(format,...)
ros::Publisher pub_joint_control_state_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
bool internalCB(JointMessage &in)
industrial::smpl_msg_connection::SmplMsgConnection * getConnection()
std::vector< std::string > all_joint_names_
ros::Publisher pub_joint_sensor_state_