5 #include <std_msgs/String.h> 6 #include <std_msgs/Bool.h> 8 #include <opencv2/opencv.hpp> 10 #include <geometry_msgs/PointStamped.h> 14 #include <robotican_common/FindObjectDynParamConfig.h> 15 #include <dynamic_reconfigure/server.h> 20 #include <ar_track_alvar_msgs/AlvarMarkers.h> 21 #include <robotican_msgs_srvs/switch_topic.h> 28 bool find_object(Mat input, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,Point3d *obj,std::string frame);
29 void cloud_cb(
const sensor_msgs::PointCloud2ConstPtr& input);
67 void cloud_cb(
const sensor_msgs::PointCloud2ConstPtr& input) {
71 pcl::PointCloud<pcl::PointXYZRGBA> cloud;
75 pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudp (
new pcl::PointCloud<pcl::PointXYZRGBA> (cloud));
77 if (cloudp->empty()) {
83 sensor_msgs::ImagePtr image_msg(
new sensor_msgs::Image);
85 image_msg->header.stamp = input->header.stamp;
86 image_msg->header.frame_id = input->header.frame_id;
90 Mat result=cv_ptr->image;
103 geometry_msgs::PoseStamped target_pose;
104 target_pose.header.frame_id=input->header.frame_id;
106 target_pose.pose.position.x =obj.x;
107 target_pose.pose.position.y = obj.y;
108 target_pose.pose.position.z = obj.z;
109 target_pose.pose.orientation.w=1;
122 geometry_msgs::PoseStamped base_object_pose;
123 listener_ptr->
transformPose(
"base_footprint", *point_ptr, base_object_pose);
127 ar_track_alvar_msgs::AlvarMarkers msg;
128 msg.header.stamp=base_object_pose.header.stamp;
129 msg.header.frame_id=
"base_footprint";
131 ar_track_alvar_msgs::AlvarMarker m;
133 m.header.stamp=base_object_pose.header.stamp;
134 m.header.frame_id=
"base_footprint";
135 m.pose=base_object_pose;
136 msg.markers.push_back(m);
138 m.pose.pose.position.z-=0.1;
140 msg.markers.push_back(m);
148 printf (
"Failure %s\n", ex.what());
152 bool find_object(Mat input, pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudp,Point3d *pr,std::string frame) {
154 Mat hsv,filtered,bw,mask;
158 out_msg.
header.frame_id= frame;
171 addWeighted(lower_hue_range, 1.0, upper_hue_range, 1.0, 0.0, mask);
177 hsv.copyTo(filtered,mask);
178 cvtColor(filtered,filtered,CV_HSV2BGR);
180 out_msg.
image = filtered;
193 morphologyEx( bw, bw, MORPH_CLOSE, element,
Point(-1,-1), 1 );
201 vector<vector<Point>> contours;
202 vector<Vec4i> hierarchy;
204 findContours(bw, contours,hierarchy,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE);
207 double largest_area=0;
208 int largest_contour_index=0;
209 for(
int i = 0; i< contours.size(); i++ )
211 double area0 = abs(contourArea(contours[i]));
212 if(area0>largest_area){
214 largest_contour_index=i;
218 if ((largest_area>
minA)&&(largest_area<
maxA)) {
221 drawContours(input, contours, (
int)largest_contour_index, Scalar(255,0,0), 3, 8, hierarchy, 0);
222 Moments mu=moments( contours[largest_contour_index],
true );
223 Point2f mc = Point2f( mu.m10/mu.m00 , mu.m01/mu.m00 );
224 circle( input, mc, 4, Scalar(0,0,255), -1, 8, 0 );
225 int pcl_index = ((int)(mc.y)*input.cols) + (
int)(mc.x);
226 circle( input, mc, 8, Scalar(0,255,0), -1, 8, 0 );
228 pr->x=cloudp->points[pcl_index].x;
229 pr->y=cloudp->points[pcl_index].y;
230 pr->z=cloudp->points[pcl_index].z;
232 if (isnan (pr->x) || isnan (pr->y) || isnan (pr->z) ) {
237 sprintf(str,
"[%.3f,%.3f,%.3f] A=%lf",pr->x,pr->y,pr->z,largest_area);
240 putText( input, str, mc, CV_FONT_HERSHEY_COMPLEX, 0.4, Scalar(255,255,255), 1, 8);
245 out_msg.
image = input;
270 inv_H = config.invert_Hue;
276 bool switch_pcl_topic(robotican_msgs_srvs::switch_topic::Request &req, robotican_msgs_srvs::switch_topic::Response &res) {
284 int main(
int argc,
char **argv) {
286 ros::init(argc, argv,
"find_objects_node");
295 dynamic_reconfigure::Server<robotican_common::FindObjectDynParamConfig> dynamicServer;
296 dynamic_reconfigure::Server<robotican_common::FindObjectDynParamConfig>::CallbackType callbackFunction;
299 dynamicServer.setCallback(callbackFunction);
303 result_image_pub = it_.
advertise(
"result", 1);
304 object_image_pub = it_.
advertise(
"hsv_filterd", 1);
309 object_pub=n.
advertise<ar_track_alvar_msgs::AlvarMarkers>(
"detected_objects", 2,
true);
311 pose_pub=pn.
advertise<geometry_msgs::PoseStamped>(
"object_pose",10);
315 listener_ptr=&listener;
318 point_sub_.
subscribe(pn,
"object_pose", 10);
void on_trackbar(int, void *)
tf::TransformListener * listener_ptr
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool find_object(Mat input, pcl::PointCloud< pcl::PointXYZRGBA >::Ptr cloud, Point3d *obj, std::string frame)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
bool switch_pcl_topic(robotican_msgs_srvs::switch_topic::Request &req, robotican_msgs_srvs::switch_topic::Response &res)
void arm_msgCallback(const boost::shared_ptr< const geometry_msgs::PoseStamped > &point_ptr)
const std::string TYPE_8UC1
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
void cloud_cb(const sensor_msgs::PointCloud2ConstPtr &input)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
void dynamicParamCallback(robotican_common::FindObjectDynParamConfig &config, uint32_t level)
CvImagePtr cvtColor(const CvImageConstPtr &source, const std::string &encoding)
void publish(const sensor_msgs::Image &message) const
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
std::string getTopic() const
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
image_transport::Publisher bw_image_pub
image_transport::Publisher result_image_pub
#define ROS_INFO_STREAM(args)
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
static geometry_msgs::Quaternion createQuaternionMsgFromRollPitchYaw(double roll, double pitch, double yaw)
void toROSMsg(const sensor_msgs::PointCloud2 &cloud, sensor_msgs::Image &image)
void obj_msgCallback(const boost::shared_ptr< const geometry_msgs::PoseStamped > &point_ptr)
ros::Publisher object_pub
ROSCPP_DECL void spinOnce()
sensor_msgs::ImagePtr toImageMsg() const
int main(int argc, char **argv)
image_transport::Publisher object_image_pub
Connection registerCallback(const C &callback)