00001
00009
00010
00011
00012
00013 #include <ros/ros.h>
00014 #include <ros/network.h>
00015 #include <string>
00016 #include <std_msgs/String.h>
00017 #include <sstream>
00018 #include "../include/drc_gui/qnode.hpp"
00019
00020
00021
00022
00023
00024 namespace drc_gui {
00025
00026
00027
00028
00029
00030 QNode::QNode(int argc, char** argv ) :
00031 init_argc(argc),
00032 init_argv(argv),
00033 mapFrameId("/map"),
00034 objectFramePrefix("object")
00035 {
00036 }
00037
00038 QNode::~QNode() {
00039 if(ros::isStarted()) {
00040 ros::shutdown();
00041 ros::waitForShutdown();
00042 }
00043 wait();
00044 }
00045
00046 bool QNode::init() {
00047 if ( ! ros::master::check() ) {
00048 return false;
00049 }
00050 ros::start();
00051
00052 ros::NodeHandle pnh("~");
00053 pnh.param("map_frame_id", mapFrameId, mapFrameId);
00054 pnh.param("object_prefix", objectFramePrefix, objectFramePrefix);
00055
00056 ros::NodeHandle n;
00057 sub_object_tf = n.subscribe("objectsStamped", 1, &QNode::callback_object_tf, this);
00058 pub_object_pos = n.advertise<drc_gui::SendPos>("send_pos_topic", 2);
00059
00060 ac = new MoveBaseClient("/move_base", true);
00061 if(!ac->waitForServer(ros::Duration(0,0))){
00062 ROS_ERROR("Waiting for the move_base action server to come up");
00063 return false;
00064 }else{
00065 ROS_INFO("Success to connect move_base action server");
00066 }
00067
00068 start();
00069 return true;
00070 }
00071
00072 void QNode::publishPos(){
00073 drc_gui::SendPos msg;
00074
00075 msg.x = camera_pos[0];
00076 msg.y = camera_pos[1];
00077 msg.z = camera_pos[2];
00078 pub_object_pos.publish(msg);
00079 }
00080
00081 void QNode::callback_object_tf(const ObjectsStampedConstPtr &msg){
00082 if(msg->objects.data.size()){
00083 for(unsigned int i=0; i<msg->objects.data.size(); i+=12)
00084 {
00085
00086 int id = (int)msg->objects.data[i];
00087 std::string objectFrameId = QString("%1_%2").arg(objectFramePrefix.c_str()).arg(id).toStdString();
00088
00089 tf::StampedTransform pose;
00090 tf::StampedTransform poseCam;
00091 try
00092 {
00093
00094
00095 tfListener.lookupTransform(mapFrameId, objectFrameId, msg->header.stamp, pose);
00096 tfListener.lookupTransform(msg->header.frame_id, objectFrameId, msg->header.stamp, poseCam);
00097 }
00098 catch(tf::TransformException & ex)
00099 {
00100 ROS_WARN("%s",ex.what());
00101 continue;
00102 }
00103
00104
00105 ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
00106 id, mapFrameId.c_str(),
00107 pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(),
00108 pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z(), pose.getRotation().w());
00109 ROS_INFO("Object_%d [x,y,z] [x,y,z,w] in \"%s\" frame: [%f,%f,%f] [%f,%f,%f,%f]",
00110 id, msg->header.frame_id.c_str(),
00111 poseCam.getOrigin().x(), poseCam.getOrigin().y(), poseCam.getOrigin().z(),
00112 poseCam.getRotation().x(), poseCam.getRotation().y(), poseCam.getRotation().z(), poseCam.getRotation().w());
00113
00114 global_pos[0] = pose.getOrigin().x();
00115 global_pos[1] = pose.getOrigin().y();
00116 global_pos[2] = pose.getOrigin().z();
00117
00118 global_ori[0] = pose.getRotation().x();
00119 global_ori[1] = pose.getRotation().y();
00120 global_ori[2] = pose.getRotation().z();
00121 global_ori[3] = pose.getRotation().w();
00122
00123 camera_pos[0] = poseCam.getOrigin().x();
00124 camera_pos[1] = poseCam.getOrigin().y();
00125 camera_pos[2] = poseCam.getOrigin().z();
00126
00127 camera_ori[0] = poseCam.getRotation().x();
00128 camera_ori[1] = poseCam.getRotation().y();
00129 camera_ori[2] = poseCam.getRotation().z();
00130 camera_ori[3] = poseCam.getRotation().w();
00131 }
00132 }
00133 Q_EMIT sig_new_object_tf();
00134 }
00135
00136 void QNode::run() {
00137 ros::Rate loop_rate(1);
00138 int count = 0;
00139 while ( ros::ok() ) {
00140
00141 ros::spinOnce();
00142 loop_rate.sleep();
00143 ++count;
00144 }
00145 std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00146 Q_EMIT rosShutdown();
00147 }
00148
00149
00150 }