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);
		//}
}
Esempio n. 2
0
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_);
}
Esempio n. 3
0
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());
}