32 #ifndef MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_RELAY_HANDLER_H 33 #define MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_RELAY_HANDLER_H 40 #include "control_msgs/FollowJointTrajectoryFeedback.h" 41 #include "sensor_msgs/JointState.h" 44 #include "trajectory_msgs/JointTrajectoryPoint.h" 46 #include "motoman_msgs/DynamicJointsGroup.h" 50 namespace joint_relay_handler
55 using trajectory_msgs::JointTrajectoryPoint;
56 using motoman_msgs::DynamicJointsGroup;
72 typedef std::map<int, RobotGroup>::iterator it_type;
84 std::map<int, RobotGroup>& robot_groups)
100 std::vector<std::string>& joint_names)
107 std::map<int, RobotGroup> robot_groups_;
113 std::map<int, ros::Publisher> pub_controls_;
114 std::map<int, ros::Publisher> pub_states_;
128 int msg_type, std::vector<std::string> &joint_names);
142 int msg_type, std::map<int, RobotGroup> &robot_groups);
153 control_msgs::FollowJointTrajectoryFeedback* control_state,
154 sensor_msgs::JointState* sensor_state);
166 control_msgs::FollowJointTrajectoryFeedback* control_state,
167 sensor_msgs::JointState* sensor_state,
int robot_id);
175 virtual bool convert_message(SimpleMessage& msg_in, DynamicJointsGroup* joint_state,
int robot_id);
183 virtual bool convert_message(SimpleMessage& msg_in, JointTrajectoryPoint* joint_state);
194 virtual bool transform(
const JointTrajectoryPoint& state_in, JointTrajectoryPoint* state_out)
196 *state_out = state_in;
209 virtual bool transform(
const DynamicJointsGroup& state_in, DynamicJointsGroup* state_out)
211 *state_out = state_in;
226 virtual bool select(
const JointTrajectoryPoint& all_joint_state,
const std::vector<std::string>& all_joint_names,
227 JointTrajectoryPoint* pub_joint_state, std::vector<std::string>* pub_joint_names);
229 virtual bool select(
const DynamicJointsGroup& all_joint_state,
const std::vector<std::string>& all_joint_names,
230 DynamicJointsGroup* pub_joint_state, std::vector<std::string>* pub_joint_names);
248 bool convert_message(JointMessage& msg_in, JointTrajectoryPoint* joint_state);
256 bool convert_message(JointMessage& msg_in, DynamicJointsGroup* joint_state,
int robot_id);
263 #endif // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_RELAY_HANDLER_H
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection, std::vector< std::string > &joint_names)
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)
ros::Publisher pub_joint_control_state_
bool internalCB(JointMessage &in)
std::vector< std::string > all_joint_names_
ros::Publisher pub_joint_sensor_state_