Message handler that relays joint positions (converts simple message types to ROS message types and publishes them) More...
#include <joint_feedback_relay_handler.h>
Public Member Functions | |
virtual bool | init (SmplMsgConnection *connection, std::vector< std::string > &joint_names) |
Class initializer. | |
virtual bool | init (SmplMsgConnection *connection, std::map< int, RobotGroup > &robot_groups) |
Class initializer. | |
JointFeedbackRelayHandler (int robot_id=-1) | |
Constructor. | |
Protected Member Functions | |
virtual bool | convert_message (SimpleMessage &msg_in, JointTrajectoryPoint *joint_state) |
Convert joint message into intermediate message-type. | |
virtual bool | convert_message (SimpleMessage &msg_in, DynamicJointsGroup *joint_state, int robot_id) |
Convert joint message into intermediate message-type. | |
bool | create_messages (SimpleMessage &msg_in, control_msgs::FollowJointTrajectoryFeedback *control_state, sensor_msgs::JointState *sensor_state) |
Convert joint message into publish message-types. | |
bool | create_messages (SimpleMessage &msg_in, control_msgs::FollowJointTrajectoryFeedback *control_state, sensor_msgs::JointState *sensor_state, int robot_id) |
Convert joint message into publish message-types. | |
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 | transform (const DynamicJointsGroup &state_in, DynamicJointsGroup *state_out) |
Transform joint state before publishing. Can be overridden to implement, e.g. robot-specific joint coupling. | |
Protected Attributes | |
int | robot_id_ |
bool | version_0_ |
Private Member Functions | |
bool | convert_message (JointFeedbackMessage &msg_in, JointTrajectoryPoint *joint_state) |
Convert joint feedback message into intermediate message-type. | |
bool | convert_message (JointFeedbackMessage &msg_in, DynamicJointsGroup *joint_state, int robot_id) |
Convert joint feedback message into intermediate message-type. | |
Static Private Member Functions | |
static bool | JointDataToVector (const industrial::joint_data::JointData &joints, std::vector< double > &vec, int len) |
Message handler that relays joint positions (converts simple message types to ROS message types and publishes them)
THIS CLASS IS NOT THREAD-SAFE
Definition at line 62 of file joint_feedback_relay_handler.h.
industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::JointFeedbackRelayHandler | ( | int | robot_id = -1 | ) | [inline] |
Constructor.
Definition at line 68 of file joint_feedback_relay_handler.h.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::convert_message | ( | SimpleMessage & | msg_in, |
JointTrajectoryPoint * | joint_state | ||
) | [protected, virtual] |
Convert joint message into intermediate message-type.
[in] | msg_in | Message from robot connection |
[out] | joint_state | JointTrajectoryPt message for intermediate processing |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 146 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::convert_message | ( | SimpleMessage & | msg_in, |
DynamicJointsGroup * | joint_state, | ||
int | robot_id | ||
) | [protected, virtual] |
Convert joint message into intermediate message-type.
[in] | msg_in | Message from robot connection |
[out] | joint_state | JointTrajectoryPt message for intermediate processing |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 134 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::convert_message | ( | JointFeedbackMessage & | msg_in, |
JointTrajectoryPoint * | joint_state | ||
) | [private] |
Convert joint feedback message into intermediate message-type.
[in] | msg_in | JointFeedbackMessage from robot connection |
[out] | joint_state | JointTrajectoryPt message for intermediate processing |
Definition at line 226 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::convert_message | ( | JointFeedbackMessage & | msg_in, |
DynamicJointsGroup * | joint_state, | ||
int | robot_id | ||
) | [private] |
Convert joint feedback message into intermediate message-type.
[in] | msg_in | JointFeedbackMessage from robot connection |
[out] | joint_state | JointTrajectoryPt message for intermediate processing |
Definition at line 176 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::create_messages | ( | SimpleMessage & | msg_in, |
control_msgs::FollowJointTrajectoryFeedback * | control_state, | ||
sensor_msgs::JointState * | sensor_state | ||
) | [protected, virtual] |
Convert joint message into publish message-types.
[in] | msg_in | Message from robot connection |
[out] | control_state | FollowJointTrajectoryFeedback message for ROS publishing |
[out] | sensor_state | JointState message for ROS publishing |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 72 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::create_messages | ( | SimpleMessage & | msg_in, |
control_msgs::FollowJointTrajectoryFeedback * | control_state, | ||
sensor_msgs::JointState * | sensor_state, | ||
int | robot_id | ||
) | [protected, virtual] |
Convert joint message into publish message-types.
[in] | msg_in | Message from robot connection |
[out] | control_state | FollowJointTrajectoryFeedback message for ROS publishing |
[out] | sensor_state | JointState message for ROS publishing |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 87 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::init | ( | SmplMsgConnection * | connection, |
std::vector< std::string > & | joint_names | ||
) | [virtual] |
Class initializer.
connection | simple message connection that will be used to send replies. |
joint_names | list of joint-names for msg-publishing.
|
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 58 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::init | ( | SmplMsgConnection * | connection, |
std::map< int, RobotGroup > & | robot_groups | ||
) | [virtual] |
Class initializer.
connection | simple message connection that will be used to send replies. |
joint_names | list of joint-names for msg-publishing.
|
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 47 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::JointDataToVector | ( | const industrial::joint_data::JointData & | joints, |
std::vector< double > & | vec, | ||
int | len | ||
) | [static, private] |
Definition at line 158 of file joint_feedback_relay_handler.cpp.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::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 | ||
) | [protected, virtual] |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 277 of file joint_feedback_relay_handler.cpp.
virtual bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::transform | ( | const DynamicJointsGroup & | state_in, |
DynamicJointsGroup * | state_out | ||
) | [inline, protected, virtual] |
Transform joint state before publishing. Can be overridden to implement, e.g. robot-specific joint coupling.
[in] | state_in | joint state, exactly as passed from robot connection. |
[out] | state_out | transformed joint state (in same order/count as input state) |
Reimplemented from industrial_robot_client::joint_relay_handler::JointRelayHandler.
Definition at line 118 of file joint_feedback_relay_handler.h.
int industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::robot_id_ [protected] |
Definition at line 88 of file joint_feedback_relay_handler.h.
bool industrial_robot_client::joint_feedback_relay_handler::JointFeedbackRelayHandler::version_0_ [protected] |
Definition at line 89 of file joint_feedback_relay_handler.h.