诺耕毫末底盘线控调试

src/ros/catkin/src/controllertocan_haomo/src/control_status.cpp

@@ -9,7 +9,8 @@ void iv::control::ControlStatus::set_brake(float brake)
 
 void iv::control::ControlStatus::set_drivespeed(float speed)
 {
-    command11.bit.drive_speed = (unsigned)speed;
+//    std::cout<<" speed "<<speed<<std::endl;
+    command11.bit.drive_speed = (unsigned)(speed*3.6);
 }
 
 void iv::control::ControlStatus::set_gear(int gear)

src/ros/catkin/src/controllertocan_haomo/src/haomo_code.cpp

@@ -28,10 +28,16 @@ void HaoMoCode::Code(const autoware_msgs::ControlCommandStampedConstPtr &cmdmsg,
 
     double fVel = cmdmsg->cmd.linear_velocity;
 
+//    std::cout<<" vel "<<fVel<<std::endl;
+
     xcontroller->control_brake(0);
     xcontroller->control_wheel(cmdmsg->cmd.steering_angle * (180.0 / M_PI) * 15.0);
     xcontroller->control_drivespeed(fVel);
 
+    xcontroller->cmd_checksum(0x10);
+    xcontroller->cmd_checksum(0x11);
+    xcontroller->cmd_checksum(0x12);
+
     adc_system_msg::canraw xraw10;
     xraw10.data.resize(8);
     xraw10.bext = false;
@@ -53,7 +59,8 @@ void HaoMoCode::Code(const autoware_msgs::ControlCommandStampedConstPtr &cmdmsg,
     xraw11.index = nindex;
     nindex++;
     xraw11.channel = 0;
-    memcpy(xraw11.data.data(), (char *)ServiceControlStatus.command10.byte, 8);
+    memcpy(xraw11.data.data(), (char *)ServiceControlStatus.command11.byte, 8);
+//    std::cout<<" vv "<<(int)xraw11.data[3]<<std::endl;
     xmsg.rawmsg.push_back(xraw11);
 
     adc_system_msg::canraw xraw12;

src/ros/catkin/src/driver_can_nvidia/src/canctrl.cpp

@@ -88,6 +88,7 @@ void canctrl::run()
         bool bCanOpen = mpcan->IsOpen();
         if(bCanOpen)
         {
+//            std::cout<<" have open "<<std::endl;
             basecan_msg xmsg[2500];
             int nRec1,nRec2,nSend1,nSend2;
             if((nRec1 =mpcan->GetMessage(0,xmsg,2500))>0)
@@ -234,6 +235,7 @@ void canctrl::sendmsg(int index,const adc_system_msg::canmsg & xmsg)
 
     void canctrl::CallbackSend1(const adc_system_msg::canmsg & xmsg)
     {
+ //           std::cout<<"cmd send msg"<<std::endl;
             sendmsg(0,xmsg);
     }
     void canctrl::CallbackSend2(const adc_system_msg::canmsg & xmsg)

src/ros/catkin/src/driver_can_nvidia/src/nvcan.cpp

@@ -76,6 +76,11 @@ nvcan::nvcan(QWaitCondition * pwc, const char * strcan0name,const char * strcan1
 
 }
 
+bool nvcan::IsOpen()
+{
+    return mbCANOpen;
+}
+
 int nvcan::ExecOpen(int &s, const char *strcanname)
 {
     struct sockaddr_can addr;
@@ -352,6 +357,7 @@ void nvcan::run()
         bRecv = false;
 
         int nrecv1 = ExecRecv(s[0],rdfs,0);
+        
         if(nrecv1 >0)bRecv = true;
         else
         {
@@ -364,6 +370,7 @@ void nvcan::run()
             if(nrecv2<0)std::cout<<"CAN Recv Error."<<std::endl;
         }
 
+//        if(bRecv)std::cout<<"recv data."<<std::endl;
         if(bRecv)continue;
 
         mWaitMutex.lock();
@@ -426,6 +433,7 @@ void nvcan::run()
                         }
                         else
                         {
+ //                           std::cout<<" exec send. "<<std::endl;
                             bSend = true;
 
                         #ifdef SEND_STAT

src/ros/catkin/src/driver_can_nvidia/src/nvcan.h

@@ -25,6 +25,7 @@ public:
     int SetMessage(const int nch,basecan_msg * pMsg);
 
     virtual void CmdSend();
+    virtual bool IsOpen();
 
 private slots:
     void onMsg(bool bCAN,int nR,const char * strres);

src/ros/catkin/src/rtk_nav992/CMakeLists.txt

@@ -3,7 +3,7 @@ project(rtk_nav992)
 
 
 message(STATUS  "Enter rtk_nav992")
-add_compile_options(-std=c++11)
+add_compile_options(-std=c++14)
 
 SET(QT_PATH  /opt/qt/5.13.2/gcc_64)
 SET(AGX_QT_PATH  /usr/include/aarch64-linux-gnu/qt5)

src/ros/catkin/src/rtk_nav992/src/main.cpp

@@ -26,6 +26,7 @@
 #include "gnss_coordinate_convert.h"
 
 static std::string _pose_topic = "/current_pose";
+static std::string _poseloc_topic = "/localizer_pose";
 static std::string _twist_topic = "/current_velocity";
 
 static double glon0 = 117.0273054;
@@ -33,6 +34,7 @@ static double glat0 = 39.1238777;
 static double ghead0 = 0;
 
 ros::Publisher pose_pub;
+ros::Publisher poseloc_pub;
 ros::Publisher twist_pub;
 
 ros::Subscriber navsatfix_sub;
@@ -146,7 +148,7 @@ int main(int argc, char **argv)
     ros::NodeHandle private_nh("~");
     ros::Rate loop_rate(100);
 
-    QUdpSocket *mudpSocketGPSIMU;
+    QUdpSocket *mudpSocketGPSIMU = new QUdpSocket();
 
     private_nh.getParam("pose_topic", _pose_topic);
     private_nh.getParam("twist_topic", _twist_topic);
@@ -160,6 +162,7 @@ int main(int argc, char **argv)
     std::cout << "Lat0 : " << glat0 << std::endl;
 
     pose_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_pose_topic, 10);
+    poseloc_pub = private_nh.advertise<geometry_msgs::PoseStamped>(_poseloc_topic, 10);
     twist_pub = private_nh.advertise<geometry_msgs::TwistStamped>(_twist_topic, 10);
 
     //Topic you want to subscribe
@@ -167,6 +170,8 @@ int main(int argc, char **argv)
     twist_sub = private_nh.subscribe("/nav992_device/Twist", 1, callback_twist);
     imu_sub = private_nh.subscribe("/nav992_device/Imu", 1, callback_imu);
 
+    
+
     while (ros::ok())
     {
         ros::spinOnce();
@@ -222,12 +227,15 @@ int main(int argc, char **argv)
         if (b_navsatfix == true && b_twist == true && b_imu == true)
         {
             pose_pub.publish(xPose);
+            poseloc_pub.publish(xPose);
             twist_pub.publish(xtwist);
             b_navsatfix = false;
             b_twist = false;
             b_imu = false;
         }
 
+//        std::cout<<"run "<<std::endl;
+
 ////////////////////////////////===UDP广播开始===///////////////////////////////////////////
         unsigned char strSend[72];
         int i;