00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include <ros/node.h>
00036 #include <door_msgs/Door.h>
00037 #include <checkerboard_detector/ObjectDetection.h>
00038
00039 #include <tf/tf.h>
00040 #include <tf/transform_datatypes.h>
00041 #include <tf/transform_listener.h>
00042
00043 #include <door_handle_detector/DoorDetector.h>
00044
00045 using namespace checkerboard_detector;
00046 using namespace tf;
00047
00048 class DoorCheckerboardDetectorNode : public ros::Node
00049 {
00050 public:
00051 std::string listen_topic_, publish_topic_,frame_id_;
00052 checkerboard_detector::ObjectDetection checkerboard_msg_;
00053 double door_width_;
00054 double door_checkerboard_x_offset_, door_checkerboard_z_offset_, checkerboard_handle_z_offset_, checkerboard_handle_x_offset_;
00055 tf::TransformListener tf_;
00056 door_msgs::Door door_msg_;
00057
00058 DoorCheckerboardDetectorNode(std::string node_name):ros::Node(node_name),tf_(*this, false, ros::Duration(10))
00059 {
00060 this->param<std::string>("door_checkerboard_detector/listen_topic",listen_topic_,"/checkerdetector/ObjectDetection");
00061 this->param<std::string>("door_checkerboard_detector/publish_topic",publish_topic_,"door_location");
00062
00063 this->param<std::string>("door_checkerboard_detector/frame_id",frame_id_,"base_link");
00064
00065 this->param<double>("door_checkerboard_detector/door_width",door_width_,0.9);
00066 this->param<double>("door_checkerboard_detector/door_checkerboard_x_offset",door_checkerboard_x_offset_,0.073);
00067 this->param<double>("door_checkerboard_detector/door_checkerboard_z_offset",door_checkerboard_z_offset_,1.47);
00068
00069 this->param<double>("door_checkerboard_detector/checkerboard_handle_z_offset",checkerboard_handle_z_offset_,0.0);
00070 this->param<double>("door_checkerboard_detector/checkerboard_handle_x_offset",checkerboard_handle_x_offset_,0.0);
00071
00072 subscribe(listen_topic_, checkerboard_msg_, &DoorCheckerboardDetectorNode::doorCallback,1);
00073 advertise<door_msgs::Door>(publish_topic_,1);
00074 advertiseService("door_handle_checkerboard_detector", &DoorCheckerboardDetectorNode::detectDoor, this);
00075
00076 }
00077
00078 ~DoorCheckerboardDetectorNode()
00079 {
00080 unadvertise(publish_topic_);
00081 unadvertiseService("door_handle_checkerboard_detector");
00082 unsubscribe(listen_topic_);
00083 }
00084
00085
00086 void transformTFPoint(const std::string &frame, const Stamped<tf::Point> &in, Stamped<tf::Point> &out)
00087 {
00088 try{
00089 tf_.transformPoint(frame,in,out);
00090 }
00091 catch(tf::LookupException& ex) {
00092 ROS_INFO("No Transform available Error\n");
00093 return;
00094 }
00095 catch(tf::ConnectivityException& ex) {
00096 ROS_INFO("Connectivity Error\n");
00097 return;
00098 }
00099 catch(tf::ExtrapolationException& ex) {
00100 ROS_INFO("Extrapolation Error %s\n", ex.what());
00101 return;
00102 }
00103 catch(tf::TransformException e) {
00104 return;
00105 }
00106 }
00107
00108 void pointTFToMsg32(const tf::Vector3 &in, geometry_msgs::Point32 &out)
00109 {
00110 out.x = in.x();
00111 out.y = in.y();
00112 out.z = in.z();
00113 }
00114
00115 bool detectDoor(door_handle_detector::DoorDetector::Request &req, door_handle_detector::DoorDetector::Response &resp)
00116 {
00117 resp.door = door_msg_;
00118 return true;
00119 }
00120
00121 void doorCallback()
00122 {
00123
00124 tf::Pose door_pose;
00125 Stamped<tf::Point> frame_p1, frame_p2, door_p1, door_p2, handle;
00126 Stamped<tf::Point> target;
00127
00128 std::string dummy = "";
00129
00130 frame_p1.frame_id_ = checkerboard_msg_.header.frame_id;
00131 frame_p2.frame_id_ = checkerboard_msg_.header.frame_id;
00132 door_p1.frame_id_ = checkerboard_msg_.header.frame_id;
00133 door_p2.frame_id_ = checkerboard_msg_.header.frame_id;
00134 handle.frame_id_ = checkerboard_msg_.header.frame_id;
00135
00136 if(checkerboard_msg_.get_objects_size() > 0)
00137 {
00138 door_msg_.header.stamp = checkerboard_msg_.header.stamp;
00139 door_msg_.header.frame_id = frame_id_;
00140 tf::poseMsgToTF(checkerboard_msg_.objects[0].pose,door_pose);
00141
00142 door_p1 = Stamped<tf::Point>(door_pose*Point(-door_checkerboard_x_offset_,door_checkerboard_z_offset_,0.0),checkerboard_msg_.header.stamp, checkerboard_msg_.header.frame_id,dummy);
00143 door_p2 = Stamped<tf::Point>(door_pose*Point(door_width_ - door_checkerboard_x_offset_,door_checkerboard_z_offset_,0.0), checkerboard_msg_.header.stamp, checkerboard_msg_.header.frame_id, dummy);
00144
00145 ROS_INFO("Door to checkerboard offsets: %f %f",door_checkerboard_x_offset_,door_checkerboard_z_offset_);
00146 ROS_INFO("Checkerboard to handle offsets: %f %f",checkerboard_handle_x_offset_,checkerboard_handle_z_offset_);
00147
00148 ROS_INFO("Transforming from %s to %s", door_p1.frame_id_.c_str(),frame_id_.c_str());
00149 ROS_INFO("door_p1 : %f %f %f", door_p1.m_floats[0], door_p1.m_floats[1], door_p1.m_floats[2]);
00150 ROS_INFO("checkerboard frame : %f %f %f", checkerboard_msg_.objects[0].pose.position.x, checkerboard_msg_.objects[0].pose.position.y, checkerboard_msg_.objects[0].pose.position.z);
00151 frame_p1 = door_p1;
00152 frame_p2 = door_p2;
00153 sleep(1);
00154 handle = Stamped<tf::Point>(door_pose*Point(checkerboard_handle_x_offset_,checkerboard_handle_z_offset_,0.0),checkerboard_msg_.header.stamp, checkerboard_msg_.header.frame_id,dummy);
00155
00156 transformTFPoint(frame_id_,door_p1,target);
00157 ROS_INFO("door_p1 transformed : %f %f %f", target.m_floats[0], target.m_floats[1], target.m_floats[2]);
00158 pointTFToMsg32(target,door_msg_.door_p1);
00159
00160 transformTFPoint(frame_id_,door_p2,target);
00161 pointTFToMsg32(target,door_msg_.door_p2);
00162
00163 transformTFPoint(frame_id_,frame_p1,target);
00164 pointTFToMsg32(target,door_msg_.frame_p1);
00165
00166 transformTFPoint(frame_id_,frame_p2,target);
00167 pointTFToMsg32(target,door_msg_.frame_p2);
00168
00169 transformTFPoint(frame_id_,handle,target);
00170 pointTFToMsg32(handle,door_msg_.handle);
00171
00172 publish(publish_topic_,door_msg_);
00173 }
00174 }
00175 };
00176
00177 int main(int argc, char** argv)
00178 {
00179 ros::init(argc,argv);
00180
00181 DoorCheckerboardDetectorNode node("test_door_detection");
00182
00183 try {
00184 node.spin();
00185 }
00186 catch(char const* e){
00187 std::cout << e << std::endl;
00188 }
00189
00190 return(0);
00191 }