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()); }