new

include/cyrobot_monitor/qnode.hpp

@@ -67,6 +67,7 @@ public:
   void set_goal(QString frame,double x,double y,double z,double w);
   void Sub_Image(QString topic,int frame_id);
   void pub_imageMap(QImage map);
+  double getRealTheta(QPointF start,QPointF end);
   QPointF transScenePoint2Map(QPointF pos);
   QPointF transMapPoint2Scene(QPointF pos);
   QMap<QString,QString> get_topic_list();

include/cyrobot_monitor/robomap.h

@@ -12,10 +12,17 @@
 #include <opencv2/highgui/highgui.hpp>
 #include <QCursor>
 #include <QtMath>
+
+namespace cyrobot_monitor {
+
 class roboMap  : public QObject, public QGraphicsItem
 {
     Q_OBJECT
+//角度转弧度
+#define degreesToradian(x) (M_PI*x/180.0)
 
+//弧度转角度
+#define radiansToDegrees(x) (180.0*x/M_PI)
 public:
   roboMap();
   QRectF  boundingRect() const;
@@ -49,9 +56,12 @@ public:
   void move(double x,double y);
   QCursor *moveCursor=NULL;
   QCursor *set2DPoseCursor=NULL;
+  QCursor *set2DGoalCursor=NULL;
   QCursor *currCursor=NULL;
 signals:
   void cursorPos(QPointF);
+  void set2DPos(QPointF start,QPointF end);
+  void set2DGoal(QPointF point,QPointF end);
 public slots:
     void paintMaps(QImage map);
     void paintRoboPos(QPointF pos,float yaw);
@@ -78,5 +88,5 @@ private:
     qreal       m_scaleDafault=1;
 
 };
-
+}
 #endif // ROBOMAP_H

src/main_window.cpp

@@ -416,22 +416,40 @@ void MainWindow::slot_return_point()
 //设置导航当前位置按钮的槽函数
 void MainWindow::slot_set_2D_Pos()
 {
-  emit signalSet2DPose();
+  if(ui.comboBox_mapType->currentIndex()){
+    map_rviz->Set_Pos();
+  }
+  else{
+      emit signalSet2DPose();
+  }
+
 // ui.label_map_msg->setText("请在地图中选择机器人的初始位置");
 }
 //设置导航目标位置按钮的槽函数
 void MainWindow::slot_set_2D_Goal()
 {
-  emit signalSet2DGoal();
+  if(ui.comboBox_mapType->currentIndex()){
+     map_rviz->Set_Goal();
+  }else{
+     emit signalSet2DGoal();
+  }
+
 //  ui.label_map_msg->setText("请在地图中选择机器人导航的目标位置");
 }
 void MainWindow::slot_move_camera_btn()
 {
-  emit signalSetMoveCamera();
+    if(ui.comboBox_mapType->currentIndex()){
+       map_rviz->Set_MoveCamera();
+    }else{
+       emit signalSetMoveCamera();
+    }
+
 }
 void MainWindow::slot_set_select()
 {
-
+  if(ui.comboBox_mapType->currentIndex()){
+   map_rviz->Set_Select();
+  }
 }
 
 

src/qnode.cpp

@@ -280,7 +280,7 @@ void QNode::speedCallback(const nav_msgs::Odometry::ConstPtr& msg)
     emit speed_y(msg->twist.twist.linear.y);
 }
 void QNode::run() {
-        ros::Rate loop_rate(30);
+        ros::Rate loop_rate(20);
         //当当前节点没有关闭时
         while ( ros::ok() ) {
             //调用消息处理回调函数
@@ -366,6 +366,35 @@ QPointF QNode::transMapPoint2Scene(QPointF pos){
   roboPos.setY(-1*(pos.y()-m_mapCenterPoint.y())/m_mapResolution+m_sceneCenterPoint.y());
   return roboPos;
 }
+double QNode::getRealTheta(QPointF start,QPointF end){
+  double y=end.y()-start.y();
+  double x=end.x()-start.x();
+  double theta=radiansToDegrees(atan(y/x));
+  qDebug()<<start<<" "<<end<<" "<<theta;
+  // 1 4
+  if(end.x()>start.x()){
+
+     // 1
+    if(end.y()>start.y()){
+      theta = -theta;
+    }
+    // 4
+    else {
+       theta = 270 - theta;
+    }
+  }else {
+  // 2 3
+     theta = 180- theta;
+//    if(end.y()>start.y()){
+//      //2
+//      theta = 180- theta;
+//    }
+//    else {
+
+//    }
+  }
+  return theta;
+}
 void QNode::pub_imageMap(QImage map){
      cv::Mat image = QImage2Mat(map);
      sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image).toImageMsg();

src/robomap.cpp

@@ -1,5 +1,8 @@
 #include "../include/cyrobot_monitor/robomap.h"
 #include <QDebug>
+
+namespace cyrobot_monitor {
+
 roboMap::roboMap()
 {
   setAcceptHoverEvents(true);
@@ -9,6 +12,7 @@ roboMap::roboMap()
   moveBy(0,0);
   moveCursor=new QCursor(QPixmap("://images/cursor_move"),0,0);
   set2DPoseCursor=new QCursor(QPixmap("://images/cursor_pos.png"),0,0);
+  set2DGoalCursor=new QCursor(QPixmap("://images/cursor_pos.png"),0,0);
   setDefault();
 }
 int roboMap::QColorToInt(const QColor& color) {
@@ -51,7 +55,7 @@ void roboMap::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, Q
 
 }
 void roboMap::drawTools(QPainter *painter){
-  if(currCursor == set2DPoseCursor){
+  if(currCursor == set2DPoseCursor || currCursor == set2DGoalCursor){
     //绘制箭头
     if(m_pressedPoint.x()!=0 && m_pressedPoint.y()!=0 &&m_pressingPoint.x()!=0 && m_pressingPoint.y()!=0){
       painter->setPen(QPen(QColor(0, 255, 0, 255), 2));
@@ -205,8 +209,8 @@ void roboMap::slot_set2DPos(){
   currCursor=set2DPoseCursor;
 }
 void roboMap::slot_set2DGoal(){
-  this->setCursor(*set2DPoseCursor); //设置自定义的鼠标样式
-  currCursor=set2DPoseCursor;
+  this->setCursor(*set2DGoalCursor); //设置自定义的鼠标样式
+  currCursor=set2DGoalCursor;
 }
 void roboMap::slot_setMoveCamera(){
   this->setCursor(*moveCursor); //设置自定义的鼠标样式
@@ -244,15 +248,25 @@ void roboMap::mouseMoveEvent(QGraphicsSceneMouseEvent *event){
 void roboMap::hoverMoveEvent(QGraphicsSceneHoverEvent *event){
      emit cursorPos(event->pos());
 }
+
 void roboMap::mouseReleaseEvent(QGraphicsSceneMouseEvent *event){
      m_isPress = false;//标记鼠标左键已经抬起
-     m_pressedPoint=QPointF(0,0);
      //如果是选择点位模式 重置
      if(currCursor == set2DPoseCursor){
        m_isOtherCursor=false;
+       emit set2DPos(m_pressedPoint,m_pressingPoint);
+       m_pressedPoint=QPointF(0,0);
+       m_pressingPoint=QPointF(0,0);
+       this->setCursor(*moveCursor); //设置自定义的鼠标样式
+       currCursor=moveCursor;
+     }else if(currCursor == set2DGoalCursor){
+       m_isOtherCursor=false;
+       emit set2DGoal(m_pressedPoint,m_pressingPoint);
        m_pressedPoint=QPointF(0,0);
        m_pressingPoint=QPointF(0,0);
        this->setCursor(*moveCursor); //设置自定义的鼠标样式
        currCursor=moveCursor;
      }
 }
+
+}