49 namespace joint_feedback_ex_relay_handler
52 std::map<int, RobotGroup> &robot_groups)
54 ROS_INFO_STREAM(
"Creating joint_feedback_ex_relay_handler with " << robot_groups.size() <<
" groups");
56 this->
node_.
advertise<control_msgs::FollowJointTrajectoryFeedback>(
"feedback_states", 1);
58 this->
node_.
advertise<motoman_msgs::DynamicJointTrajectoryFeedback>(
"dynamic_feedback_states", 1);
62 this->robot_groups_ = robot_groups;
73 std::vector<std::string> &joint_names)
86 control_msgs::FollowJointTrajectoryFeedback* control_state,
87 sensor_msgs::JointState* sensor_state)
92 motoman_msgs::DynamicJointTrajectoryFeedback dynamic_control_state;
102 motoman_msgs::DynamicJointState dyn_joint_state;
103 dyn_joint_state.num_joints = control_state->joint_names.size();
104 dyn_joint_state.group_number = group_number;
106 dyn_joint_state.positions = control_state->actual.positions;
107 dyn_joint_state.velocities = control_state->actual.velocities;
108 dyn_joint_state.accelerations = control_state->actual.accelerations;
109 dynamic_control_state.joint_feedbacks.push_back(dyn_joint_state);
119 control_msgs::FollowJointTrajectoryFeedback* control_state,
120 sensor_msgs::JointState* sensor_state,
int robot_id)
122 DynamicJointsGroup all_joint_state;
125 LOG_ERROR(
"Failed to convert SimpleMessage");
129 DynamicJointsGroup xform_joint_state;
130 if (!
transform(all_joint_state, &xform_joint_state))
132 LOG_ERROR(
"Failed to transform joint state");
137 DynamicJointsGroup pub_joint_state;
138 std::vector<std::string> pub_joint_names;
139 if (!
select(xform_joint_state, robot_groups_[robot_id].get_joint_names(), &pub_joint_state, &pub_joint_names))
141 LOG_ERROR(
"Failed to select joints for publishing");
147 *control_state = control_msgs::FollowJointTrajectoryFeedback();
149 control_state->joint_names = pub_joint_names;
150 control_state->actual.positions = pub_joint_state.positions;
151 control_state->actual.velocities = pub_joint_state.velocities;
152 control_state->actual.accelerations = pub_joint_state.accelerations;
153 control_state->actual.time_from_start = pub_joint_state.time_from_start;
157 *sensor_state = sensor_msgs::JointState();
159 sensor_state->name = pub_joint_names;
160 sensor_state->position = pub_joint_state.positions;
161 sensor_state->velocity = pub_joint_state.velocities;
173 int num_jnts = robot_groups_[robot_id].get_joint_names().size();
182 LOG_ERROR(
"Failed to parse position data from JointFeedbackMessage");
188 joint_state->positions.clear();
199 LOG_ERROR(
"Failed to parse velocity data from JointFeedbackMessage");
205 joint_state->velocities.clear();
211 if (acceleration_field)
216 LOG_ERROR(
"Failed to parse acceleration data from JointFeedbackMessage");
222 joint_state->accelerations.clear();
228 bool time_field = msg_in.
getTime(value);
244 std::vector<double> &vec,
249 LOG_ERROR(
"Failed to copy JointData. Len (%d) out of range (0 to %d)",
255 for (
int i = 0; i < len; ++i)
industrial::shared_types::shared_int getGroupsNumber()
bool init(industrial::smpl_msg_connection::SmplMsgConnection *connection, std::vector< std::string > &joint_names)
bool getPositions(industrial::joint_data::JointData &dest)
void publish(const boost::shared_ptr< M > &message) const
std::vector< double > values
virtual bool init(SmplMsgConnection *connection, std::vector< std::string > &joint_names)
Class initializer.
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)
Class encapsulated joint feedback ex message generation methods (either to or from a industrial::simp...
virtual bool convert_message(JointFeedbackMessage &msg_in, DynamicJointsGroup *joint_state, int robot_id)
Convert joint message into intermediate message-type.
#define LOG_ERROR(format,...)
bool getJoint(industrial::shared_types::shared_int index, industrial::shared_types::shared_real &value) const
bool getTime(industrial::shared_types::shared_real &time)
static bool JointDataToVector(const industrial::joint_data::JointData &joints, std::vector< double > &vec, int len)
bool getVelocities(industrial::joint_data::JointData &dest)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
industrial::shared_types::shared_int valid_fields_from_message_
bit-mask of (optional) fields that have been initialized with valid data
ros::Publisher dynamic_pub_joint_control_state_
ros::Publisher pub_joint_sensor_state_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher pub_joint_control_state_
bool create_messages(SimpleMessage &msg_in, control_msgs::FollowJointTrajectoryFeedback *control_state, sensor_msgs::JointState *sensor_state)
Convert joint message into publish message-types.
std::vector< industrial::joint_feedback_message::JointFeedbackMessage > getJointMessages()
#define ROS_INFO_STREAM(args)
int getMaxNumJoints() const
bool getAccelerations(industrial::joint_data::JointData &dest)
bool init(industrial::simple_message::SimpleMessage &msg)
Initializes message from a simple message.
Enumeration of motoman-specific message types. See simple_message.h for a listing of "standard" messa...