Go to the documentation of this file.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 <boost/lexical_cast.hpp>
00019 #include <segbot_controller/qnode.hpp>
00020
00021
00022
00023
00024
00025 namespace segbot_controller {
00026
00027
00028
00029
00030
00031 QNode::QNode(int argc, char** argv ) :
00032 init_argc(argc),
00033 init_argv(argv)
00034 {
00035 init();
00036 client.reset(new actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction>("move_base", true));
00037 }
00038
00039 QNode::~QNode() {
00040 if(ros::isStarted()) {
00041 ros::shutdown();
00042 ros::waitForShutdown();
00043 }
00044 wait();
00045 }
00046
00047 void QNode::goalpub(const geometry_msgs::PoseStamped::ConstPtr msg) {
00048
00049 move_base_msgs::MoveBaseGoal newmovebasegoal;
00050
00051
00052 newmovebasegoal.target_pose.header = msg->header;
00053 newmovebasegoal.target_pose.pose = msg->pose;
00054
00055
00056 mostRecentGoal.reset(new move_base_msgs::MoveBaseGoal);
00057 *mostRecentGoal = newmovebasegoal;
00058 client->sendGoal(newmovebasegoal);
00059 }
00060
00061 bool QNode::init() {
00062 ros::init(init_argc,init_argv,"segbot_controller");
00063 if ( ! ros::master::check() ) {
00064 return false;
00065 }
00066 ros::start();
00067
00068 ros::NodeHandle n;
00069
00070 sub.reset(new ros::Subscriber());
00071 *sub = n.subscribe("move_base_simple/interm_goal", 1000, &QNode::goalpub, this);
00072
00073 start();
00074 return true;
00075 }
00076
00077 void QNode::stop()
00078 {
00079 client->cancelGoal();
00080 }
00081
00082
00083 void QNode::cont()
00084 {
00085 client->sendGoal(*mostRecentGoal);
00086 }
00087
00088
00089 void QNode::run() {
00090 ros::spin();
00091 std::cout << "Ros shutdown, proceeding to close the gui." << std::endl;
00092
00093 }
00094
00095
00096 void QNode::log( const LogLevel &level, const std::string &msg) {
00097 logging_model.insertRows(logging_model.rowCount(),1);
00098 std::stringstream logging_model_msg;
00099 switch ( level ) {
00100 case(Debug) : {
00101 ROS_DEBUG_STREAM(msg);
00102 logging_model_msg << "[DEBUG] [" << ros::Time::now() << "]: " << msg;
00103 break;
00104 }
00105 case(Info) : {
00106 ROS_INFO_STREAM(msg);
00107 logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
00108 break;
00109 }
00110 case(Warn) : {
00111 ROS_WARN_STREAM(msg);
00112 logging_model_msg << "[INFO] [" << ros::Time::now() << "]: " << msg;
00113 break;
00114 }
00115 case(Error) : {
00116 ROS_ERROR_STREAM(msg);
00117 logging_model_msg << "[ERROR] [" << ros::Time::now() << "]: " << msg;
00118 break;
00119 }
00120 case(Fatal) : {
00121 ROS_FATAL_STREAM(msg);
00122 logging_model_msg << "[FATAL] [" << ros::Time::now() << "]: " << msg;
00123 break;
00124 }
00125 }
00126 QVariant new_row(QString(logging_model_msg.str().c_str()));
00127 logging_model.setData(logging_model.index(logging_model.rowCount()-1),new_row);
00128
00129 }
00130
00131 }