37 #include <opencv2/opencv.hpp> 45 DiagnosticNodelet::onInit();
48 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
53 sub_ =
pnh_->subscribe(
"input/camera_info", 1,
69 cv::Mat mask_image = cv::Mat::zeros(info_msg->height, info_msg->width, CV_8UC1);
71 for (
int u = 0; u < info_msg->width; u++) {
72 for (
int v = 0;
v < info_msg->height;
v++) {
79 mask_image.data[mask_image.step *
v + mask_image.elemSize() * u] = 255;
92 double default_padding, default_scale;
93 pnh.
param(
"self_see_default_padding", default_padding, 0.01);
94 pnh.
param(
"self_see_default_scale", default_scale, 1.0);
95 std::vector<robot_self_filter::LinkInfo> links;
97 if(!pnh.
hasParam(
"self_see_links")) {
98 ROS_WARN(
"No links specified for self filtering.");
101 pnh.
getParam(
"self_see_links", ssl_vals);
103 ROS_WARN(
"Self see links need to be an array");
105 if(ssl_vals.
size() == 0) {
106 ROS_WARN(
"No values in self see links array");
108 for(
int i = 0; i < ssl_vals.
size(); i++) {
109 robot_self_filter::LinkInfo li;
111 ROS_WARN(
"Self see links entry %d is not a structure. Stopping processing of self see links",i);
114 if(!ssl_vals[i].hasMember(
"name")) {
115 ROS_WARN(
"Self see links entry %d has no name. Stopping processing of self see links",i);
118 li.name = std::string(ssl_vals[i][
"name"]);
119 if(!ssl_vals[i].hasMember(
"padding")) {
120 ROS_DEBUG(
"Self see links entry %d has no padding. Assuming default padding of %g",i,default_padding);
121 li.padding = default_padding;
123 li.padding = ssl_vals[i][
"padding"];
125 if(!ssl_vals[i].hasMember(
"scale")) {
126 ROS_DEBUG(
"Self see links entry %d has no scale. Assuming default scale of %g",i,default_scale);
127 li.scale = default_scale;
129 li.scale = ssl_vals[i][
"scale"];
PLUGINLIB_EXPORT_CLASS(jsk_perception::RobotToMaskImage, nodelet::Nodelet)
void publish(const boost::shared_ptr< M > &message) const
cv::Point3d projectPixelTo3dRay(const cv::Point2d &uv_rect) const
boost::shared_ptr< robot_self_filter::SelfMask< pcl::PointXYZ > > self_mask_
tf::TransformListener tf_listener_
Type const & getType() const
bool fromCameraInfo(const sensor_msgs::CameraInfo &msg)
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
virtual void unsubscribe()
virtual void infoCallback(const sensor_msgs::CameraInfo::ConstPtr &info_msg)
bool getParam(const std::string &key, std::string &s) const
bool hasParam(const std::string &key) const
virtual void initSelfMask(const ros::NodeHandle &pnh)
sensor_msgs::ImagePtr toImageMsg() const