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 MarkerDisplay::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
std::string authority = marker_evt.getPublisherName();
std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
void callback(const ros::MessageEvent<tf::tfMessage const>& msg_evt)
{
const tf::tfMessage& message = *(msg_evt.getConstMessage());
std::string authority = msg_evt.getPublisherName(); // lookup the authority
double average_offset = 0;
boost::mutex::scoped_lock my_lock(map_lock_);
for (unsigned int i = 0; i < message.transforms.size(); i++)
{
frame_authority_map[message.transforms[i].child_frame_id] = authority;
double offset = (ros::Time::now() - message.transforms[i].header.stamp).toSec();
average_offset += offset;
std::map<std::string, std::vector<double> >::iterator it = delay_map.find(message.transforms[i].child_frame_id);
if (it == delay_map.end())
{
delay_map[message.transforms[i].child_frame_id] = std::vector<double>(1,offset);
}
else
{
it->second.push_back(offset);
if (it->second.size() > 1000)
it->second.erase(it->second.begin());
}
}
average_offset /= max((size_t) 1, message.transforms.size());
//create the authority log
std::map<std::string, std::vector<double> >::iterator it2 = authority_map.find(authority);
if (it2 == authority_map.end())
{
authority_map[authority] = std::vector<double>(1,average_offset);
}
else
{
it2->second.push_back(average_offset);
if (it2->second.size() > 1000)
it2->second.erase(it2->second.begin());
}
//create the authority frequency log
std::map<std::string, std::vector<double> >::iterator it3 = authority_frequency_map.find(authority);
if (it3 == authority_frequency_map.end())
{
authority_frequency_map[authority] = std::vector<double>(1,ros::Time::now().toSec());
}
else
{
it3->second.push_back(ros::Time::now().toSec());
if (it3->second.size() > 1000)
it3->second.erase(it3->second.begin());
}
};
void MarkerDisplayCustom::failedMarker(const ros::MessageEvent<visualization_msgs::Marker>& marker_evt, tf::FilterFailureReason reason)
{
/// Changed in indigo
// std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, marker->__connection_header ? (*marker->__connection_header)["callerid"] : "unknown", reason);
// setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
visualization_msgs::Marker::ConstPtr marker = marker_evt.getConstMessage();
std::string authority = marker_evt.getPublisherName();
std::string error = context_->getFrameManager()->discoverFailureReason(marker->header.frame_id, marker->header.stamp, authority, reason);
setMarkerStatus(MarkerID(marker->ns, marker->id), StatusProperty::Error, error);
}
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());
}
void ImageNodelet::processCameraInfo( const ros::MessageEvent<sensor_msgs::CameraInfo const>& event, const std::string& topic )
{
boost::mutex::scoped_lock lock(image_history_mutex_);
const std::string& publisher_name = event.getPublisherName();
// publishing on my own topic; return
if(publisher_name == this->getName())
return;
//ROS_ERROR("Received camera info from %s on topic %s",publisher_name.c_str(),topic.c_str());
const sensor_msgs::CameraInfo::ConstPtr& msg = event.getMessage();
unsigned int image_index;
for(image_index = 0; image_index < image_history_.size(); image_index++)
if(msg->header.stamp == image_history_[image_index].header.stamp)
break;
// if it doesn't exist
if(image_index == image_history_.size())
{
std::cout << "Adding new image to history (" << id_counter_ << "); using CameraInfo." << std::endl;
ImageStruct newimg;
newimg.id = id_counter_++;
newimg.camera_info = *msg;
newimg.topic = topic;
newimg.topic = newimg.topic.erase(newimg.topic.find("/camera_info"),strlen("/camera_info"));
image_history_.push_back(newimg);
}
else
{
std::cout << "Adding CameraInfo to existing history image (" << image_history_[image_index].id << ")." << std::endl;
image_history_[image_index].camera_info = *msg;
publishImageAdded(image_index);
}
}
void ImageNodelet::processImage( const ros::MessageEvent<sensor_msgs::Image const>& event, const std::string& topic )
{
boost::mutex::scoped_lock lock(image_history_mutex_);
const std::string& publisher_name = event.getPublisherName();
std::vector<int> qualitytype;
qualitytype.push_back(CV_IMWRITE_JPEG_QUALITY);
qualitytype.push_back(90);
// publishing on my own topic; return
if(publisher_name == this->getName())
return;
//ROS_ERROR("Received image from %s on topic %s",publisher_name.c_str(),topic.c_str());
const sensor_msgs::Image::ConstPtr& msg = event.getMessage();
unsigned int image_index;
for(image_index = 0; image_index < image_history_.size(); image_index++)
if(msg->header.stamp == image_history_[image_index].camera_info.header.stamp)
break;
// if it doesn't exist
if(image_index == image_history_.size())
{
std::cout << "Adding new image to history (" << id_counter_ << "); using Image." << std::endl;
ImageStruct newimg;
newimg.id = id_counter_++;
newimg.header.stamp = msg->header.stamp;
//newimg.image = *msg;
newimg.topic = topic;
newimg.topic = newimg.topic.erase(newimg.topic.find("/image_raw"),strlen("/image_raw"));
image_history_.push_back(newimg);
}
else
{
std::cout << "Adding Image to existing history image (" << image_history_[image_index].id << ")." << std::endl;
// image_history_[image_index].image = *msg;
image_history_[image_index].header.stamp = msg->header.stamp;
publishImageAdded(image_index);
}
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(*msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
// ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::Size2i img_size;
img_size.width=(double)msg->width;
img_size.height=msg->height;
cv::resize(cv_ptr->image, cv_ptr->image, img_size, 0, 0, cv::INTER_NEAREST);
std::stringstream stream;
stream.str("");
stream << id_counter_-1;
std::cout<<"Image size:"<<img_size.width;
const char dir_path[] = "/home/vigir/image";
boost::filesystem::path dir(dir_path);
if(boost::filesystem::create_directory(dir))
{
std::cout << "Successfully created directory /home/vigir/image" << "\n";
}
else
std::cout<<"\nFolder not created!!";
imwrite("/home/vigir/image/"+stream.str()+".jpg",cv_ptr->image,qualitytype);
}
