static GString *gmime_message_to_json(GMimeMessage *message, guint content_option) {
MessageData *mdata = convert_message(message, content_option);
JSON_Value *root_value = json_value_init_object();
JSON_Object *root_object = json_value_get_object(root_value);
json_object_set_value(root_object, "from", addresses_list_to_json(mdata->from));
json_object_set_value(root_object, "to", addresses_list_to_json(mdata->to));
json_object_set_value(root_object, "replyTo", addresses_list_to_json(mdata->reply_to));
json_object_set_value(root_object, "cc", addresses_list_to_json(mdata->cc));
json_object_set_value(root_object, "bcc", addresses_list_to_json(mdata->bcc));
json_object_set_string(root_object, "messageId", mdata->message_id);
json_object_set_string(root_object, "subject", mdata->subject);
json_object_set_string(root_object, "date", mdata->date);
json_object_set_value(root_object, "inReplyTo", references_to_json(mdata->in_reply_to));
json_object_set_value(root_object, "references", references_to_json(mdata->references));
json_object_set_value(root_object, "text", message_body_to_json(mdata->text));
json_object_set_value(root_object, "html", message_body_to_json(mdata->html));
json_object_set_value(root_object, "attachments", message_attachments_list_to_json(mdata->attachments));
free_message_data(mdata);
gchar *serialized_string = json_serialize_to_string(root_value);
json_value_free(root_value);
GString *json_string = g_string_new(serialized_string);
g_free(serialized_string);
return json_string;
}
bool JointRelayHandler::convert_message(SimpleMessage& msg_in, DynamicJointsGroup* joint_state, int robot_id)
{
JointMessage joint_msg;
if (!joint_msg.init(msg_in))
{
LOG_ERROR("Failed to initialize joint message");
return false;
}
return convert_message(joint_msg, joint_state, robot_id);
}
bool JointRelayHandler::convert_message(SimpleMessage& msg_in, JointTrajectoryPoint* joint_state)
{
JointMessage joint_msg;
if (!joint_msg.init(msg_in))
{
LOG_ERROR("Failed to initialize joint message");
return false;
}
return convert_message(joint_msg, joint_state);
}
// TODO: Add support for other message fields (effort, desired pos)
bool JointRelayHandler::create_messages(SimpleMessage& msg_in,
control_msgs::FollowJointTrajectoryFeedback* control_state,
sensor_msgs::JointState* sensor_state)
{
// read state from robot message
JointTrajectoryPoint all_joint_state;
if (!convert_message(msg_in, &all_joint_state))
{
LOG_ERROR("Failed to convert SimpleMessage");
return false;
}
// apply transform, if required
JointTrajectoryPoint xform_joint_state;
if (!transform(all_joint_state, &xform_joint_state))
{
LOG_ERROR("Failed to transform joint state");
return false;
}
// select specific joints for publishing
JointTrajectoryPoint pub_joint_state;
std::vector<std::string> pub_joint_names;
if (!select(xform_joint_state, all_joint_names_, &pub_joint_state, &pub_joint_names))
{
LOG_ERROR("Failed to select joints for publishing");
return false;
}
// assign values to messages
*control_state = control_msgs::FollowJointTrajectoryFeedback(); // always start with a "clean" message
control_state->header.stamp = ros::Time::now();
control_state->joint_names = pub_joint_names;
control_state->actual.positions = pub_joint_state.positions;
control_state->actual.velocities = pub_joint_state.velocities;
control_state->actual.accelerations = pub_joint_state.accelerations;
control_state->actual.time_from_start = pub_joint_state.time_from_start;
*sensor_state = sensor_msgs::JointState(); // always start with a "clean" message
sensor_state->header.stamp = ros::Time::now();
sensor_state->name = pub_joint_names;
sensor_state->position = pub_joint_state.positions;
sensor_state->velocity = pub_joint_state.velocities;
this->pub_joint_control_state_.publish(*control_state);
this->pub_joint_sensor_state_.publish(*sensor_state);
return true;
}
