void in_cb(const ros::MessageEvent<ShapeShifter>& msg_event)
{
boost::shared_ptr<ShapeShifter const> const &msg = msg_event.getConstMessage();
if (!g_advertised)
{
g_pub = msg->advertise(*g_node, g_output_topic, 10, true);
foreign_advertise(msg_event.getConnectionHeader()["type"]);
g_advertised = true;
printf("advertised as %s\n", g_output_topic.c_str());
}
g_pub.publish(msg);
}
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());
}
