|
|
@@ -9,7 +9,9 @@
|
|
|
#include "cyber/time/time.h"
|
|
|
#include "shenlanfd.h"
|
|
|
#include <canmsg.pb.h>
|
|
|
-#include "chassis.pb.h"
|
|
|
+#include "adchassis.pb.h"
|
|
|
+#include "canmsg.pb.h"
|
|
|
+#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
|
|
|
|
|
|
static bool gbthreadrun = true;
|
|
|
static std::thread * gpthreadsend;
|
|
|
@@ -23,6 +25,69 @@ static std::shared_ptr<apollo::cyber::Reader<iv::can::canmsg>>
|
|
|
can0recv_reader_;
|
|
|
static std::shared_ptr<apollo::cyber::Reader<iv::chassis>>
|
|
|
chassis_reader_;
|
|
|
+static std::shared_ptr<apollo::cyber::Reader<apollo::control::ControlCommand>>
|
|
|
+ controller_reader_;
|
|
|
+
|
|
|
+static void ExecSend()
|
|
|
+{
|
|
|
+ static int nCount = 0;
|
|
|
+ static int gnIndex = 0;
|
|
|
+ nCount++;
|
|
|
+ iv::can::canmsg xmsg;
|
|
|
+ iv::can::canraw xraw;
|
|
|
+ unsigned char byte_1C4[32];
|
|
|
+ gpShenlanfd->Get1C4(byte_1C4);
|
|
|
+
|
|
|
+ xraw.set_id(0x1C4);
|
|
|
+ xraw.set_data(byte_1C4,32);
|
|
|
+ xraw.set_bext(false);
|
|
|
+ xraw.set_bremote(false);
|
|
|
+ xraw.set_len(32);
|
|
|
+ iv::can::canraw * pxraw1C4 = xmsg.add_rawmsg();
|
|
|
+ pxraw1C4->CopyFrom(xraw);
|
|
|
+// qDebug(" 0x144: %02X %02X %02X %02X %02X %02X %02X %02X",byte_144[0],byte_144[1],byte_144[2],byte_144[3],
|
|
|
+// byte_144[4],byte_144[5],byte_144[6],byte_144[7]);
|
|
|
+ xmsg.set_channel(0);
|
|
|
+ xmsg.set_index(gnIndex);
|
|
|
+
|
|
|
+ unsigned char byte_24E[64];
|
|
|
+ gpShenlanfd->Get1C4(byte_24E);
|
|
|
+ xraw.set_id(0x24E);
|
|
|
+ xraw.set_data(byte_24E,64);
|
|
|
+ xraw.set_bext(false);
|
|
|
+ xraw.set_bremote(false);
|
|
|
+ xraw.set_len(64);
|
|
|
+ if(nCount%2 == 1)
|
|
|
+ {
|
|
|
+ iv::can::canraw * pxraw24E = xmsg.add_rawmsg();
|
|
|
+ pxraw24E->CopyFrom(xraw);
|
|
|
+ }
|
|
|
+ xmsg.set_channel(0);
|
|
|
+ xmsg.set_index(gnIndex);
|
|
|
+
|
|
|
+ unsigned char byte_25E[32];
|
|
|
+ gpShenlanfd->Get1C4(byte_25E);
|
|
|
+ xraw.set_id(0x25E);
|
|
|
+ xraw.set_data(byte_25E,32);
|
|
|
+ xraw.set_bext(false);
|
|
|
+ xraw.set_bremote(false);
|
|
|
+ xraw.set_len(32);
|
|
|
+// if(nCount == 10)
|
|
|
+ if(nCount%2 == 1) //25Ede zhouqi shi 20ms
|
|
|
+ {
|
|
|
+ iv::can::canraw * pxraw25E = xmsg.add_rawmsg();
|
|
|
+ pxraw25E->CopyFrom(xraw);
|
|
|
+// nCount = 0;
|
|
|
+ }
|
|
|
+ xmsg.set_channel(0);
|
|
|
+ xmsg.set_index(gnIndex);
|
|
|
+
|
|
|
+ std::shared_ptr<iv::can::canmsg> canmsg_ptr;
|
|
|
+ iv::can::canmsg * pcanmsg = new iv::can::canmsg();
|
|
|
+ pcanmsg->CopyFrom(xmsg);
|
|
|
+ canmsg_ptr.reset(pcanmsg);
|
|
|
+ can0send_writer_->Write(canmsg_ptr);
|
|
|
+}
|
|
|
|
|
|
void threadsend()
|
|
|
{
|
|
|
@@ -31,7 +96,8 @@ void threadsend()
|
|
|
|
|
|
while(gbthreadrun)
|
|
|
{
|
|
|
- std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
|
|
+ std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
|
|
+ ExecSend();
|
|
|
}
|
|
|
|
|
|
std::cout<<" threadsendrecv exit."<<std::endl;
|
|
|
@@ -47,6 +113,62 @@ void RecvChassis(const std::shared_ptr<iv::chassis> &xchassis)
|
|
|
gpShenlanfd->SetSpeed(xchassis->vel());
|
|
|
}
|
|
|
|
|
|
+void RecvController(const std::shared_ptr<apollo::control::ControlCommand> &xcmd)
|
|
|
+{
|
|
|
+ double facc = 0;
|
|
|
+ double ftorque = 0;
|
|
|
+ double fbrake = xcmd->brake();
|
|
|
+ double fwheelangle = 0;
|
|
|
+
|
|
|
+ facc = xcmd->acceleration();
|
|
|
+
|
|
|
+ double fVehWeight = 1800;
|
|
|
+ double fg = 9.8;
|
|
|
+ double fRollForce = 50;
|
|
|
+ const double fRatio = 2.5;
|
|
|
+ double fNeed = fRollForce + fVehWeight*facc;
|
|
|
+
|
|
|
+ ftorque = fNeed/fRatio;
|
|
|
+
|
|
|
+ if(fbrake>0.0001)
|
|
|
+ {
|
|
|
+ fbrake = fbrake * (-5.0) /100.0;
|
|
|
+ ftorque = 0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ftorque<0)ftorque = 0;
|
|
|
+
|
|
|
+
|
|
|
+
|
|
|
+// facc = xctrlcmd.linear_acceleration();
|
|
|
+ fwheelangle = xcmd->steering_target() * 430.0/100.0;
|
|
|
+ if(fwheelangle>430)fwheelangle = 430;
|
|
|
+ if(fwheelangle<-430)fwheelangle = -430;
|
|
|
+
|
|
|
+ iv::dcs xdcs;
|
|
|
+ xdcs.mfBrake = fbrake;
|
|
|
+ xdcs.mfTorque = ftorque;
|
|
|
+ xdcs.mfWheel = fwheelangle;
|
|
|
+ xdcs.mblampleft = false; xdcs.mblampright = false;
|
|
|
+ if(xcmd->has_signal())
|
|
|
+ {
|
|
|
+ if(xcmd->signal().has_turn_signal())
|
|
|
+ {
|
|
|
+ if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_LEFT)
|
|
|
+ {
|
|
|
+ xdcs.mblampleft = true;
|
|
|
+ }
|
|
|
+ if(xcmd->signal().turn_signal() == apollo::common::VehicleSignal_TurnSignal::VehicleSignal_TurnSignal_TURN_RIGHT)
|
|
|
+ {
|
|
|
+ xdcs.mblampright = true;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ gpShenlanfd->SetDecision(xdcs);
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
int main(int argc, char *argv[])
|
|
|
{
|
|
|
// QCoreApplication a(argc, argv);
|
|
|
@@ -68,6 +190,13 @@ int main(int argc, char *argv[])
|
|
|
}
|
|
|
);
|
|
|
|
|
|
+ controller_reader_ = pilot_node->CreateReader<apollo::control::ControlCommand>(
|
|
|
+ "/aa",
|
|
|
+ [](const std::shared_ptr<apollo::control::ControlCommand> &xcmd){
|
|
|
+ RecvController(xcmd);
|
|
|
+ }
|
|
|
+ );
|
|
|
+
|
|
|
|
|
|
|
|
|
gpShenlanfd = new Shenlanfd();
|
