32 #ifndef MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_FEEDBACK_RELAY_HANDLER_H 33 #define MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_FEEDBACK_RELAY_HANDLER_H 41 #include "motoman_msgs/DynamicJointsGroup.h" 45 namespace joint_feedback_relay_handler
52 using trajectory_msgs::JointTrajectoryPoint;
53 using motoman_msgs::DynamicJointsGroup;
82 std::vector<std::string> &joint_names);
85 std::map<int, RobotGroup> &robot_groups);
110 control_msgs::FollowJointTrajectoryFeedback* control_state,
111 sensor_msgs::JointState* sensor_state);
114 control_msgs::FollowJointTrajectoryFeedback* control_state,
115 sensor_msgs::JointState* sensor_state,
int robot_id);
118 virtual bool transform(
const DynamicJointsGroup& state_in, DynamicJointsGroup* state_out)
120 *state_out = state_in;
124 virtual bool select(
const DynamicJointsGroup& all_joint_state,
const std::vector<std::string>& all_joint_names,
125 DynamicJointsGroup* pub_joint_state, std::vector<std::string>* pub_joint_names);
129 std::vector<double> &vec,
int len);
152 #endif // MOTOMAN_DRIVER_INDUSTRIAL_ROBOT_CLIENT_JOINT_FEEDBACK_RELAY_HANDLER_H virtual bool transform(const DynamicJointsGroup &state_in, DynamicJointsGroup *state_out)
Transform joint state before publishing. Can be overridden to implement, e.g. robot-specific joint co...
bool create_messages(SimpleMessage &msg_in, control_msgs::FollowJointTrajectoryFeedback *control_state, sensor_msgs::JointState *sensor_state)
Convert joint message into publish message-types.
virtual bool convert_message(SimpleMessage &msg_in, JointTrajectoryPoint *joint_state)
Convert joint message into intermediate message-type.
static bool JointDataToVector(const industrial::joint_data::JointData &joints, std::vector< double > &vec, int len)
virtual bool select(const DynamicJointsGroup &all_joint_state, const std::vector< std::string > &all_joint_names, DynamicJointsGroup *pub_joint_state, std::vector< std::string > *pub_joint_names)
virtual bool init(SmplMsgConnection *connection, std::vector< std::string > &joint_names)
Class initializer.
Message handler that relays joint positions (converts simple message types to ROS message types and p...
JointFeedbackRelayHandler(int robot_id=-1)
Constructor.
Message handler that relays joint positions (converts simple message types to ROS message types and p...