template <class T> void DDSProxyPoseStamped::messageCallback(
const ros::MessageEvent<T const>& event) {
ROS_DEBUG("[Debug]: Received message event of type geometry_msgs::PoseStamped\n");
// if (event.getPublisherName() != node_name) {
ROS_DEBUG("Received message from %s", event.getPublisherName().c_str());
ros::Time receipt_time = event.getReceiptTime();
const geometry_msgs::PoseStampedConstPtr& msg = event.getMessage();
proxyPoseStamped *proxyPoseStamped_msg = new proxyPoseStamped();
// Filling in data for *proxyPoseStamped.idl*
std::string frame_id = "/";
frame_id.append(robot_name.c_str());
frame_id.append(std::string(msg->header.frame_id).c_str());
proxyPoseStamped_msg->header.frame_id = frame_id.c_str();
proxyPoseStamped_msg->header.seq = msg->header.seq;
proxyPoseStamped_msg->header.stamp.nsec = msg->header.stamp.nsec;
proxyPoseStamped_msg->header.stamp.sec = msg->header.stamp.sec;
// fill pose/position
proxyPoseStamped_msg->pose.position.x = msg->pose.position.x;
proxyPoseStamped_msg->pose.position.y = msg->pose.position.y;
proxyPoseStamped_msg->pose.position.z = msg->pose.position.z;
// fill pose/orientation
proxyPoseStamped_msg->pose.orientation.x = msg->pose.orientation.x;
proxyPoseStamped_msg->pose.orientation.y = msg->pose.orientation.y;
proxyPoseStamped_msg->pose.orientation.z = msg->pose.orientation.z;
proxyPoseStamped_msg->pose.orientation.w = msg->pose.orientation.w;
ReturnCode_t ret = m_data_writer->write(*proxyPoseStamped_msg, NULL);
checkStatus(ret, "proxyPoseStamped::Write");
// ROS_INFO("Print return code %d", ret);
//}
}
void rcInCB(const ros::MessageEvent<mavros_msgs::RCIn const>& event)
{
const std::string& sender = event.getPublisherName();
const ros::M_string& header = event.getConnectionHeader();
ros::Time receipt_time = event.getReceiptTime();
const mavros_msgs::RCIn::ConstPtr& msg = event.getMessage();
const mavros_msgs::RCIn rc_in = *(msg.get());
joy_.axes[JOY_AXES_THROTTLE] = scaleRC(rc_in.channels[RC_THROTTLE], false);
joy_.axes[JOY_AXES_YAW] = scaleRC(rc_in.channels[RC_YAW], false);
joy_.axes[JOY_AXES_PITCH] = scaleRC(rc_in.channels[RC_PITCH], false);
joy_.axes[JOY_AXES_ROLL] = scaleRC(rc_in.channels[RC_ROLL], false);
joy_.axes[JOY_AXES_AUX] = scaleRC(rc_in.channels[RC_AUX3], false);
joy_.buttons[JOY_BUTTON_0] = switchRC2(rc_in.channels[RC_GEAR]);
joy_.buttons[JOY_BUTTON_1] = switchRC3(rc_in.channels[RC_FLAP]);
joy_.buttons[JOY_BUTTON_2] = switchRC3(rc_in.channels[RC_AUX2]);
joy_.header.stamp = msg.get()->header.stamp;
joy_pub_.publish(joy_);
}
void smfCallback(const ros::MessageEvent<smfwin::SMF const>& event)
{
const std::string& publisher_name = event.getPublisherName();
ros::M_string header = event.getConnectionHeader();
ros::Time receipt_time = event.getReceiptTime();
const smfwin::SMF::ConstPtr& msg = event.getMessage();
//ROS_INFO("callback");
ros::M_string::iterator it = header.find("type");
std::string mtype = it->second;
it = header.find("message_definition");
std::string mdef = it->second;
std::stringstream ss;
ros::WallTime t= ros::WallTime::now();
std::stringstream s; // Allocates memory on stack
s << t.sec << "." << t.nsec;
std::string tstamp = s.str();
distributeMessage(publisher_name, msg->type.c_str(), msg->data.c_str(), tstamp, 0);
//ROS_INFO("Message sent from ROS to %s type", msg->type.c_str());
}
