diff --git "a/\033OA" "b/\033OA" new file mode 100644 index 00000000..05d9f14c --- /dev/null +++ "b/\033OA" @@ -0,0 +1,35 @@ +diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +index e4e90fb..2e7a0a8 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +@@ -64,13 +64,13 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track + mpcParams.Q_term = 1.0*[20.0,1.0,10.0,20.0,50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[1,1,1,1,1,1] # cost matrix for derivative cost of states +- mpcParams.QderivU = 1.0*[5.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs ++ mpcParams.QderivU = 1.0*[10.0,1.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay + mpcParams.Q_lane = 1 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity +- mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s ++ mpcParams.Q_slack = 1*[5*20.0,1*20.0,1*10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories +  + elseif selectedStates.simulator == true # if the simulator is in use +diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +index dc2d986..93c7f26 100755 +--- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py ++++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +@@ -224,8 +224,8 @@ def state_estimation(): +  + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) +- #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) +- # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v ++ #R = diag([4*5.0,4*5.0,1.0,10.0,2*100.0,2*1000.0,2*1000.0, 4*5.0,4*5.0,10.0,1.0, 2*10.0,10.0]) ++ # x, y, v, psi,psiDot,a_x,a_y, x, y, psi, v +  + # Set up track parameters + l = Localization() diff --git a/.gitignore b/.gitignore index d4fabeb1..b02f9446 100644 --- a/.gitignore +++ b/.gitignore @@ -65,3 +65,4 @@ controller.py Dator/db.sqlite3 workspace/src/barc/rosbag/ *.ropeproject/ +.DS_Store diff --git a/workspace/src/barc_gui/src/barc_gui/__init__.py b/.gitmodules similarity index 100% rename from workspace/src/barc_gui/src/barc_gui/__init__.py rename to .gitmodules diff --git a/arduino/arduino_nano328_node/arduino_nano328_node.ino b/arduino/arduino_nano328_node/arduino_nano328_node.ino index c5a70d16..c2dfeb2d 100644 --- a/arduino/arduino_nano328_node/arduino_nano328_node.ino +++ b/arduino/arduino_nano328_node/arduino_nano328_node.ino @@ -21,12 +21,13 @@ WARNING: // include libraries #include -#include -#include +//#include +#include #include #include -#include "Maxbotix.h" #include +#include +#include /************************************************************************** CAR CLASS DEFINITION (would like to refactor into car.cpp and car.h but can't figure out arduino build process so far) @@ -48,6 +49,10 @@ class Car { int getEncoderFR(); int getEncoderBL(); int getEncoderBR(); + unsigned long getEncoder_dTime_FL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_FR(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BL(); //(ADDED BY TOMMI 7JULY2016) + unsigned long getEncoder_dTime_BR(); //(ADDED BY TOMMI 7JULY2016) // Interrupt service routines void incFR(); void incFL(); @@ -55,6 +60,12 @@ class Car { void incBL(); void calcThrottle(); void calcSteering(); + float getVelocityEstimate(); + float vel_FL = 0; + float vel_FR = 0; + float vel_BL = 0; + float vel_BR = 0; + private: // Pin assignments const int ENC_FL_PIN = 2; @@ -68,13 +79,13 @@ class Car { // Car properties // unclear what this is for - const int noAction = 0; + const float noAction = 0.0; // Motor limits // TODO are these the real limits? - const int MOTOR_MAX = 120; - const int MOTOR_MIN = 40; - const int MOTOR_NEUTRAL = 90; + const float MOTOR_MAX = 150.0; + const float MOTOR_MIN = 40.0; + const float MOTOR_NEUTRAL = 90.0; // Optional: smaller values for testing safety /* const int MOTOR_MAX = 100; */ /* const int MOTOR_MIN = 75; */ @@ -84,10 +95,10 @@ class Car { // 120] judging from the sound of the servo pushing beyond a mechanical limit // outside that range. The offset may be 2 or 3 deg and the d_theta_max is then // ~31. - const int D_THETA_MAX = 30; - const int THETA_CENTER = 90; - const int THETA_MAX = THETA_CENTER + D_THETA_MAX; - const int THETA_MIN = THETA_CENTER - D_THETA_MAX; + const float D_THETA_MAX = 30.0; + const float THETA_CENTER = 90.0; + const float THETA_MAX = THETA_CENTER + D_THETA_MAX; + const float THETA_MIN = THETA_CENTER - D_THETA_MAX; // Interfaces to motor and steering actuators Servo motor; @@ -122,12 +133,33 @@ class Car { int BL_count = 0; int BR_count = 0; + // Delta time withing two magnets //(ADDED BY TOMMI 7JULY2016) + // F = front, B = back, L = left, R = right //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_new_time = 0; //(ADDED BY TOMMI 7JULY2016) + + volatile unsigned long FL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long FR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BL_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + volatile unsigned long BR_old_time = 0; //(ADDED BY TOMMI 7JULY2016) + + unsigned long FL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long FR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BL_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + unsigned long BR_DeltaTime = 0; //(ADDED BY TOMMI 7JULY2016) + + // Utility functions uint8_t microseconds2PWM(uint16_t microseconds); float saturateMotor(float x); float saturateServo(float x); }; +// Boolean keeping track of whether the Arduino has received a signal from the ECU recently +int received_ecu_signal = 0; + // Initialize an instance of the Car class as car Car car; @@ -137,6 +169,7 @@ Car car; // figure it out, please atone for my sins. void ecuCallback(const barc::ECU& ecu) { car.writeToActuators(ecu); + received_ecu_signal = 1; } void incFLCallback() { car.incFL(); @@ -160,29 +193,18 @@ void calcThrottleCallback() { // Variables for time step volatile unsigned long dt; volatile unsigned long t0; +volatile unsigned long ecu_t0; // Global message variables // Encoder, RC Inputs, Electronic Control Unit, Ultrasound barc::ECU ecu; -barc::ECU rc_inputs; -barc::Encoder encoder; -barc::Ultrasound ultrasound; +barc::Vel_est vel_est; + ros::NodeHandle nh; -ros::Publisher pub_encoder("encoder", &encoder); -ros::Publisher pub_rc_inputs("rc_inputs", &rc_inputs); -ros::Publisher pub_ultrasound("ultrasound", &ultrasound); ros::Subscriber sub_ecu("ecu_pwm", ecuCallback); - - -// Set up ultrasound sensors -/* -Maxbotix us_fr(14, Maxbotix::PW, Maxbotix::LV); // front -Maxbotix us_bk(15, Maxbotix::PW, Maxbotix::LV); // back -Maxbotix us_rt(16, Maxbotix::PW, Maxbotix::LV); // right -Maxbotix us_lt(17, Maxbotix::PW, Maxbotix::LV); // left -*/ +ros::Publisher pub_vel_est("vel_est", &vel_est); // vel est publisher /************************************************************************** ARDUINO INITIALIZATION @@ -191,21 +213,27 @@ void setup() { // Set up encoders, rc input, and actuators car.initEncoders(); - car.initRCInput(); + //car.initRCInput(); car.initActuators(); // Start ROS node nh.initNode(); // Publish and subscribe to topics - nh.advertise(pub_encoder); - nh.advertise(pub_rc_inputs); - nh.advertise(pub_ultrasound); +// nh.advertise(pub_encoder); + //nh.advertise(pub_encoder_dt_FL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_FR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BL); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_encoder_dt_BR); //(ADDED BY TOMMI 7JULY2016) + //nh.advertise(pub_rc_inputs); + //nh.advertise(pub_ultrasound); + nh.advertise(pub_vel_est); nh.subscribe(sub_ecu); // Arming ESC, 1 sec delay for arming and ROS car.armActuators(); t0 = millis(); + ecu_t0 = millis(); } @@ -217,29 +245,28 @@ void loop() { // compute time elapsed (in ms) dt = millis() - t0; + // kill the motor if there is no ECU signal within the last 1s + if( (millis() - ecu_t0) >= 200){ + if(!received_ecu_signal){ + car.killMotor(); + } else{ + received_ecu_signal = 0; + } + ecu_t0 = millis(); + } + if (dt > 50) { car.readAndCopyInputs(); - // TODO make encoder and rc_inputs private properties on car? Then - // car.publishEncoder(); and car.publishRCInputs(); - encoder.FL = car.getEncoderFL(); - encoder.FR = car.getEncoderFR(); - encoder.BL = car.getEncoderBL(); - encoder.BR = car.getEncoderBR(); - pub_encoder.publish(&encoder); - - rc_inputs.motor = car.getRCThrottle(); - rc_inputs.servo = car.getRCSteering(); - pub_rc_inputs.publish(&rc_inputs); - - // publish ultra-sound measurement - /* - ultrasound.front = us_fr.getRange(); - ultrasound.back = us_bk.getRange(); - ultrasound.right = us_rt.getRange(); - ultrasound.left = us_lt.getRange(); - pub_ultrasound.publish(&ultrasound); - */ + vel_est.vel_est = car.getVelocityEstimate(); + vel_est.vel_fl = car.vel_FL; + vel_est.vel_fr = car.vel_FR; + vel_est.vel_bl = car.vel_BL; + vel_est.vel_br = car.vel_BR; + vel_est.header.stamp = nh.now(); + pub_vel_est.publish(&vel_est); // publish estimated velocity + ////////////////////////////////////////////////!!!! + t0 = millis(); } @@ -279,10 +306,10 @@ void Car::initEncoders() { pinMode(ENC_FL_PIN, INPUT_PULLUP); pinMode(ENC_BR_PIN, INPUT_PULLUP); pinMode(ENC_BL_PIN, INPUT_PULLUP); - enableInterrupt(ENC_FR_PIN, incFRCallback, CHANGE); - enableInterrupt(ENC_FL_PIN, incFLCallback, CHANGE); - enableInterrupt(ENC_BR_PIN, incBRCallback, CHANGE); - enableInterrupt(ENC_BL_PIN, incBLCallback, CHANGE); + enableInterrupt(ENC_FR_PIN, incFRCallback, FALLING); //enables interrupts from Pin ENC_FR_PIN, when signal changes (CHANGE). And it call the function 'incFRCallback' + enableInterrupt(ENC_FL_PIN, incFLCallback, FALLING); + enableInterrupt(ENC_BR_PIN, incBRCallback, FALLING); + enableInterrupt(ENC_BL_PIN, incBLCallback, FALLING); } void Car::initRCInput() { @@ -306,7 +333,7 @@ void Car::armActuators() { void Car::writeToActuators(const barc::ECU& ecu) { float motorCMD = saturateMotor(ecu.motor); float servoCMD = saturateServo(ecu.servo); - motor.write(motorCMD); + motor.writeMicroseconds( (uint16_t) (1500 + (motorCMD-90.0)*1000.0/180.0)) steering.write(servoCMD); } @@ -339,21 +366,29 @@ void Car::calcSteering() { void Car::incFL() { FL_count_shared++; + FL_old_time = FL_new_time; //(ADDED BY TOMMI 7JULY2016) + FL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FL_FLAG; } void Car::incFR() { FR_count_shared++; + FR_old_time = FR_new_time; //(ADDED BY TOMMI 7JULY2016) + FR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= FR_FLAG; } void Car::incBL() { BL_count_shared++; + BL_old_time = BL_new_time; //(ADDED BY TOMMI 7JULY2016) + BL_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BL_FLAG; } void Car::incBR() { BR_count_shared++; + BR_old_time = BR_new_time; //(ADDED BY TOMMI 7JULY2016) + BR_new_time = micros(); // new instant of passing magnet is saved //(ADDED BY TOMMI 7JULY2016) updateFlagsShared |= BR_FLAG; } @@ -375,15 +410,19 @@ void Car::readAndCopyInputs() { } if(updateFlags & FL_FLAG) { FL_count = FL_count_shared; + FL_DeltaTime = FL_new_time - FL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & FR_FLAG) { FR_count = FR_count_shared; + FR_DeltaTime = FR_new_time - FR_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BL_FLAG) { BL_count = BL_count_shared; + BL_DeltaTime = BL_new_time - BL_old_time; //(ADDED BY TOMMI 7JULY2016) } if(updateFlags & BR_FLAG) { BR_count = BR_count_shared; + BR_DeltaTime = BR_new_time - BR_old_time; //(ADDED BY TOMMI 7JULY2016) } // clear shared update flags and turn interrupts back on updateFlagsShared = 0; @@ -410,3 +449,36 @@ int Car::getEncoderBL() { int Car::getEncoderBR() { return BR_count; } + +unsigned long Car::getEncoder_dTime_FL() { //(ADDED BY TOMMI 7JULY2016) + return FL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_FR() { //(ADDED BY TOMMI 7JULY2016) + return FR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016)+ +unsigned long Car::getEncoder_dTime_BL() { //(ADDED BY TOMMI 7JULY2016) + return BL_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) +unsigned long Car::getEncoder_dTime_BR() { //(ADDED BY TOMMI 7JULY2016) + return BR_DeltaTime; //(ADDED BY TOMMI 7JULY2016) +} //(ADDED BY TOMMI 7JULY2016) + +float Car::getVelocityEstimate() { + vel_FL = 0.0; + vel_FR = 0.0; + vel_BL = 0.0; + vel_BR = 0.0; + if(FL_DeltaTime > 0){ + vel_FL = 2.0*3.141593*0.036/2.0*1.0/FL_DeltaTime*1000000.0; + } + if(FR_DeltaTime > 0){ + vel_FR = 2.0*3.141593*0.036/2.0*1.0/FR_DeltaTime*1000000.0; + } + if(BL_DeltaTime > 0){ + vel_BL = 2.0*3.141593*0.036/2.0*1.0/BL_DeltaTime*1000000.0; + } + if(BR_DeltaTime > 0){ + vel_BR = 2.0*3.141593*0.036/2.0*1.0/BR_DeltaTime*1000000.0; + } + return ( vel_FL + vel_FR ) / 2.0; +} diff --git a/c b/c new file mode 100644 index 00000000..bfbf9620 --- /dev/null +++ b/c @@ -0,0 +1,51 @@ +diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +index b33c103..be58230 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl +@@ -540,11 +540,11 @@ type MpcModel_convhull + # @NLconstraint(mdl, u_Ol[i+1,1]-u_Ol[i,1] >= -0.2) + # end +  +- @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.06) +- @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.06) ++ @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.08) ++ @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.08) + for i=1:N-1 # Constraints on u: +- @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.06) +- @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.06) ++ @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.08) ++ @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.08) + end +  +  +diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +index e4e90fb..ae130af 100644 +--- a/workspace/src/barc/src/barc_lib/LMPC/functions.jl ++++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl +@@ -68,9 +68,9 @@ function InitializeParameters(mpcParams::MpcParams,mpcParams_pF::MpcParams,track + mpcParams.Q_term_cost = 3 # scaling of Q-function + mpcParams.delay_df = 3 # steering delay + mpcParams.delay_a = 1 # acceleration delay +- mpcParams.Q_lane = 1 # weight on the soft constraint for the lane ++ mpcParams.Q_lane = 2 # weight on the soft constraint for the lane + mpcParams.Q_vel = 1 # weight on the soft constraint for the maximum velocity +- mpcParams.Q_slack = 1*[5*20.0,20.0,10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s ++ mpcParams.Q_slack = 1*[5*20.0,1*20.0,1*10.0,30.0,80.0,50.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + mpcParams.Q_obs = ones(Nl*selectedStates.Np)# weight to esclude some of the old trajectories +  + elseif selectedStates.simulator == true # if the simulator is in use +diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +index dc2d986..18c8ad9 100755 +--- a/workspace/src/barc/src/state_estimation_SensorKinematicModel.py ++++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel.py +@@ -224,8 +224,8 @@ def state_estimation(): +  + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) +- #R = diag([20.0,20.0,1.0,10.0,100.0,1000.0,1000.0, 20.0,20.0,10.0,1.0, 10.0,10.0]) +- # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v ++ R = diag([4*5.0,4*5.0,1.0,10.0,2*100.0,2*1000.0,2*1000.0, 4*5.0,4*5.0,10.0,1.0, 2*10.0,10.0]) ++ # x, y, v, psi,psiDot,a_x,a_y, x, y, psi, v +  + # Set up track parameters + l = Localization() diff --git a/env_loader_odroid.sh b/env_loader_odroid.sh new file mode 100755 index 00000000..da0b4a4f --- /dev/null +++ b/env_loader_odroid.sh @@ -0,0 +1,8 @@ +#!/bin/bash +source ~/barc/scripts/startup.sh +source /opt/ros/indigo/setup.bash +source ~/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/odroid/julia +export ROS_IP=10.0.0.1 +export ROS_MASTER_URI=http://10.0.0.14:11311 +exec "$@" diff --git a/env_loader_pc.sh b/env_loader_pc.sh new file mode 100755 index 00000000..ba5d4e80 --- /dev/null +++ b/env_loader_pc.sh @@ -0,0 +1,8 @@ +#!/bin/bash +source ~/barc/scripts/startup.sh +source /opt/ros/kinetic/setup.bash +source /home/shuqi/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia +export ROS_IP=127.0.0.1 +export ROS_MASTER_URI=http://127.0.0.1:11311 +exec "$@" diff --git a/env_loader_pc_shuqi.sh b/env_loader_pc_shuqi.sh new file mode 100755 index 00000000..ddf2680d --- /dev/null +++ b/env_loader_pc_shuqi.sh @@ -0,0 +1,7 @@ +#!/bin/bash +source /opt/ros/kinetic/setup.bash +source /home/mpcubuntu/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 +exec "$@" diff --git a/env_loader_pc_vis.sh b/env_loader_pc_vis.sh new file mode 100755 index 00000000..d1ff51a7 --- /dev/null +++ b/env_loader_pc_vis.sh @@ -0,0 +1,10 @@ +#!/bin/bash +source /opt/ros/kinetic/setup.bash +source /home/mpcubuntu/barc/workspace/devel/setup.bash +#export PATH=$PATH:/home/ugo/julia/bin/julia +# export ROS_IP=192.168.10.147 +# export ROS_MASTER_URI=http://192.168.10.74:11311 +# export DISPLAY=:0.0 +export ROS_IP=192.168.10.147 +export ROS_MASTER_URI=http://192.168.10.147:11311 +exec "$@" diff --git a/env_loader_ugo_visual.sh b/env_loader_ugo_visual.sh new file mode 100755 index 00000000..ddeca33c --- /dev/null +++ b/env_loader_ugo_visual.sh @@ -0,0 +1,7 @@ +#!/bin/bash +source /opt/ros/kinetic/setup.bash +source /home/ugo/GitHub/barc/workspace/devel/setup.bash +export DISPLAY=:0.0 +export ROS_IP=192.168.10.157 +export ROS_MASTER_URI=http://192.168.10.147:11311 +exec "$@" diff --git a/env_loader_yoga.sh b/env_loader_yoga.sh new file mode 100755 index 00000000..5dfc3b6d --- /dev/null +++ b/env_loader_yoga.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +source ~/barc/scripts/startup.sh +source /opt/ros/kinetic/setup.bash +source ~/barc/workspace/devel/setup.bash + +export ROS_IP=192.168.10.74 +export ROS_MASTER_URI=http://192.168.10.74:11311 + +exec "$@" diff --git a/eval_sim/Kalman simulation/sim_kalman.jl b/eval_sim/Kalman simulation/sim_kalman.jl new file mode 100644 index 00000000..186ef945 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman.jl @@ -0,0 +1,224 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + +Q = diagm([0.1,0.1,0.1,0.1,0.1]) +R = diagm([0.1,0.1,0.1,10.0]) + +function main(code::AbstractString) + global Q, R + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1]) + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_pwm_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/25 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,4) + u = zeros(sz,2) + + P = zeros(5,5) + x_est = zeros(length(t),5) + + yaw0 = imu_meas.z[1,6] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + + for i=2:length(t) + # Collect measurements and inputs for this iteration + y_gps = gps_meas.z[gps_meas.t.>t[i],:][1,:] + y_yaw = imu_meas.z[imu_meas.t.>t[i],6][1]-yaw0 + #a_x = imu_meas.z[imu_meas.t.>t[i],7][1] + y_vel_est = vel_est.z[vel_est.t.>t[i]][1] + y[i,:] = [y_gps y_yaw y_vel_est] + y[:,3] = unwrap!(y[:,3]) + u[i,:] = cmd_log.z[cmd_log.t.>t[i],:][1,:] + #u[i,1] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1]-94.14)/2.7678 + #u[i,2] = (cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1]-91.365)/105.6 + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],1][1] == 90 + # u[i,1] = 0 + # end + # if cmd_pwm_log.z[cmd_pwm_log.t.>t[i],2][1] == 90 + # u[i,2] = 0 + # end + + # Adapt R-value of GPS according to distance to last point: + gps_dist[i] = norm(y[i,1:2]-x_est[i-1,1:2]) + if gps_dist[i] > 0.3 + R[1,1] = 1+100*gps_dist[i]^2 + R[2,2] = 1+100*gps_dist[i]^2 + else + R[1,1] = 1 + R[2,2] = 1 + end + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est[i,:], P) = ekf(simModel,x_est[i-1,:]',P,h,y[i,:]',Q,R,args) + end + # ax1=subplot(211) + # plot(t,y,"-x",t,x_est,"-*") + # grid("on") + # legend(["x","y","psi","v"]) + # subplot(212,sharex=ax1) + # plot(t,u) + # grid("on") + # legend(["a_x","d_f"]) + figure(1) + ax=subplot(5,1,1) + for i=1:4 + subplot(5,1,i,sharex=ax) + plot(t,y[:,i],t,x_est[:,i],"-*") + grid("on") + end + subplot(5,1,5,sharex=ax) + plot(cmd_pwm_log.t,cmd_pwm_log.z) + grid("on") + + + figure(2) + subplot(2,1,1) + title("Comparison raw GPS data and estimate") + plot(t-t0,y[:,1],t-t0,x_est[:,1],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["x_m","x_est"]) + grid("on") + subplot(2,1,2) + plot(t-t0,y[:,2],t-t0,x_est[:,2],"-*") + xlabel("t [s]") + ylabel("Position [m]") + legend(["y_m","y_est"]) + grid("on") + + figure(3) + plot(t,gps_dist) + title("GPS distances") + grid("on") + # figure() + # plot(gps_meas.z[:,1],gps_meas.z[:,2],"-x",x_est[:,1],x_est[:,2],"-*") + # grid("on") + # title("x-y-view") + figure(4) + plot(t-t0,x_est,"-x",pos_info.t-t0,pos_info.z[:,[6,7,4,10,16]],"-*") + title("Comparison simulation (-x) onboard-estimate (-*)") + grid("on") + legend(["x","y","psi","v","psi_drift"]) + + unwrapped_yaw_meas = unwrap!(imu_meas.z[:,6]) + figure(5) + plot(t-t0,x_est[:,3],"-*",imu_meas.t-t0,unwrapped_yaw_meas-unwrapped_yaw_meas[1]) + title("Comparison yaw") + grid("on") + + figure(6) + plot(t-t0,x_est[:,4],"-*",vel_est.t-t0,vel_est.z) + grid("on") + title("Comparison of velocity measurement and estimate") + legend(["estimate","measurement"]) + + #figure(7) + #plt[:hist](gps_dist,300) + #grid("on") + nothing +end + +function h(x,args) + C = [eye(4) zeros(4,1)] + C[4,4] = 0 + #C[3,3] = 0 + C[3,5] = 1 + return C*x +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1 +end + +function simModel(z,args) + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.63 * z[4]^2 * sign(z[4])) # v + #zNext[4] = z[4] + dt*(z[6]) # v + #zNext[4] = z[4] + dt*(sqrt(z[6]^2+z[7]^2)) # v + zNext[5] = z[5] # psi_drift + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end \ No newline at end of file diff --git a/eval_sim/Kalman simulation/sim_kalman_imu.jl b/eval_sim/Kalman simulation/sim_kalman_imu.jl new file mode 100644 index 00000000..ec7a24f0 --- /dev/null +++ b/eval_sim/Kalman simulation/sim_kalman_imu.jl @@ -0,0 +1,353 @@ +# This script can be used to simulate the onboard-Kalman filter using real recorded sensor data. +# This can be useful to find the optimal Kalman parameters (Q,R) or to just do experiments and find an optimal sensor fusion process. + +using PyPlot +using JLD + + +function main(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + #cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(imu_meas.t[1],vel_est.t[1],gps_meas.t[1],cmd_log.t[1])+0.3 + t_end = min(imu_meas.t[end],vel_est.t[end],gps_meas.t[end],cmd_log.t[end]) + + l_A = 0.125 + l_B = 0.125 + + dt = 1/50 + t = t0:dt:t_end + sz = length(t) + y = zeros(sz,6) + u = zeros(sz,2) + + y_gps_imu = zeros(sz,13) + q = zeros(sz,2) + + P = zeros(7,7) + x_est = zeros(length(t),7) + P_gps_imu = zeros(14,14) + x_est_gps_imu = zeros(length(t),14) + + yaw0 = imu_meas.z[t0.>imu_meas.t,6][end] + gps_dist = zeros(length(t)) + + yaw_prev = yaw0 + y_gps_imu[1,4] = 0 + + qa = 1000 + qp = 1000 + + #Q_gps_imu = diagm([1/6*dt^3,1/6*dt^3,1/2*dt^2,1/2*dt^2,dt,dt,dt,dt,0.001,0.001,0.001]) + #Q_gps_imu = diagm([0.01,0.01,0.1,0.1,1.0,1.0,0.1,1.0,0.01]) + #R_gps_imu = diagm([0.1,0.1,1.0,0.1,1.0,100.0,100.0]) + #Q_gps_imu = diagm([0.1,0.1,0.1,0.1,1.0,1.0,0.1,1.0,0.01, 0.01,0.01,1.0,1.0,0.1]) + # x, y, vx, vy, ax, ay, psi, psidot, psidrift + Q_gps_imu = diagm([1/20*dt^5*qa,1/20*dt^5*qa,1/3*dt^3*qa,1/3*dt^3*qa,dt*qa,dt*qa,1/3*dt^3*qp,dt*qp,0.1, 0.01,0.01,0.1,0.1,0.1]) + #R_gps_imu = diagm([1.0,1.0,1.0,0.1,10.0,100.0,100.0, 1.0,1.0,0.1,0.5]) + R_gps_imu = diagm([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,10.0, 10.0,10.0]) + # x, y, v, psi, psidot, ax, a_y x, y, psi, v, v_x, v_y + + y_vel_prev = 0.0 + y_vel_est = 0.0 + + for i=2:length(t) + # Collect measurements and inputs for this iteration + # Interpolate GPS-measurements: + x_p = gps_meas.z[(t[i].>gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1.gps_meas.t_msg) & (t[i]-1. 1 + c_x = [t_p.^2 t_p ones(sl,1)]\x_p + c_y = [t_p.^2 t_p ones(sl,1)]\y_p + x_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_x + y_int = [(t[i]-t0)^2 (t[i]-t0) 1] * c_y + else + x_int = 0 + y_int = 0 + end + + y_gps = gps_meas.z[t[i].>gps_meas.t,:][end,:] + y_gps = [x_int y_int] + y_gps = pos_info.z[t[i].>pos_info.t,12:13][end,:] + + y_yaw = pos_info.z[t[i].>pos_info.t,14][end] + y_yawdot = imu_meas.z[t[i].>imu_meas.t,3][end] + + att = imu_meas.z[t[i].>imu_meas.t,4:6][end,:] + acc = imu_meas.z[t[i].>imu_meas.t,7:9][end,:] + rot = imu_meas.z[t[i].>imu_meas.t,1:3][end,:] + acc_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*acc' + #rot_f = rotMatrix('y',-att[2])*rotMatrix('x',-att[1])*rot' + a_x = imu_meas.z[t[i].>imu_meas.t,7][end] + a_y = imu_meas.z[t[i].>imu_meas.t,8][end] + a_x = acc_f[1] + a_y = acc_f[2] + #y_yawdot = rot_f[3] + + u[i,1] = cmd_log.z[t[i].>cmd_log.t,1][end] + u[i,2] = cmd_log.z[t[i]-0.2.>cmd_log.t,2][end] + + #y_vel_est = vel_est.z[t[i].>vel_est.t,1][end] + if pos_info.z[t[i].>pos_info.t,15][end] != y_vel_prev + y_vel_est = pos_info.z[t[i].>pos_info.t,15][end] + y_vel_prev = y_vel_est + else + y_vel_est = y_vel_est + dt*(u[i,1] - 0.5*x_est_gps_imu[i-1,13]) + end + + + bta = atan(0.5*tan(u[i,2])) + y_gps_imu[i,:] = [y_gps y_vel_est y_yaw y_yawdot a_x a_y y_gps y_yaw y_vel_est y_vel_est*cos(bta) y_vel_est*sin(bta)] + y_gps_imu[:,4] = unwrap!(y_gps_imu[:,4]) + y_gps_imu[:,10] = unwrap!(y_gps_imu[:,10]) + + + args = (u[i,:],dt,l_A,l_B) + + # Calculate new estimate + (x_est_gps_imu[i,:], P_gps_imu,q[i,1],q[i,2]) = ekf(simModel_gps_imu,x_est_gps_imu[i-1,:]',P_gps_imu,h_gps_imu,y_gps_imu[i,:]',Q_gps_imu,R_gps_imu,args) + end + + println("Yaw0 = $yaw0") + figure(1) + plot(t-t0,x_est_gps_imu[:,1:2],"-*",gps_meas.t-t0,gps_meas.z,"-+",t-t0,y_gps_imu[:,1:2],"--x") + plot(t-t0,x_est_gps_imu[:,10:11],"-x") + #plot(pos_info.t-t0,pos_info.z[:,12:13],"-+") + #plot(pos_info.t-t0,pos_info.z[:,6:7],"-x") + grid("on") + legend(["x_est","y_est","x_meas","y_meas","x_int","y_int","x_est2","y_est2"]) + title("Comparison x,y estimate and measurement") + + figure(2) + plot(t-t0,x_est_gps_imu[:,[7,9,12,14]],imu_meas.t-t0,imu_meas.z[:,6]-yaw0) + plot(pos_info.t-t0,pos_info.z[:,[10,16]],"-x") + grid("on") + legend(["psi_est","psi_drift_est","psi_est_2","psi_drift_est_2","psi_meas","psi_onboard","psi_drift_onboard"]) + + figure(3) + v = sqrt(x_est_gps_imu[:,3].^2+x_est_gps_imu[:,4].^2) + plot(t-t0,v,"-*",t-t0,x_est_gps_imu[:,[3:4,13]],"--",vel_est.t-t0,vel_est.z[:,1]) + plot(pos_info.t-t0,pos_info.z[:,8:9],"-x") + legend(["v_est","v_x_est","v_y_est","v_kin_est","v_meas","v_x_onboard","v_y_onboard"]) + grid("on") + title("Velocity estimate and measurement") + + figure(4) + plot(imu_meas.t-t0,imu_meas.z[:,7:8],"x",pos_info.t-t0,pos_info.z[:,17:18],t-t0,x_est_gps_imu[:,5:6]) + grid("on") + legend(["a_x_meas","a_y_meas","a_x_onboard","a_y_onboard","a_x_est","a_y_est",]) + + # PLOT DIRECTIONS + figure(5) + plot(x_est_gps_imu[:,10],x_est_gps_imu[:,11]) + grid("on") + title("x-y-view") + for i=1:10:size(pos_info.t,1) + dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-+") + end + for i=1:10:size(t,1) + bta = atan(l_A/(l_A+l_B)*tan(u[i,2])) + dir = [cos(x_est_gps_imu[i,12]+bta) sin(x_est_gps_imu[i,12]+bta)] # 7 = imu-yaw, 12 = model-yaw + lin = [x_est_gps_imu[i,10:11]; x_est_gps_imu[i,10:11] + 0.1*dir] + plot(lin[:,1],lin[:,2],"-*") + end + axis("equal") + + # figure(6) + # plot(t-t0,q) + # grid("on") + # title("Errors") + figure(7) + plot(imu_meas.t-t0,imu_meas.z[:,3],pos_info.t-t0,pos_info.z[:,11],t-t0,x_est_gps_imu[:,8]) + legend(["psiDot_meas","psiDot_onboard","psiDot_est"]) + grid("on") + nothing +end + +function h_gps_imu(x,args) + y = zeros(13) + y[1] = x[1] # x + y[2] = x[2] # y + y[3] = sqrt(x[3]^2+x[4]^2) # v + y[4] = x[7]+x[9] # psi + y[5] = x[8] # psiDot + y[6] = x[5] # a_x + y[7] = x[6] # a_y + y[8] = x[10] # x + y[9] = x[11] # y + y[10] = x[12]+x[14] # psi + y[11] = x[13] # v + y[12] = x[3] # v_x + y[13] = x[4] # v_y + return y +end +function ekf(f, mx_k, P_k, h, y_kp1, Q, R, args) + xDim = size(mx_k,1) + mx_kp1 = f(mx_k,args) + A = numerical_jac(f,mx_k,args) + P_kp1 = A*P_k*A' + Q + my_kp1 = h(mx_kp1,args) + H = numerical_jac(h,mx_kp1,args) + P12 = P_kp1*H' + q_GPS = (y_kp1-my_kp1)[1:2]'*inv(H*P12+R)[1:2,1:2]*(y_kp1-my_kp1)[1:2] + q_GPS2 = (y_kp1-my_kp1)[10:11]'*inv(H*P12+R)[10:11,10:11]*(y_kp1-my_kp1)[10:11] + # if q_GPS[1] > 0.005 + # R[1,1] = 100 + # R[2,2] = 100 + # else + # R[1,1] = 1 + # R[2,2] = 1 + # end + # if q_GPS2[1] > 0.005 + # R[8,8] = 100 + # R[9,9] = 100 + # else + # R[8,8] = 1 + # R[9,9] = 1 + # end + K = P12*inv(H*P12+R) + mx_kp1 = mx_kp1 + K*(y_kp1-my_kp1) + P_kp1 = (K*R*K' + (eye(xDim)-K*H)*P_kp1)*(eye(xDim)-K*H)' + return mx_kp1, P_kp1, q_GPS[1], q_GPS2[1] +end + +function simModel_gps_imu(z,args) + zNext = copy(z) + (u,dt,l_A,l_B) = args + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + #zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) + 1/2*dt^2*(cos(z[7])*z[5]-sin(z[7])*z[6]) # x + #zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) + 1/2*dt^2*(sin(z[7])*z[5]+cos(z[7])*z[6]) # y + zNext[1] = z[1] + dt*(cos(z[7])*z[3] - sin(z[7])*z[4]) # x + zNext[2] = z[2] + dt*(sin(z[7])*z[3] + cos(z[7])*z[4]) # y + zNext[3] = z[3] + dt*(z[5]+z[8]*z[4]) # v_x + zNext[4] = z[4] + dt*(z[6]-z[8]*z[3]) # v_y + zNext[5] = z[5] # a_x + zNext[6] = z[6] # a_y + zNext[7] = z[7] + dt*z[8] # psi + zNext[8] = z[8] # psidot + zNext[9] = z[9] # drift_psi + zNext[10] = z[10] + dt*(z[13]*cos(z[12] + bta)) # x + zNext[11] = z[11] + dt*(z[13]*sin(z[12] + bta)) # y + zNext[12] = z[12] + dt*(z[13]/l_B*sin(bta)) # psi + zNext[13] = z[13] + dt*(u[1] - 0.5*z[13]) # v + zNext[14] = z[14] # psi_drift_2 + return zNext +end + +function numerical_jac(f,x,args) + y = f(x,args) + jac = zeros(size(y,1),size(x,1)) + eps = 1e-5 + xp = copy(x) + for i = 1:size(x,1) + xp[i] = x[i]+eps/2.0 + yhi = f(xp,args) + xp[i] = x[i]-eps/2.0 + ylo = f(xp,args) + xp[i] = x[i] + jac[:,i] = (yhi-ylo)/eps + end + return jac +end + +function initPlot() # Initialize Plots for export + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function unwrap!(p) + length(p) < 2 && return p + for i = 2:length(p) + d = p[i] - p[i-1] + if abs(d) > pi + p[i] -= floor((d+pi) / (2*pi)) * 2pi + end + end + return p +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end + +function plot_velocities(code::AbstractString) + #log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + cmd_log = d_rec["cmd_log"] + + acc = diff(vel_est.z[:,1])./diff(vel_est.t_msg) + acc = smooth(acc,5) + t0 = vel_est.t[1] + ax1=subplot(211) + plot(vel_est.t-t0,vel_est.z) + plot(vel_est.t_msg-t0,vel_est.z,"--") + legend(["vel","fl","fr","bl","br"]) + grid("on") + xlabel("t [s]") + title("Velocity") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + plot(cmd_log.t_msg-t0,cmd_log.z[:,1],"--") + plot(vel_est.t_msg[1:end-1]-t0,acc) + grid("on") + legend(["u_a","u_a_sent","acceleration"]) + title("Commands") + xlabel("t [s]") + ylabel("") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/Other tests/NormalizeImu.jl b/eval_sim/Other tests/NormalizeImu.jl new file mode 100644 index 00000000..b3f6c0b9 --- /dev/null +++ b/eval_sim/Other tests/NormalizeImu.jl @@ -0,0 +1,44 @@ +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + t0 = imu_meas.t[1] + sz = size(imu_meas.t,1) + a_norm = zeros(sz,3) + for i=1:sz + a_norm[i,:] = (rotMatrix('y',-imu_meas.z[i,5])*rotMatrix('x',-imu_meas.z[i,4])*imu_meas.z[i,7:9]')' + end + figure() + plot(imu_meas.t-t0,imu_meas.z[:,7:8]) + grid("on") + figure() + plot(imu_meas.t-t0,a_norm[:,1:2]) + grid("on") + println("Mean raw: $(mean(imu_meas.z[:,8])), std raw: $(std(imu_meas.z[:,8]))") + println("Mean filtered: $(mean(a_norm[:,2])), std filtered: $(std(a_norm[:,2]))") +end + +function rotMatrix(s::Char,deg::Float64) + A = zeros(3,3) + if s=='x' + A = [1 0 0; + 0 cos(deg) sin(deg); + 0 -sin(deg) cos(deg)] + elseif s=='y' + A = [cos(deg) 0 -sin(deg); + 0 1 0; + sin(deg) 0 cos(deg)] + elseif s=='z' + A = [cos(deg) sin(deg) 0 + -sin(deg) cos(deg) 0 + 0 0 1] + else + warn("Wrong angle for rotation matrix") + end + return A +end diff --git a/eval_sim/Parameter estimation/find_a_mapping.jl b/eval_sim/Parameter estimation/find_a_mapping.jl new file mode 100644 index 00000000..fe6dde2a --- /dev/null +++ b/eval_sim/Parameter estimation/find_a_mapping.jl @@ -0,0 +1,109 @@ + +using PyPlot +using JLD +using Optim + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function main(code::AbstractString) + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + res = optimize(c->cost(cmd_opt,v_opt,c,0),[0.001,0,0.1,0.001,0]) + c = Optim.minimizer(res) + c2 = [1/c[1], c[2]/c[1], 1/c[4], c[5]/c[4]] + cost(cmd_opt,v_opt,c,1) + println("c = $c") + println("and c_2 = $c2") +end + +function v_over_u(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + L_b = 0.125 + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t),2) + psiDot = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i,:] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,:][end,:] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + delta = atan2(psiDot*0.25,v_x) + c = ((cmd[:,1]-91)/6.5-0.5*v)./delta.^2 + c = c[abs(c).<100] + plot(c) + grid("on") + figure() + plot(v) + grid("on") + #plot(cmd[:,1],v) +end + +function cost(cmd,v,c,p) + cost = 0 + v_s = zeros(length(v)) + v_s[1] = v[1] + v_s[2] = v[2] + for i=2:length(cmd)-1 + if cmd[i]-cmd[i-1] != 0 + v_s[i] = v[i] + end + if cmd[i] > 90 + v_s[i+1] = v_s[i] + 0.02*(c[1]*cmd[i] + c[2] - c[3]*v_s[i]) + else + v_s[i+1] = max(v_s[i] + 0.02*(c[4]*cmd[i] + c[5] - c[3]*v_s[i]),0) + end + end + if p==1 + plot((1:length(v_s))*0.02,v_s,(1:length(v))*0.02,v) + grid("on") + legend(["v_sim","v_meas"]) + title("Curve fitting acceleration") + xlabel("t [s]") + ylabel("v [m/s]") + end + cost = norm(v_s-v) + return cost +end diff --git a/eval_sim/Parameter estimation/find_d_f_delay.jl b/eval_sim/Parameter estimation/find_d_f_delay.jl new file mode 100644 index 00000000..5c118c81 --- /dev/null +++ b/eval_sim/Parameter estimation/find_d_f_delay.jl @@ -0,0 +1,66 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + #v2 = copy(v) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + + # idx = v_x.>1.1 + # psiDot = psiDot[idx] + # v_x = v_x[idx] + # cmd = cmd[idx] + delta = atan2(psiDot*0.25,v_x) + + figure(1) + ax2=subplot(211) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") + + figure(3) + plot(t-t0,delta,cmd_pwm_log.t-t0,(cmd_pwm_log.z[:,2]-90)/89.0) + grid("on") + xlabel("t [s]") + ylabel("d_f [rad]") + legend(["delta_true","delta_input"]) + + figure(2) + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_d_f_mapping.jl b/eval_sim/Parameter estimation/find_d_f_mapping.jl new file mode 100644 index 00000000..7de6122b --- /dev/null +++ b/eval_sim/Parameter estimation/find_d_f_mapping.jl @@ -0,0 +1,213 @@ + +using PyPlot +using JLD + +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +function main() + #log_path_record = ("$(homedir())/open_loop/output-record-8bbb.jld","$(homedir())/open_loop/output-record-3593.jld") + log_path_record = ("$(homedir())/open_loop/output-record-1486.jld",) + res_cmd = Float64[] + res_delta = Float64[] + res_t = Float64[] + L_b = 0.125 + for j=1:1 + d_rec = load(log_path_record[j]) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+0.5 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-0.5 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + res_cmd = cat(1,res_cmd,cmd) + res_delta = cat(1,res_delta,atan2(psiDot*0.25,v_x)) + res_t = cat(1,t) + end + res_delta = res_delta[res_cmd.>0] + res_cmd = res_cmd[res_cmd.>0] + res_delta = res_delta[res_cmd.<120] + res_cmd = res_cmd[res_cmd.<120] + res_delta = res_delta[res_cmd.>60] + res_cmd = res_cmd[res_cmd.>60] + sz = size(res_cmd,1) + + cmd_left = res_cmd[res_cmd.>103] + delta_left = res_delta[res_cmd.>103] + cmd_right = res_cmd[res_cmd.<80] + delta_right = res_delta[res_cmd.<80] + + coeff = [res_cmd ones(sz,1)]\res_delta + x = [1,200] + y = [1 1;200 1]*coeff + + coeff_r = [cmd_right ones(size(cmd_right,1),1)]\delta_right + xr = [1,200] + yr = [1 1;200 1]*coeff_r + + coeff_l = [cmd_left ones(size(cmd_left,1),1)]\delta_left + xl = [1,200] + yl = [1 1;200 1]*coeff_l + + ax1=subplot(211) + plot(res_t,res_delta) + grid("on") + subplot(212,sharex=ax1) + plot(res_t,res_cmd) + grid("on") + + figure(2) + plot(res_cmd,res_delta,"*",xl,yl,xr,yr) + + plot(vel_est.t,vel_est.z) + grid("on") + legend(["vel_avg","v_fl","v_fr","v_bl","v_br"]) +end + +function main_pwm(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + + L_b = 0.125 + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1])+1.0 + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end])-1.0 + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] < 90 || cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] > 95 + if cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] == cmd_pwm_log.z[t[i]-1.0.>cmd_pwm_log.t,2][end] + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + end + end + + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + delta = atan2(psiDot*0.25,v_x) + + idx = copy( (delta.<1.0) & (delta.> -1.0) & (cmd .< 90) & (cmd .> 50) ) + delta = delta[idx] + cmd = cmd[idx] + + sz = size(cmd,1) + coeff = [cmd ones(sz,1)]\delta + x = [1,200] + y = [1 1;200 1]*coeff + + plot(cmd,delta,"*",x,y) + grid("on") + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") + +end + +function main_pwm2(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + #v2 = copy(v) + psiDot = zeros(length(t)) + cmd = zeros(length(t)) + for i=1:length(t) + #v[i] = vel_est.z[t[i].>vel_est.t,1][end] + v[i] = pos_info.z[t[i].>pos_info.t,15][end] + psiDot[i] = imu_meas.z[t[i].>imu_meas.t,3][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,2][end] + end + v_x = real(sqrt(complex(v.^2-psiDot.^2*L_b^2))) + v_y = L_b*psiDot + + idx = v_x.>1.1 + psiDot = psiDot[idx] + v_x = v_x[idx] + cmd = cmd[idx] + delta = atan2(psiDot*0.25,v_x) + + c = [cmd ones(size(cmd,1))]\delta + + x = [60;120] + y = [x ones(2)]*c + + plot(cmd,delta,"+",x,y) + grid("on") + legend(["Measurements","Linear approximation"]) + xlabel("u_PWM") + ylabel("delta_F") + + figure(1) + ax2=subplot(211) + plot(t-t0,delta,cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + xlabel("t [s]") + legend(["delta_true","delta_input"]) + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,2]) + grid("on") + + figure(2) + ax1=subplot(211) + plot(cmd_pwm_log.t-t0,floor(cmd_pwm_log.z[:,2])) + grid("on") + subplot(212,sharex=ax1) + plot(vel_est.t-t0,vel_est.z,vel_est.t-t0,mean(vel_est.z[:,2:3],2,),"--") + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br","v_mean"]) +end + +function showV_V_F_relation() + L_F = 0.125 + L_R = 0.125 + delta = -0.3:.01:0.3 + v_F = 1.0 + v = L_R/L_F*cos(delta).*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + v2 = L_R/L_F*sqrt(1+L_F^2/(L_F+L_R)^2*tan(delta).^2) + plot(delta,v,delta,v2) + grid("on") + xlabel("delta") + ylabel("v/v_F") +end \ No newline at end of file diff --git a/eval_sim/Parameter estimation/find_u_a_drag.jl b/eval_sim/Parameter estimation/find_u_a_drag.jl new file mode 100644 index 00000000..20c20862 --- /dev/null +++ b/eval_sim/Parameter estimation/find_u_a_drag.jl @@ -0,0 +1,67 @@ + +using PyPlot +using JLD + +function main(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + L_b = 0.125 + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1],imu_meas.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end],imu_meas.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + cmd_raw = zeros(length(t)) + + for i=1:length(t) + if cmd_log.z[t[i].>cmd_log.t,1][end] > 0 && vel_est.z[t[i].>vel_est.t,1][end] > 0.2 + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + cmd[i] = cmd_log.z[t[i].>cmd_log.t,1][end] + cmd_raw[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + end + + idx = (v.>1) & (cmd_raw .> 95) + + sz = size(v[idx],1) + A = [cmd_raw[idx] ones(sz,1)] + b = v[idx] + coeff = A\b + + x = [90,110] + y = [90 1;110 1]*coeff + + #plot(cmd,v) + figure(1) + ax1=subplot(211) + plot(vel_est.t,vel_est.z) + grid("on") + legend(["v","v_fl","v_fr","v_bl","v_br"]) + subplot(212,sharex=ax1) + plot(cmd_pwm_log.t,cmd_pwm_log.z[:,1]) + grid("on") + + figure(2) + plot(cmd[cmd.>0],cmd[cmd.>0]./v[cmd.>0],"*") + xlabel("u_a") + ylabel("u_a/v") + grid("on") + + figure(3) + plot(cmd_raw[cmd_raw.>80],v[cmd_raw.>80],"*",x,y) + grid("on") + xlabel("PWM signal") + ylabel("v [m/s]") + + println("c1 = $(1/coeff[1])") + println("c2 = $(coeff[2]/coeff[1])") +end diff --git a/eval_sim/Parameter estimation/puresysID.jl b/eval_sim/Parameter estimation/puresysID.jl new file mode 100644 index 00000000..c2cb3a40 --- /dev/null +++ b/eval_sim/Parameter estimation/puresysID.jl @@ -0,0 +1,101 @@ + +using PyPlot +using JLD + +function main(code::AbstractString,n::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + d_rec = load(log_path_record) + + delay_df = 5 + + #n = 2 + + #idx0 = (oldTraj.oldTraj[:,6,n].> 0) & (oldTraj.oldTraj[:,6,n].< 17.76) + #oldTraj.oldTraj = oldTraj.oldTraj[idx0,:,:] + #oldTraj.oldInput = oldTraj.oldInput[idx0,:,:] + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + sz = size(oldTraj.oldTraj[oldTraj.oldTraj[:,1,n].<1000,:,n],1) + oldTraj.oldTraj[1:sz,:,n] = smooth(oldTraj.oldTraj[1:sz,:,n],20) + #oldTraj.oldInput[1:sz,:,n] = smooth(oldTraj.oldInput[1:sz,:,n],2) + # plot(oldTraj.oldInput[1:sz,:,n]) + # for i=2:sz + # oldTraj.oldInput[i,:,n] = oldTraj.oldInput[i-1,:,n] + (oldTraj.oldInput[i,:,n]-oldTraj.oldInput[i-1,:,n])*0.5 + # end + # plot(oldTraj.oldInput[1:sz,:,n],"--") + #sz = 200 + idx_d = 20:5:sz-5 + idx = 20:5:sz-10 + sz1 = size(idx,1) + + y_psi = zeros(5*sz1) + A_psi = zeros(5*sz1,3) + y_vy = zeros(5*sz1) + A_vy = zeros(5*sz1,4) + y_vx = zeros(5*sz1) + A_vx = zeros(5*sz1,4) + + for i=0:4 + y_psi[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,3,n]) + A_psi[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_psi = (A_psi'*A_psi)\A_psi'*y_psi + + for i=0:4 + y_vy[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,2,n]) + A_vy[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldTraj[idx+i,1,n].*oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,3,n]./oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i-delay_df,2,n]] + end + c_vy = (A_vy'*A_vy)\A_vy'*y_vy + + for i=0:4 + y_vx[(1:sz1)+i*sz1] = diff(oldTraj.oldTraj[idx_d+i,1,n]) + A_vx[(1:sz1)+i*sz1,:] = [oldTraj.oldTraj[idx+i,2,n] oldTraj.oldTraj[idx+i,3,n] oldTraj.oldTraj[idx+i,1,n] oldTraj.oldInput[idx+i,1,n]] + end + c_vx = (A_vx'*A_vx)\A_vx'*y_vx + + println("psi:") + println(c_psi) + println("vy:") + println(c_vy) + println("vx:") + println(c_vx) + + figure(1) + plot(A_psi*c_psi) + plot(y_psi,"--") + #plot(A_psi,"--") + title("Psi") + + figure(2) + plot(A_vy*c_vy) + plot(y_vy,"--") + #plot(A_vy,"--") + title("Vy") + + figure(3) + plot(A_vx*c_vx) + plot(y_vx,"--") + #plot(A_vx,"--") + title("Vx") + + # figure(4) + # plot(oldTraj.oldTimes[:,1],oldTraj.oldTraj[:,3,1]) + # plot(pos_info.t,pos_info.z[:,11]) + # plot(imu_meas.t,imu_meas.z[:,3]) + nothing +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end \ No newline at end of file diff --git a/eval_sim/PlotDataTutorial.pdf b/eval_sim/PlotDataTutorial.pdf new file mode 100644 index 00000000..7e33984d Binary files /dev/null and b/eval_sim/PlotDataTutorial.pdf differ diff --git a/eval_sim/README.md b/eval_sim/README.md new file mode 100644 index 00000000..e72bb0e5 --- /dev/null +++ b/eval_sim/README.md @@ -0,0 +1,78 @@ +Overview over evaluation functions +================================== +All evaluation functions use a code (String) which specifies an experiment. It is a 4-character combination which is created by the ROS master. All files that are saved during one experiment use this code. +Files that are saved: "output-record-(code).jld" from the recorder, "output-LMPC-(code).jld" from the LMPC node, and, if it is a simulation, "output-SIM-(code).jld" (contains real state information). +!! at the beginning denots most used functions. + +eval_data.jl +============ +Main file for all kinds of post-experiment evaluation + +eval_sim(code) +-------------- +Used for general evaluation of *simulations*. It uses 'real' simulation and 'measured' data and compares these. This can be helpful to develop a good state estimator. + +!! eval_run(code) +----------------- +Most general evaluation of experiments. Similar to eval_sim, but does not use simulation data. + +!! eval_LMPC(code) +------------------ +More detailed evaluation of specific LMPC data (MPC cost, sysID coefficients, curvature approximations, ...) + +plot_friction_circle(code, lap) +------------------------------- +Plots a_y over a_x. + +plot_v_ey_over_s(code, laps) +---------------------------- +Plots v and e_y over s for selected laps. Also plots lap times for all laps. + +plot_v_over_xy(code, lap) +------------------------- +Plots velocity in different colors on xy. + +eval_open_loop(code) +-------------------- +Similar to eval_run but for open-loop experiments (which are saved in a different folder). + +eval_predictions_kin(code) +------------------------- +Plots predictions for kinematic model (in path following mode) + +eval_predictions(code) +--------------------- +Plots predictions for dynamic model (in LMPC mode) + +eval_sysID(code) +--------------- +Plots sysID coefficients + +eval_oldTraj(code, lap) +----------------- +Plots data of one selected oldTrajectory (-> data of one specific lap +overlapping data before and after finish line) + +eval_LMPC_coeff(code, step) +-------------------- +Plots terminal constraints (both polynom and prediction) for one specific step in the LMPC process. + +!! anim_LMPC_coeff(code) +-------------------- +Animates predictions and terminal constraints (starting from first LMPC step) + +checkTimes(code) +---------------- +Check solving times and status (optimal/infeasible/user limit). + +Helper functions: +---------------- +create_track(half_track_width) -> used to create the track for plotting +initPlot() -> used to initialize plots for proper export and printing + +sim_kalman_imu.jl +================= +This file simulates a Kalman filter using real sensor data that was recorded during an experiment. It can be used to find the optimal structure and tuning values for the estimator. + +Parameter estimation folder +=========================== +Contains various files and functions to estimate BARC specific input mapping parameters (e.g. plot steering delay, least-square approximation for acceleration and steering, ...) \ No newline at end of file diff --git a/eval_sim/create_track.py b/eval_sim/create_track.py new file mode 100644 index 00000000..2659b37a --- /dev/null +++ b/eval_sim/create_track.py @@ -0,0 +1,95 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.03 +theta = 0 +d_theta = 0 + +N = 40 + +def add_curve(theta, length, angle): + d_theta = 0 + curve = 2*sum(arange(1,length/2+1,1))+length/2 + for i in range(0,length): + if i < length/2+1: + d_theta = d_theta + angle / curve + else: + d_theta = d_theta - angle / curve + theta = hstack((theta,theta[-1] + d_theta)) + return theta + + +theta = array([0]) + +# SOPHISTICATED TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,60,-2*pi/3) +# theta = add_curve(theta,90,pi) +# theta = add_curve(theta,80,-5*pi/6) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,50,0) +# theta = add_curve(theta,40,-pi/4) +# theta = add_curve(theta,30,pi/4) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,25,0) +# theta = add_curve(theta,50,-pi/2) +# theta = add_curve(theta,28,0) + +# GOGGLE TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,30,pi/3) +# theta = add_curve(theta,20,-pi/6) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,35,0) + +# SIMPLE GOGGLE TRACK +# theta = add_curve(theta,30,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,30,-pi/5) +# theta = add_curve(theta,20,pi/10) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,40,-pi/2) +# theta = add_curve(theta,37,0) + +# SIMPLE GOGGLE TRACK +# theta = add_curve(theta,60,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,60,-pi/5) +# theta = add_curve(theta,40,pi/10) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi/2) +# theta = add_curve(theta,75,0) + +# SIMPLE TRACK +# theta = add_curve(theta,10,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,20,0) +# theta = add_curve(theta,80,-pi) +# theta = add_curve(theta,9,0) + +for i in range(0,size(theta)): + x = hstack((x, x[-1] + cos(theta[i])*ds)) + y = hstack((y, y[-1] + sin(theta[i])*ds)) + +print x +print y +plt.plot(x,y,'-o') +plt.axis('equal') +plt.grid() +plt.show() \ No newline at end of file diff --git a/eval_sim/create_track_bezier.py b/eval_sim/create_track_bezier.py new file mode 100644 index 00000000..c5fe3dd8 --- /dev/null +++ b/eval_sim/create_track_bezier.py @@ -0,0 +1,54 @@ +from numpy import * +import matplotlib.pyplot as plt + +x = array([0]) # starting point +y = array([0]) +ds = 0.06 +theta = 0 +d_theta = 0 + +N = 40 + +def create_bezier(p0,p1,p2,dt): + t = arange(0,1+dt,dt)[:,None] + p = (1-t)**2*p0 + 2*(1-t)*t*p1+t**2*p2 + return p + +p0 = array([[0,0], + [2,0], + [2,-2], + [1,-4], + [3,-4], + [5,-5], + [3,-6], + [1,-6], + [-0.5,-5.5], + [-1.5,-5], + [-3,-4], + [-3,-2], + [0,0]]) +p1 = array([0,0,0.75,-1,1,inf,0,0,-1,0,inf,inf,0]) + +p = p0[0,:] +for i in range(0,size(p0,0)-1): + b1 = p0[i,1] - p1[i]*p0[i,0] + b2 = p0[i+1,1] - p1[i+1]*p0[i+1,0] + a1 = p1[i] + a2 = p1[i+1] + x = (b2-b1)/(a1-a2) + y = a1*x + b1 + if p1[i] == inf: + x = p0[i,0] + y = a2*x+b2 + elif p1[i+1] == inf: + x = p0[i+1,0] + y = a1*x+b1 + if a1 == a2: + p = vstack((p,p0[i+1,:])) + else: + p = vstack((p,create_bezier(p0[i,:],array([[x,y]]),p0[i+1,:],0.01))) + +plt.plot(p[:,0],p[:,1],'-',p0[:,0],p0[:,1],'*') +plt.grid() +plt.axis('equal') +plt.show() diff --git a/eval_sim/eval_data.jl b/eval_sim/eval_data.jl new file mode 100644 index 00000000..99494997 --- /dev/null +++ b/eval_sim/eval_data.jl @@ -0,0 +1,2121 @@ +using JLD +using PyPlot +using PyCall +@pyimport matplotlib.animation as animation +@pyimport matplotlib.patches as patch +using JLD, ProfileView +# pos_info[1] = s +# pos_info[2] = eY +# pos_info[3] = ePsi +# pos_info[4] = v +# pos_info[5] = s_start +# pos_info[6] = x +# pos_info[7] = y +# pos_info[8] = v_x +# pos_info[9] = v_y +# pos_info[10] = psi +# pos_info[11] = psiDot +# pos_info[12] = x_raw +# pos_info[13] = y_raw +# pos_info[14] = psi_raw +# pos_info[15] = v_raw +# pos_info[16] = psi_drift +# pos_info[17] = a_x +# pos_info[18] = a_y + +include("../workspace/src/barc/src/barc_lib/classes.jl") + +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data (when it was received by this recorder) + t_msg::Array{Float64} # time that the message was sent + z::Array{T} # measurement values +end + +# THIS FUNCTION EVALUATES DATA THAT WAS LOGGED BY THE SIMULATOR (INCLUDES "REAL" SIMULATION DATA) +# *********************************************************************************************** + +function eval_sim(code::AbstractString) + log_path_sim = "$(homedir())/simulations/output-SIM-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_sim = load(log_path_sim) + d_rec = load(log_path_record) + + imu_meas = d_sim["imu_meas"] + gps_meas = d_sim["gps_meas"] + z = d_sim["z"] + cmd_log = d_sim["cmd_log"] + slip_a = d_sim["slip_a"] + pos_info = d_rec["pos_info"] + vel_est = d_rec["vel_est"] + + t0 = pos_info.t[1] + track = create_track(0.4) + + figure() + ax1=subplot(311) + plot(z.t-t0,z.z,"-*") + title("Real states") + grid() + legend(["x","y","v_x","v_y","psi","psi_dot","a","d_f"]) + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-*") + title("Inputs") + grid() + legend(["u","d_f"]) + subplot(313,sharex=ax1) + plot(slip_a.t-t0,slip_a.z,"-*") + title("Slip angles") + grid() + legend(["a_f","a_r"]) + + figure() + plot(z.z[:,1],z.z[:,2],"-",gps_meas.z[:,1],gps_meas.z[:,2],".",pos_info.z[:,6],pos_info.z[:,7],"-") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["Real state","GPS meas","Estimate"]) + + figure() + title("Comparison of psi") + plot(imu_meas.t-t0,imu_meas.z,"-x",z.t-t0,z.z[:,5:6],pos_info.t-t0,pos_info.z[:,10:11],"-*") + legend(["imu_psi","imu_psi_dot","real_psi","real_psi_dot","est_psi","est_psi_dot"]) + grid() + + figure() + title("Comparison of v") + plot(z.t-t0,z.z[:,3:4],z.t-t0,sqrt(z.z[:,3].^2+z.z[:,4].^2),pos_info.t-t0,pos_info.z[:,8:9],"-*",pos_info.t-t0,sqrt(pos_info.z[:,8].^2+pos_info.z[:,9].^2),"-*",vel_est.t-t0,vel_est.z) + legend(["real_xDot","real_yDot","real_v","est_xDot","est_yDot","est_v","v_x_meas"]) + grid() + + figure() + title("Comparison of x,y") + plot(z.t-t0,z.z[:,1:2],pos_info.t-t0,pos_info.z[:,6:7],"-*",gps_meas.t-t0,gps_meas.z) + legend(["real_x","real_y","est_x","est_y","meas_x","meas_x"]) + grid() +end + +function eval_convhull(code::AbstractString,laps::Array{Int64},switch::Bool,obstacle::Bool) + + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + + Data = load(log_path_LMPC) + + oldSS_xy = Data["oldSS_xy"] + oldSS = Data["oldSS"] + selectedStates = Data["selectedStates"] + selStates = Data["selStates"] + statesCost = Data["statesCost"] + pred_sol = Data["pred_sol"] + pred_input = Data["pred_input"] + one_step_error = Data["one_step_error"] + lapStatus = Data["lapStatus"] + posInfo = Data["posInfo"] + eps_alpha = Data["eps_alpha"] + cvx = Data["cvx"] + cvy = Data["cvy"] + cpsi = Data["cpsi"] + input = Data["input"] + cost = Data["mpcCost"] + costSlack = Data["mpcCostSlack"] + obs_log = Data["obs_log"] + sol_u = Data["sol_u"] + final_counter = Data["final_counter"] + status = Data["sol_status"] + + Nl = selectedStates.Nl + Np = selectedStates.Np + buffersize = 5000 + currentIt = 200#lapStatus.currentIt + + flag = zeros(2) + + + track = create_track(0.4) + + pred_horizon = size(pred_sol)[1] - 1 + + println("prediction horizon N= ", pred_horizon) + + ospe1 = [] + ospe2 = [] + ospe3 = [] + ospe4 = [] + ospe5 = [] + ospe6 = [] + + slack1 = [] + slack2 = [] + slack3 = [] + slack4 = [] + slack5 = [] + slack6 = [] + + for l = laps + ospe1 = vcat(ospe1,one_step_error[1:currentIt,1,l]) + ospe2 = vcat(ospe2,one_step_error[1:currentIt,2,l]) + ospe3 = vcat(ospe3,one_step_error[1:currentIt,3,l]) + ospe4 = vcat(ospe4,one_step_error[1:currentIt,4,l]) + ospe5 = vcat(ospe5,one_step_error[1:currentIt,5,l]) + ospe6 = vcat(ospe6,one_step_error[1:currentIt,6,l]) + + slack1 = vcat(slack1,costSlack[1:currentIt,1,l]) + slack2 = vcat(slack2,costSlack[1:currentIt,2,l]) + slack3 = vcat(slack3,costSlack[1:currentIt,3,l]) + slack4 = vcat(slack4,costSlack[1:currentIt,4,l]) + slack5 = vcat(slack5,costSlack[1:currentIt,5,l]) + slack6 = vcat(slack6,costSlack[1:currentIt,6,l]) + end + + t_ospe = linspace(1,length(ospe1),length(ospe1)) + + figure() + plot(t_ospe,ospe1,t_ospe,ospe2,t_ospe,ospe3,t_ospe,ospe4,t_ospe,ospe5,t_ospe,ospe6) + legend(["vx","vy","psiDot","ePsi","eY","s"]) + grid("on") + title("One step prediction errors") + + figure() + plot(t_ospe,slack1,t_ospe,slack2,t_ospe,slack3,t_ospe,slack4,t_ospe,slack5,t_ospe,slack6) + legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + title("Slack costs") + grid("on") + + + + + + for i = laps + + pred_sol_xy, xyPathAngle = xyObstacle(oldSS,obs_log,1,i,track) + + #println("pred sol= ",pred_sol_xy[:,1]) + # for index=1:buffersize + # if status[index,i] == :UserLimit + # flag[1]=index + # flag[2]= i + + # break + # end + # end + + + vx_alpha = eps_alpha[1,1:currentIt,i] + vy_alpha = eps_alpha[2,1:currentIt,i] + psiDot_alpha = eps_alpha[3,1:currentIt,i] + ePsi_alpha = eps_alpha[4,1:currentIt,i] + eY_alpha = eps_alpha[5,1:currentIt,i] + s_alpha = eps_alpha[6,1:currentIt,i] + + cvx1 = cvx[1:currentIt,1,i] + cvx2 = cvx[1:currentIt,2,i] + cvx3 = cvx[1:currentIt,3,i] + cvy1 = cvy[1:currentIt,1,i] + cvy2 = cvy[1:currentIt,2,i] + cvy3 = cvy[1:currentIt,3,i] + cvy4 = cvy[1:currentIt,4,i] + cpsi1 = cpsi[1:currentIt,1,i] + cpsi2 = cpsi[1:currentIt,2,i] + cpsi3 = cpsi[1:currentIt,3,i] + + + if obstacle == false + figure() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + elseif obstacle == true + + ellfig = figure(3) + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,2]-(pred_sol_xy[2,1]),pred_sol_xy[1,2]-(pred_sol_xy[1,1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,1],pred_sol_xy[2,1]], 0.4, 0.2, angle = 90) + ax[:add_artist](ell1) + end + + + grid("on") + title("X-Y view of Lap $i") + + t = linspace(1,currentIt,currentIt) + + + + + + figure() + + plot(t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + title("Inputs in lap $i") + grid("on") + legend(["a","d_f"]) + + + #figure() + + # subplot(221) + # plot(t,one_step_error[1:currentIt,1,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # # annotate("UserLimit",xy=[flag[1],one_step_error[flag[1],1,flag[2]]],xytext=[flag[1]+0.1,one_step_error[flag[1],1,flag[2]]+0.1],xycoords="data",arrowprops=["facecolor"=>"black"]) + # title("One step prediction error for v_x in lap $i") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,1,i])[1]) + # legend(["ospe","a_x","d_f"]) + # grid("on") + + # subplot(222) + # plot(t,one_step_error[1:currentIt,2,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,2,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for v_y in lap $i") + # grid("on") + + # subplot(223) + # plot(t,one_step_error[1:currentIt,3,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,3,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for psiDot in lap $i") + # grid("on") + + # subplot(224) + # plot(t,one_step_error[1:currentIt,4,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,4,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for ePsi in lap $i") + # grid("on") + + + # figure() + + # subplot(221) + # plot(t,one_step_error[1:currentIt,5,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,5,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for eY in lap $i") + # grid("on") + + # subplot(222) + # plot(t,one_step_error[1:currentIt,6,i],t,input[1:currentIt,1,i],"-*",t,input[1:currentIt,2,i],"-+") + # #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + # legend(["ospe","a_x","d_f"]) + # title("One step prediction error for s in lap $i") + # grid("on") + + + # figure() + # plot(t,costSlack[1:currentIt,1,i],t,costSlack[1:currentIt,2,i],t,costSlack[1:currentIt,3,i],t,costSlack[1:currentIt,4,i],t,costSlack[1:currentIt,5,i],t,costSlack[1:currentIt,6,i]) + # legend(["slack cost on vx","slack cost on vy","slack cost on psiDot","slack cost on ePsi","slack cost on eY","slack cost on s"]) + # title("Slack costs") + # grid("on") + + + + # figure() + # plot(t,one_step_error[1:currentIt,1,i],t,one_step_error[1:currentIt,2,i],t,one_step_error[1:currentIt,3,i],t,one_step_error[1:currentIt,4,i],t,one_step_error[1:currentIt,5,i],t,one_step_error[1:currentIt,6,i]) + # legend(["vx","vy","psiDot","ePsi","eY","s"]) + # grid("on") + # title("One step prediction errors") + + # println("one step prediction error= ",one_step_error[1:30,:,i]) + + + figure() + + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,1,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vx in lap $i") + grid("on") + + subplot(222) + plot(t,oldSS.oldSS[1:currentIt,2,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("vy in lap $i") + grid("on") + + subplot(223) + plot(t,oldSS.oldSS[1:currentIt,3,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("psiDot in lap $i") + grid("on") + + subplot(224) + plot(t,oldSS.oldSS[1:currentIt,4,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("ePsi in lap $i") + grid("on") + + figure() + subplot(221) + plot(t,oldSS.oldSS[1:currentIt,5,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("eY in lap $i") + grid("on") + + subplot(222) + plot(t,oldSS.oldSS[1:currentIt,6,i],"-*") + #ylim(-0.0001,findmax(one_step_error[1:currentIt,6,i])[1]) + title("s in lap $i") + grid("on") + + subplot(223) + velocity= sqrt(oldSS.oldSS[1:currentIt,2,i].^2 + oldSS.oldSS[1:currentIt,1,i].^2) + plot(t,velocity,"-*") + title("Overall velocity in lap $i") + grid("on") + + + + figure() + + subplot(221) + plot(t,cvx1,t,cvx2,t,cvx3) + legend(["cvx1","cvx2","cvx3"]) + title("C_Vx in lap $i") + grid("on") + + subplot(222) + plot(t,cvy1,t,cvy2,t,cvy3,t,cvy4) + legend(["cvx1","cvx2","cvx3","cvy4"]) + title("C_Vy in lap $i") + grid("on") + + subplot(223) + plot(t,cpsi1,t,cpsi2,t,cpsi3) + legend(["cpsi1","cpsi2","cpsi3"]) + title("C_Psi in lap $i") + grid("on") + + figure() + plot(t,cost[1:currentIt,2,i],t,cost[1:currentIt,3,i],t,cost[1:currentIt,4,i],t,cost[1:currentIt,6,i]) + legend(["terminal Cost","control Cost","derivative Cost","lane Cost"]) + title("Costs of the Mpc in lap $i") + grid("on") + + + + + + + + if switch == true + + + + for j = 2:2000 + + solution_status = status[final_counter[i-1]+j] + + vx_pred = pred_sol[:,1,j,i] + vy_pred = pred_sol[:,2,j,i] + psiDot_pred = pred_sol[:,3,j,i] + ePsi_pred = pred_sol[:,4,j,i] + eY_pred = pred_sol[:,5,j,i] + s_pred = pred_sol[:,6,j,i] + + + oldvx = selStates[1:Np,1,j,i] + oldvx2 = selStates[Np+1:2*Np,1,j,i] + #oldvx3 = selStates[2*Np+1:3*Np,1,j,i] + oldvy = selStates[1:Np,2,j,i] + oldvy2 = selStates[Np+1:2*Np,2,j,i] + #oldvy3 = selStates[2*Np+1:3*Np,2,j,i] + oldpsiDot = selStates[1:Np,3,j,i] + oldpsiDot2 = selStates[Np+1:2*Np,3,j,i] + #oldpsiDot3 = selStates[2*Np+1:3*Np,3,j,i] + oldePsi = selStates[1:Np,4,j,i] + oldePsi2 = selStates[Np+1:2*Np,4,j,i] + #oldePsi3 = selStates[2*Np+1:3*Np,4,j,i] + oldeY = selStates[1:Np,5,j,i] + oldeY2 = selStates[Np+1:2*Np,5,j,i] + #oldeY3 = selStates[2*Np+1:3*Np,5,j,i] + olds = selStates[1:Np,6,j,i] + olds2 = selStates[Np+1:2*Np,6,j,i] + #olds3 = selStates[2*Np+1:3*Np,6,j,i] + + + t = linspace(1,j,j) + + s_ey_pred = zeros(pred_horizon+1,2) + + s_ey_pred[:,1] = pred_sol[:,6,j,i] + s_ey_pred[:,2] = pred_sol[:,5,j,i] + + #println(s_ey_pred) + + + xy_predictions = xyPrediction(oldSS,s_ey_pred,track) + + #println("xyPredictions= ",xy_predictions) + + + if obstacle == false + figure(19) + clf() + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i]) + plot(xy_predictions[1,:]',xy_predictions[2,:]',"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + #plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Prediction at iteration $j, status = $solution_status") + # end + + elseif obstacle == true + ellfig = figure(19) + clf() + # plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"*") + #plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + #for i=1:4:size(x_est,1) + # for index=1:length(oldSS_xy[:,1,i]) + # z_pred = zeros(size(pred_sol)[1],4) + # #z_pred[1,:] = x_est[i,:] + # z_pred[1,:] = oldSS_xy[j,:,i] + # for j2=2:size(pred_sol)[1] + # z_pred[j2,:] = simModel(z_pred[j2-1,:],pred_input[j2-1,:,j,i],0.1,0.125,0.125) + # end + + + ax = ellfig[:add_subplot](1,1,1) + ax[:set_aspect]("equal") + plot(oldSS_xy[:,1,i],oldSS_xy[:,2,i],"-.") + # plot(oldSS_xy[:,1,i-1],oldSS_xy[:,2,i-1],"ob") + plot(track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-")#,track[:,1],track[:,2],"b.") + + angle_ell = atan2(pred_sol_xy[2,j+1]-(pred_sol_xy[2,j-1]),pred_sol_xy[1,j+1]-(pred_sol_xy[1,j-1])) + angle_deg = (angle_ell*180)/pi + + ell1 = patch.Ellipse([pred_sol_xy[1,j],pred_sol_xy[2,j]], 0.4, 0.2, angle = angle_deg) + ax[:add_artist](ell1) + + plot(oldSS_xy[j,1,i],oldSS_xy[j,2,i],"og") + grid("on") + # title("Predicted solution in lap $i, iteration $j") + title("Position at iteration $j") + # end + end + + figure(15) + clf() + subplot(221) + plot(s_pred,vx_pred,"or") + plot(olds,oldvx,"b") + plot(olds2,oldvx2,"b") + #plot(olds3,oldvx3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,2,:,i])[1],findmax(oldTraj.z_pred_sol[:,2,:,i])[1]) + title("State vx in lap $i, iteration $j, status = $solution_status") + grid("on") + + subplot(222) + plot(s_pred,vy_pred,"or") + plot(olds,oldvy,"b") + plot(olds2,oldvy2,"b") + #plot(olds3,oldvy3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,3,:,i])[1],findmax(oldTraj.z_pred_sol[:,3,:,i])[1]) + title("State vy in lap $i, iteration $j, status = $solution_status ") + grid("on") + + subplot(223) + plot(s_pred,psiDot_pred,"or") + plot(olds,oldpsiDot,"b") + plot(olds2,oldpsiDot2,"b") + #plot(olds3,oldpsiDot3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State psiDot in lap $i , iteration $j, status = $solution_status") + grid("on") + + subplot(224) + plot(s_pred,ePsi_pred,"or") + plot(olds,oldePsi,"b") + plot(olds2,oldePsi2,"b") + #plot(olds3,oldePsi3,"b") + #ylim(findmin(oldTraj.z_pred_sol[:,4,:,i])[1],findmax(oldTraj.z_pred_sol[:,4,:,i])[1]) + title("State ePsi in lap $i, iteration $j, status = $solution_status ") + grid("on") + + + figure(16) + clf() + subplot(221) + plot(s_pred,eY_pred,"or") + plot(olds,oldeY,"b") + plot(olds2,oldeY2,"b") + #plot(olds3,oldeY3,"b") + + if obstacle == true + for obs_ind = 1:size(obs_log)[3] + if obs_log[j,1,obs_ind,i] < max(olds[end],olds2[end]) + plot(obs_log[j,1,obs_ind,i],obs_log[j,2,obs_ind,i],"ko") + end + end + end + + title("State eY in lap $i, iteration $j, status = $solution_status ") + grid("on") + + figure(17) + clf() + + subplot(121) + velocity= sqrt(oldSS.oldSS[1:j,2,i].^2 + oldSS.oldSS[1:j,1,i].^2) + plot(t,velocity,"-*") + legend(["velocity"]) + + + subplot(122) + #plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") + legend(["a_x","d_f"]) + + title("Comparison between velocity and inputs in lap $i, iteration $j ") + grid("on") + + figure(18) + clf() + + subplot(121) + plot(t,oldSS.oldSS[1:j,5,i],"-*") + legend(["e_y"]) + + subplot(122) + # plot(linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,1,j,i],linspace(1,size(pred_input)[1],size(pred_input)[1]),pred_input[:,2,j,i]) + plot(t,input[1:j,1,i],t,input[1:j,2,i]) + plot(linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,1,j,i],"-*",linspace(length(t),length(t)+pred_horizon,pred_horizon),pred_input[:,2,j,i],"-*") + legend(["a_x","d_f"]) + + title("Comparison between e_y and inputs in lap $i, iteration $j ") + grid("on") + + figure(20) + clf() + subplot(211) + plot(t,oldSS.oldSS[1:j,5,i],"-*") + title("eY in lap $i, iteration $j") + grid("on") + plot(linspace(j,j+size(pred_sol)[1],size(pred_sol)[1]),pred_sol[:,5,j,i],"-+") + + subplot(212) + plot(t,cost[1:j,6,i]) + title("lane cost in lap $i, iteration $j") + grid("on") + + + + + + sleep(0.5) + end + end + end + +end + +# THIS FUNCTION EVALUATES DATA THAT WAS RECORDED BY BARC_RECORD.JL +# **************************************************************** +function eval_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + #log_path_record = "$(homedir())/open_loop/output-record-0ed5.jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = pos_info.t[1] + track = create_track(0.4) + + # Calculate accelerations + acc = smooth(diff(smooth(vel_est.z[:,1],10))./diff(vel_est.t),10) + figure(10) + plot(vel_est.t[1:end-1]-t0,acc,cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") + legend(["Acc","u_a"]) + + + figure() + plot(gps_meas.z[:,1],gps_meas.z[:,2],"-.",pos_info.z[:,6],pos_info.z[:,7],"-*") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid(1) + title("x-y-view") + axis("equal") + legend(["GPS meas","estimate"]) + + figure() + #plot(imu_meas.t-t0,imu_meas.z[:,7:9]) + plot(pos_info.t-t0,pos_info.z[:,19:20]) + grid("on") + legend(["a_x","a_y"]) + ylabel("a [m/s^2]") + xlabel("t [s]") + title("Measured accelerations") + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,6:7],"-",gps_meas.t_msg-t0,gps_meas.z,"--") + grid(1) + title("GPS comparison") + xlabel("t [s]") + ylabel("Position [m]") + legend(["x_meas","y_meas","x_est","y_est"]) + + figure() + ax2=subplot(211) + title("Commands") + plot(cmd_log.t-t0,cmd_log.z,"-*",cmd_log.t_msg-t0,cmd_log.z,"-x") + grid("on") + xlabel("t [s]") + subplot(212,sharex=ax2) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z,"-*") + grid("on") + xlabel("t [s]") + + figure() + title("Comparison of psi") + plot(imu_meas.t-t0,imu_meas.z[:,6],imu_meas.t-t0,imu_meas.z[:,3],"-x",pos_info.t-t0,pos_info.z[:,10:11],"-*",pos_info.t-t0,pos_info.z[:,16],"-*",pos_info.t-t0,pos_info.z[:,14]) + legend(["imu_psi","imu_psi_dot","est_psi","est_psi_dot","psi_drift","psi_raw"]) + grid() + + figure() + plot(pos_info.t-t0,pos_info.z[:,2:3]) + legend(["e_y","e_psi"]) + title("Deviations from reference") + grid("on") + xlabel("t [s]") + ylabel("eY [m], ePsi [rad]") + + # figure() + # title("Raw IMU orientation data") + # plot(imu_meas.t-t0,imu_meas.z[:,1:3],"--",imu_meas.t-t0,imu_meas.z[:,4:6],imu_meas.t_msg-t0,imu_meas.z[:,4:6]) + # grid("on") + # legend(["w_x","w_y","w_z","roll","pitch","yaw"]) + + figure() + title("v measurements and estimate") + plot(vel_est.t-t0,vel_est.z[:,1],"-x",pos_info.t-t0,pos_info.z[:,[8:9,4]],"-+") + legend(["v_raw","est_xDot","est_yDot","est_v"]) + grid() + + # figure() + # title("s, eY and inputs") + # ax1=subplot(211) + # plot(pos_info.t-t0,pos_info.z[:,2:3],"-*") + # grid("on") + # legend(["eY","ePsi"]) + # subplot(212,sharex=ax1) + # plot(cmd_log.t-t0,cmd_log.z,"-*") + # grid("on") + # legend(["u","d_f"]) + nothing +end + +function plot_friction_circle(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + idx = (pos_info.t.>t_start) & (pos_info.t.= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(smooth(oldTraj.oldTraj[idx,6,laps[i]],5),oldTraj.oldTraj[idx,1,laps[i]],label="Lap $(laps[i])") + end + legend() + grid("on") + xlabel("s [m]") + ylabel("v [m/s]") + xlim([0,19.11]) + ylim([0,3.2]) + + # plot e_y over s + figure(2) + for i=1:n_laps + idx = (oldTraj.oldTraj[:,6,laps[i]] .>= 0.0) & (oldTraj.oldTraj[:,6,laps[i]] .<= 19.11) + plot(oldTraj.oldTraj[idx,6,laps[i]],oldTraj.oldTraj[idx,5,laps[i]],label="Lap $(laps[i])") + end + legend() + grid("on") + xlabel("s [m]") + ylabel("e_Y [m]") + xlim([0,19.11]) + ylim([-0.5,0.5]) + + # plot lap time over iterations + laps_run = size(oldTraj.oldCost[oldTraj.oldCost.>1],1) + lap_times = zeros(laps_run) + for i=1:laps_run + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].>=0,i][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,i].<=19.11,i][end] + lap_times[i] = t_end-t_start + end + figure(3) + plot(1:size(lap_times,1),lap_times,"-o") + grid("on") + xlabel("Lap number") + ylabel("Lap time [s]") + ylim([0,25]) +end + +function plot_v_over_xy(code::AbstractString,lap::Int64) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + pos_info = d_rec["pos_info"] + + # Find timing of selected lap: + t_start = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].>=0,lap][1] + t_end = oldTraj.oldTimes[oldTraj.oldTraj[:,6,lap].<=19.11,lap][end] + println("Laptime = $(t_end-t_start) s") + + idx = (pos_info.t.>=t_start) & (pos_info.t.<=t_end) + + track = create_track(0.4) + + figure() + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + scatter(pos_info.z[idx,6],pos_info.z[idx,7],c=pos_info.z[idx,8],cmap=ColorMap("jet"),edgecolors="face",vmin=0.5,vmax=3.0) + grid(1) + title("x-y-view") + axis("equal") + cb = colorbar() + cb[:set_label]("Velocity [m/s]") + println("Average v_x = ",mean(pos_info.z[idx,8])," m/s") + println("Max v_x = ",findmax(pos_info.z[idx,8])[1]," m/s") + +end + + +function eval_open_loop(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$(code).jld" + d_rec = load(log_path_record) + + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + + t0 = pos_info.t[1] + + figure() + ax3=subplot(211) + title("Comparison speed and input") + plot(vel_est.t-t0,vel_est.z,"-*",vel_est.t_msg-t0,vel_est.z,"-x") + legend(["vel_est"]) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_pwm_log.t-t0,cmd_pwm_log.z[:,1],cmd_pwm_log.t_msg-t0,cmd_pwm_log.z[:,1]) + grid("on") + + gps_speed_raw = diff(gps_meas.z)./diff(gps_meas.t) + gps_speed = [0;sqrt(gps_speed_raw[:,1].^2+gps_speed_raw[:,2].^2)] + figure() + title("Comparison GPS and encoder speed") + plot(vel_est.t-t0,vel_est.z,"-*",gps_meas.t-t0,gps_speed,"-x",pos_info.t_msg-t0,pos_info.z[:,4],"-+") + grid("on") + legend(["encoder","gps","estimator"]) + + figure() + title("Acceleration data") + plot(imu_meas.t-t0,imu_meas.z[:,7:9],"-x",imu_meas.t_msg-t0,imu_meas.z[:,7:9],"-*") + legend(["a_x","a_y","a_z"]) + grid() + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*",pos_info.t-t0,pos_info.z[:,12:13],"-x",pos_info.t-t0,pos_info.z[:,6:7]) + legend(["x_gps","y_gps","x_filtered","y_filtered","x_est","y_est"]) + grid("on") + + figure() + plot(gps_meas.t-t0,gps_meas.z,"-*") + legend(["x_gps","y_gps"]) + xlabel("t [s]") + ylabel("Position [m]") + grid("on") +end + +# THIS FUNCTION EVALUATES MPC-SPECIFIC DATA +# ***************************************** +function eval_LMPC(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + + t0 = t[1] + + + figure(2) + ax1=subplot(311) + plot(pos_info.t-t0,pos_info.z[:,8],".",t-t0,state[:,1],"-*") + title("Comparison of publishing and receiving time") + legend(["x_dot_estimate","x_dot_MPC"]) + grid("on") + xlabel("t [s]") + ylabel("v_x [m/s]") + subplot(312,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z,"-o",t-t0,cmd[1:length(t),:],"-*",t-t0,state[:,7]) + legend(["a_rec","d_f_rec","a_MPC","d_f_MPC","acc_filter"]) + grid("on") + subplot(313,sharex=ax1) + plot(t-t0,c_Vx) + title("System ID xDot coefficients") + grid("on") + legend(["1","2","3"]) + + figure(3) + plot(t,state) + grid("on") + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + + figure() + subplot(311) + title("c_Vx") + plot(t-t0,c_Vx) + legend(["1","2","3"]) + grid("on") + subplot(312) + title("c_Vy") + plot(t-t0,c_Vy) + legend(["1","2","3","4"]) + grid("on") + subplot(313) + title("c_Psi") + plot(t-t0,c_Psi) + legend(["1","2","3"]) + grid("on") + + # *********** CURVATURE ********************* + figure() + c = zeros(size(curv,1),1) + for i=1:size(curv,1) + s = state[i,6] + c[i] = ([s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + #c[i] = ([s.^3 s.^2 s.^1 s.^0] * curv[i,:]')[1] + end + plot(state[:,6],c,"-o") + for i=1:5:size(curv,1) + if isnan(sol_z[1,6,i]) + s = sol_z[:,1,i] + else + s = sol_z[:,6,i] + end + c = zeros(size(curv,1),1) + c = [s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + #c = [s.^3 s.^2 s.^1 s.^0] * curv[i,:]' + plot(s,c,"-*") + end + title("Curvature over path") + xlabel("Curvilinear abscissa [m]") + ylabel("Curvature") + grid() + + track = create_track(0.4) + figure() + hold(1) + plot(x_est[:,1],x_est[:,2],"-*") + title("Estimated position") + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + axis("equal") + grid(1) + # HERE YOU CAN CHOOSE TO PLOT DIFFERENT DATA: + # CURRENT HEADING (PLOTTED BY A LINE) + # for i=1:10:size(pos_info.t,1) + # dir = [cos(pos_info.z[i,10]) sin(pos_info.z[i,10])] + # lin = [pos_info.z[i,6:7]; pos_info.z[i,6:7] + 0.1*dir] + # plot(lin[:,1],lin[:,2],"-+") + # end + + # PREDICTED PATH + for i=1:4:size(x_est,1) + z_pred = zeros(11,4) + z_pred[1,:] = x_est[i,:] + for j=2:11 + z_pred[j,:] = simModel(z_pred[j-1,:],sol_u[j-1,:,i],0.1,0.125,0.125) + end + plot(z_pred[:,1],z_pred[:,2],"-*") + end + + # PREDICTED REFERENCE PATH (DEFINED BY POLYNOM) + # for i=1:size(x_est,1) + # s = 0.4:.1:2.5 + # ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + # x = ss*coeffX[i,:]' + # y = ss*coeffY[i,:]' + # plot(x,y) + # end + + # rg = 100:500 + # figure() + # plot(s_start[rg]+state[rg,1],state[rg,2:4],"-o") + # title("Comparison states and prediction") + # legend(["ey","epsi","v"]) + # grid(1) + # for i=100:5:500 + # plot(s_start[i]+sol_z[:,1,i],sol_z[:,2:4,i],"-*") + # end + + # figure() + # plot(oldTraj[:,6,1,2],oldTraj[:,1:5,1,2],"-x") + # title("Old Trajectory") + # legend(["v_x","v_y","psiDot","ePsi","eY"]) + # xlabel("s") + # grid(1) + + figure() + ax1=subplot(211) + title("MPC states and cost") + plot(t-t0,state) + legend(["v_x","v_y","psiDot","ePsi","eY","s"]) + grid(1) + subplot(212,sharex = ax1) + plot(t-t0,cost) + grid(1) + legend(["costZ","costZTerm","constZTerm","derivCost","controlCost","laneCost"]) + + figure() + ax1=subplot(211) + title("States and inputs") + plot(t-t0,state[:,1:5]) + legend(["v_x","v_y","psiDot","ePsi","eY"]) + grid(1) + subplot(212,sharex = ax1) + plot(cmd_log.t-t0,cmd_log.z) + grid(1) + legend(["u","d_f"]) +end + +function eval_predictions_kin(code::AbstractString) + # This function helps to evaluate predictions of the *kinematic* model + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + z = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_lmpc = d_lmpc["cmd"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + sz = size(z,1) + N = size(sol_z,1)-1 # number of steps (prediction horizon) + figure(1) + plot(pos_info.t-t0,pos_info.z[:,2],"--g") + plot(t-t0,z[:,5],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,2,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_y") + + figure(2) + plot(pos_info.t-t0,pos_info.z[:,3],"--g") + plot(t-t0,z[:,4],"-b") + for i=1:sz + plot((t[i]-t0):.1:(t[i]-t0+0.1*N),sol_z[:,3,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("e_psi") + + figure(3) + plot(pos_info.t-t0,(pos_info.z[:,8].^2+pos_info.z[:,9].^2).^0.5,"--g") + plot(t-t0,(z[:,1].^2+z[:,2].^2).^0.5,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*N,N+1),sol_z[:,4,i],"--xr") + end + grid("on") + xlabel("t [s]") + legend(["Estimate","LMPC estimate","Prediction"]) + title("v") + + figure(4) + plot(cmd_log.t-t0,cmd_log.z,"--g") + plot(t-t0,cmd_lmpc,"-b") + for i=1:sz + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-2),N-1),sol_u[1:N-1,1,i],"--xr") + plot(linspace(t[i]-t0,t[i]-t0+0.1*(N-3),N-2),sol_u[1:N-2,2,i],"--xr") + end + grid("on") + + # Calculate one-step-errors: + one_step_err = zeros(sz,4) + for i=1:sz-1 + one_step_err[i,:] = sol_z[2,1:4,i] - [z[i+1,6] z[i+1,5] z[i+1,4] norm(z[i+1,1:2])] + end + + # Calculate 'real' d_f + L_b = 0.125 + v_x = real(sqrt(complex((pos_info.z[:,8].^2+pos_info.z[:,9].^2)-pos_info.z[:,11].^2*L_b^2))) + delta = atan2(pos_info.z[:,11]*0.25,v_x) + + figure(5) + ax1=subplot(211) + plot(t-t0,one_step_err) + grid("on") + legend(["s","ey","epsi","v"]) + subplot(212,sharex=ax1) + plot(t-t0,cmd_lmpc) + plot(cmd_log.t_msg-t0,cmd_log.z) + plot(cmd_log.t-t0,cmd_log.z,"--") + plot(pos_info.t-t0,delta) + grid("on") + +end +function eval_predictions(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + z = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + step_diff = d_lmpc["step_diff"] + + t0 = t[1] + + N = size(sol_z,1)-1 # number of steps (prediction horizon) + + #N = 7 + sz = size(t,1) + # Calculate one-step-errors: + ########### + # s + # eY + # ePsi + # v + ########### + # v_x + # v_y + # psiDot + # ePsi + # eY + # s + ########### + one_step_err = NaN*ones(sz,6) + for i=1:sz-1 + if isnan(sol_z[1,5,i]) # if we are in path following mode + one_step_err[i,1:4] = sol_z[2,1:4,i] - [z[i+1,[6,5,4]] norm(z[i+1,1:2])] + else + one_step_err[i,:] = sol_z[2,[6,5,4,1,2,3],i] - z[i+1,[6,5,4,1,2,3]] + end + end + i=1 + while isnan(sol_z[1,5,i]) + i = i+1 + end + figure(1) + plot(t-t0,step_diff) + grid("on") + title("One-step-errors") + legend(["xDot","yDot","psiDot","ePsi","eY"]) + + + figure(3) + plot(pos_info.t-t0,pos_info.z[:,11],"-o") + title("Open loop predictions psidot") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,3,i],"-x") + end + end + grid("on") + + figure(4) + plot(pos_info.t-t0,pos_info.z[:,9],"-o") + title("Open loop predictions v_y") + for i=1:1:size(t,1)-N + if sol_z[1,5,i]==NaN + #plot(t[i:i+N]-t0,sol_z[:,2,i],"+") + else + plot(t[i:i+N]-t0,sol_z[:,2,i],"-x") + end + end + grid("on") + + + figure(2) + ax1=subplot(411) + title("Open loop predictions e_y") + plot(pos_info.t-t0,pos_info.z[:,2],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,2,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,5,i]) + end + end + grid("on") + + subplot(412,sharex=ax1) + title("Open loop predictions e_psi") + plot(pos_info.t-t0,pos_info.z[:,3],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,3,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,4,i]) + end + end + grid("on") + + subplot(413,sharex=ax1) + title("Open loop predictions v") + plot(pos_info.t-t0,pos_info.z[:,8],"-o") + for i=1:2:size(t,1)-N + if sol_z[1,5,i]==NaN + plot(t[i:i+N]-t0,sol_z[:,4,i],"-+") + else + plot(t[i:i+N]-t0,sol_z[:,1,i]) + end + end + grid("on") + + N = 7 + subplot(414,sharex=ax1) + title("Open loop inputs") + for i=1:2:size(t,1)-N + plot(t[i:i+N-3]-t0,sol_u[1:N-2,2,i],t[i:i+N-1]-t0,sol_u[1:N,1,i]) + end + grid("on") +end + + +function eval_sysID(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + oldTraj = d_lmpc["oldTraj"] + t = d_lmpc["t"] + state = d_lmpc["state"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + cost = d_lmpc["cost"] + curv = d_lmpc["curv"] + c_Vx = d_lmpc["c_Vx"] + c_Vy = d_lmpc["c_Vy"] + c_Psi = d_lmpc["c_Psi"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + step_diff = d_lmpc["step_diff"] # this is the command how it was sent by the MPC + + x_est = d_lmpc["x_est"] + coeffX = d_lmpc["coeffX"] + coeffY = d_lmpc["coeffY"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + cmd_log = d_rec["cmd_log"] # this is the command how it was received by the simulator + pos_info = d_rec["pos_info"] + + t0 = imu_meas.t[1] + + figure(1) # longitudinal (xDot) + ax1=subplot(211) + title("Vx") + plot(t-t0,c_Vx) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax1) + plot(cmd_log.t-t0,cmd_log.z[:,1]) + grid("on") + + figure(2) # longitudinal (xDot) + ax2=subplot(211) + title("Psi") + plot(t-t0,c_Psi) + legend(["c1","c2","c3"]) + grid("on") + subplot(212,sharex=ax2) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") + + figure(3) # longitudinal (xDot) + ax3=subplot(211) + title("Vy") + plot(t-t0,c_Vy) + legend(["c1","c2","c3","c4"]) + grid("on") + subplot(212,sharex=ax3) + plot(cmd_log.t-t0,cmd_log.z[:,2]) + grid("on") +end + + +function eval_oldTraj(code::AbstractString,i::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + #t = d["t"] + plot(oldTraj.oldTimes[:,i],oldTraj.oldTraj[:,:,i],"-x") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY","s"]) + figure() + plot(oldTraj.oldTimes[:,i],oldTraj.oldInput[:,:,i],"-x") + grid("on") + legend(["a","d_f"]) + figure() + plot(oldTraj.oldTraj[:,6,i],oldTraj.oldTraj[:,1:5,i],"-x") + grid("on") + legend(["v_x","v_x","psiDot","ePsi","eY"]) +end + +function eval_LMPC_coeff(code::AbstractString,k::Int64) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + cost = d["cost"] + + s = sol_z[:,6,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(311) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) + grid() + title("Position = $(s[1]), k = $k") + xlabel("s") + ylabel("e_Y") + subplot(312) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(313) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + grid() + xlabel("s") + ylabel("v_x") + println("Cost = $(cost[k,3])") +end + +function anim_LMPC_coeff(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d = load(log_path_LMPC) + oldTraj = d["oldTraj"] + sol_z = d["sol_z"] + sol_u = d["sol_u"] + coeffCost = d["coeffCost"] + coeffConst = d["coeffConst"] + cost = d["cost"] + sol_status = d["sol_status"] + + i=1 + while isnan(sol_z[1,6,i]) + i = i+1 + end + for k=i:size(cost,1) + clf() + s = sol_z[:,6,k] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + subplot(511) + plot(s,sol_z[:,5,k],"-o",s,ss*coeffConst[:,1,5,k],s,ss*coeffConst[:,2,5,k]) + ylim([-0.4,0.4]) + grid() + title("Position = $(s[1]), k = $k, status = $(sol_status[k])") + xlabel("s") + ylabel("e_Y") + subplot(512) + plot(s,sol_z[:,4,k],"-o",s,ss*coeffConst[:,1,4,k],s,ss*coeffConst[:,2,4,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("e_Psi") + subplot(513) + plot(s,sol_z[:,3,k],"-o",s,ss*coeffConst[:,1,3,k],s,ss*coeffConst[:,2,3,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("psiDot") + subplot(514) + plot(s,sol_z[:,1,k],"-o",s,ss*coeffConst[:,1,1,k],s,ss*coeffConst[:,2,1,k]) + ylim([0.6,1.5]) + grid() + xlabel("s") + ylabel("v_x") + subplot(515) + plot(s,sol_z[:,2,k],"-o",s,ss*coeffConst[:,1,2,k],s,ss*coeffConst[:,2,2,k]) + ylim([-0.5,0.5]) + grid() + xlabel("s") + ylabel("v_y") + println("Cost = $(cost[k,3])") + end +end + +function visualize_tdiff(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + t_diff = diff(pos_info.t_msg) + t_diff = t_diff[t_diff .< 0.08] + plt[:hist](t_diff,100) + grid("on") + xlabel("t_diff") + nothing +end + +function anim_run(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + d_rec = load(log_path_record) + pos_info = d_rec["pos_info"] + L_a = 0.125 + w = 0.15 + alpha = atan(w/2/L_a) + l = sqrt(L_a^2+(w/2)^2) + t0 = pos_info.t[1] + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes(xlim = (-3,3),ylim=(-5,1)) + + track = create_track(0.4) + plot(track[:,1],track[:,2],"b.",track[:,3],track[:,4],"r-",track[:,5],track[:,6],"r-") + grid("on") + car = ax[:plot]([],[],"r-+")[1] + gps = ax[:plot]([],[],"g*")[1] + h_ti = ax[:title]("abc") + function init() + car[:set_data]([],[]) + return (car,None) + end + function animate(k) + i = k + 2000 + car_c = [pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi-alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]+pi+alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]+pi+alpha)*l; + pos_info.z[i,6]+cos(pos_info.z[i,10]-alpha)*l pos_info.z[i,7]+sin(pos_info.z[i,10]-alpha)*l] + car[:set_data]([car_c[:,1]],[car_c[:,2]]) + gps[:set_data]([pos_info.z[max(1,i-100):i,12]],[pos_info.z[max(1,i-100):i,13]]) + ax[:set_title]("t = $(pos_info.t[i]-t0)") + #title(pos_info.t[i]-t0) + return (car,gps,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=1000, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end + +function anim_constraints(code::AbstractString) + log_path_record = "$(homedir())/simulations/output-record-$(code).jld" + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_rec = load(log_path_record) + d_lmpc = load(log_path_LMPC) + + t = d_lmpc["t"] + sol_z = d_lmpc["sol_z"] + sol_u = d_lmpc["sol_u"] + coeffCost = d_lmpc["coeffCost"] + coeffConst = d_lmpc["coeffConst"] + + #Construct Figure and Plot Data + fig = figure(figsize=(10,10)) + ax = axes() + + pred_vx = ax[:plot]([],[],"r-+")[1] + poly_vx1 = ax[:plot]([],[],"--")[1] + poly_vx2 = ax[:plot]([],[],"--")[1] + + function init() + #car[:set_data]([],[]) + return (None) + end + function animate(k) + i = k + 1000 + s = sol_z[:,6,i] + ss = [s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + pred_vx[:set_data]([s],[sol_z[:,1,i]]) + poly_vx1[:set_data]([s],[ss*coeffConst[:,1,1,i]]) + poly_vx2[:set_data]([s],[ss*coeffConst[:,2,1,i]]) + + return (pred_vx,poly_vx1,poly_vx2,None) + end + t=0:30 + anim = animation.FuncAnimation(fig, animate, frames=100, interval=50) + anim[:save]("test2.mp4", bitrate=-1, extra_args=["-vcodec", "libx264", "-pix_fmt", "yuv420p"]); +end + +# ***************************************************************** +# ****** HELPER FUNCTIONS ***************************************** +# ***************************************************************** + +function anim_MPC(z) + figure() + hold(0) + grid(1) + for i=1:size(z,3) + plot(z[:,:,i]) + xlim([1,11]) + ylim([-2,2]) + sleep(0.01) + end +end + +function anim_curv(curv) + s = 0.0:.05:2.0 + figure() + hold(0) + #ss = [s.^10 s.^9 s.^8 s.^7 s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + ss = [s.^6 s.^5 s.^4 s.^3 s.^2 s.^1 s.^0] + for i=1:size(curv,1) + c = ss*curv[i,:]' + plot(s,c) + xlim([0,2]) + ylim([-1.5,1.5]) + sleep(0.1) + end +end + +# function eval_prof() +# Profile.clear() +# @load "$(homedir())/simulations/profile.jlprof" +# ProfileView.view(li, lidict=lidict) +# end + +function create_track(w) + x = [0.0] # starting point + y = [0.0] + x_l = [0.0] # starting point + y_l = [w] + x_r = [0.0] # starting point + y_r = [-w] + ds = 0.03 + + theta = [0.0] + + # SOPHISTICATED TRACK + # add_curve(theta,30,0.0) + # add_curve(theta,60,-2*pi/3) + # add_curve(theta,90,pi) + # add_curve(theta,80,-5*pi/6) + # add_curve(theta,10,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,50,0.0) + # add_curve(theta,40,-pi/4) + # add_curve(theta,30,pi/4) + # add_curve(theta,20,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,25,0.0) + # add_curve(theta,50,-pi/2) + # add_curve(theta,28,0.0) + + # # SIMPLE track + # add_curve(theta,50,0) + # add_curve(theta,100,-pi) + # add_curve(theta,100,0) + # add_curve(theta,100,-pi) + # add_curve(theta,49,0) + + # GOGGLE TRACK + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,-pi/6) + # add_curve(theta,30,pi/3) + # add_curve(theta,20,-pi/6) + # add_curve(theta,40,-pi/2) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + # SIMPLE GOGGLE TRACK + + # add_curve(theta,30,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,20,pi/10) + # add_curve(theta,30,-pi/5) + # add_curve(theta,20,pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,35,0) + + add_curve(theta,60,0) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,40,pi/10) + add_curve(theta,60,-pi/5) + add_curve(theta,40,pi/10) + add_curve(theta,80,-pi/2) + add_curve(theta,20,0) + add_curve(theta,80,-pi/2) + add_curve(theta,75,0) + + # SIMPLE GOOGLE TRACK FOR 3110 + + # add_curve(theta,25,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,17,-pi/10) + # add_curve(theta,25,pi/5) + # add_curve(theta,17,-pi/10) + # add_curve(theta,40,-pi/2) + # add_curve(theta,10,0) + # add_curve(theta,40,-pi/2) + # add_curve(theta,32,0) + + # OVAL TRACK FOR TESTS IN VSD + + # add_curve(theta,80,0) + # add_curve(theta,110,-pi) + # add_curve(theta,160,0) + # add_curve(theta,110,-pi) + # add_curve(theta,80,0) + + # add_curve(theta,53,0) + # add_curve(theta,73,-pi) + # add_curve(theta,106,0) + # add_curve(theta,73,-pi) + # add_curve(theta,53,0) + + + # # SHORT SIMPLE track + # add_curve(theta,10,0) + # add_curve(theta,80,-pi) + # add_curve(theta,20,0) + # add_curve(theta,80,-pi) + # add_curve(theta,9,0) + + for i=1:length(theta) + push!(x, x[end] + cos(theta[i])*ds) + push!(y, y[end] + sin(theta[i])*ds) + push!(x_l, x[end-1] + cos(theta[i]+pi/2)*w) + push!(y_l, y[end-1] + sin(theta[i]+pi/2)*w) + push!(x_r, x[end-1] + cos(theta[i]-pi/2)*w) + push!(y_r, y[end-1] + sin(theta[i]-pi/2)*w) + end + track = cat(2, x, y, x_l, y_l, x_r, y_r) + #plot(track[:,1],track[:,2]) + return track + #plot(x,y,x_l,y_l,x_r,y_r) +end + +function add_curve(theta::Array{Float64}, length::Int64, angle) + d_theta = 0 + curve = 2*sum(1:length/2)+length/2 + for i=0:length-1 + if i < length/2+1 + d_theta = d_theta + angle / curve + else + d_theta = d_theta - angle / curve + end + push!(theta, theta[end] + d_theta) + end +end + +function initPlot() + linewidth = 0.4 + rc("axes", linewidth=linewidth) + rc("lines", linewidth=linewidth, markersize=2) + #rc("font", family="") + rc("axes", titlesize="small", labelsize="small") # can be set in Latex + rc("xtick", labelsize="x-small") + rc("xtick.major", width=linewidth/2) + rc("ytick", labelsize="x-small") + rc("ytick.major", width=linewidth/2) + rc("legend", fontsize="small") + rc("font",family="serif") + rc("font",size=8) + rc("figure",figsize=[4.5,3]) + #rc("pgf", texsystem="pdflatex",preamble=L"""\usepackage[utf8x]{inputenc}\usepackage[T1]{fontenc}\usepackage{lmodern}""") +end + +function simModel(z,u,dt,l_A,l_B) + + # kinematic bicycle model + # u[1] = acceleration + # u[2] = steering angle + + bta = atan(l_A/(l_A+l_B)*tan(u[2])) + + zNext = copy(z) + zNext[1] = z[1] + dt*(z[4]*cos(z[3] + bta)) # x + zNext[2] = z[2] + dt*(z[4]*sin(z[3] + bta)) # y + zNext[3] = z[3] + dt*(z[4]/l_B*sin(bta)) # psi + zNext[4] = z[4] + dt*(u[1] - 0.5 * z[4]) # v + + return zNext +end + +function checkTimes(code::AbstractString) + log_path_LMPC = "$(homedir())/simulations/output-LMPC-$(code).jld" + d_lmpc = load(log_path_LMPC) + + t_solv = d_lmpc["t_solv"] + sol_status = d_lmpc["sol_status"] + cmd = d_lmpc["cmd"] # this is the command how it was sent by the MPC + sol_status_int = zeros(size(t_solv,1)) + t = d_lmpc["t"] + t0 = t[1] + + for i=1:size(sol_status,1) + if sol_status[i]==:Optimal + sol_status_int[i] = 1 + elseif sol_status[i]==:Infeasible + sol_status_int[i] = 2 + elseif sol_status[i]==:UserLimit + sol_status_int[i] = 3 + end + end + ax1=subplot(211) + plot(t,t_solv) + plot(t,sol_status_int,"*") + grid("on") + subplot(212,sharex=ax1) + plot(t,cmd,"-x") + grid("on") + + figure(2) + plot(t-t0,t_solv) + title("Solver time") + grid("on") + xlabel("t [s]") + ylabel("Solver time [s]") +end + +function checkConnectivity(code::AbstractString) + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + pos_info = d_rec["pos_info"] + imu_meas = d_rec["imu_meas"] + gps_meas = d_rec["gps_meas"] + + plot(gps_meas.t,gps_meas.z,"-x",gps_meas.t_msg,gps_meas.z,"--x",pos_info.t,pos_info.z[:,12:13],"-*",pos_info.t_msg,pos_info.z[:,12:13],"--*") + grid("on") +end + +function smooth(x,n) + y = zeros(size(x)) + for i=1:size(x,1) + start = max(1,i-n) + fin = min(size(x,1),start + 2*n) + y[i,:] = mean(x[start:fin,:],1) + end + return y +end + +function xyObstacle(oldSS,obs_log::Array{Float64},obstacle::Int64,lap::Int64,track::Array{Float64}) + + obs = obs_log[:,:,obstacle,lap] + + + + buffersize = size(obs)[1] + + + + # println("obs= ",obs) + + OrderXY = 10 + OrderThetaCurv = 8 + + + + ds = 0.03 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,buffersize,1) + xyPathAngle = [] + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:buffersize + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + #println("number of nodes= ",n_nodes) + s_start = (obs[i,1] - 1) + #println("s_start= ",s_start) + s_end = (obs[i,1] + 5) + #println("s_end= ",s_end) + s_nearest = obs[i,1] + + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) + + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end + + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) + + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = obs[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = obs[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* obs[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + obs[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - obs[i,2]*sin(xyPathAngle) + + + end + + return pred_sol_xy, xyPathAngle +end + + + + + +function xyPrediction(oldSS,s_ey_pred::Array{Float64},track::Array{Float64}) + + + pred_horizon = size(s_ey_pred)[1] + + + OrderXY = 10 + OrderThetaCurv = 8 + + + + ds = 0.03 + + s_vec = zeros(OrderXY+1) + + pred_sol_xy = zeros(2,pred_horizon,1) + xyPathAngle = [] + + x_track = track[:,1] + + y_track = track[:,2] + + #println("x_track= ",x_track) + # println("nodes= ",size([x_track'; y_track'])) + for i = 1:pred_horizon + + + + + nodes = [x_track'; y_track'] + n_nodes = size(x_track)[1] + #println("number of nodes= ",n_nodes) + s_start = (s_ey_pred[i,1] - 1) + #println("s_start= ",s_start) + s_end = (s_ey_pred[i,1] + 5) + #println("s_end= ",s_end) + s_nearest = s_ey_pred[i,1] + + idx_start = round(s_start / ds) + #println("idx_start= ",idx_start) + idx_end = round(s_end / ds) + #println("idx_end= ",idx_end) + + + # if idx_start>n_nodes + # idx_start=idx_start%n_nodes + # idx_end=idx_end%n_nodes + # end + + + + if idx_start<=0 + nodes_XY = hcat(nodes[:,n_nodes+idx_start:n_nodes],nodes[:,1:idx_end]) # then stack the end and beginning of a lap together + # #nodes_Y = hcat(nodes[2,n_nodes+idx_start:n_nodes],nodes[2,1:idx_end]) + + #idx_start = n_nodes+idx_start + elseif idx_end>=n_nodes # if the end is behind the finish line + nodes_XY = hcat(nodes[:,idx_start:n_nodes],nodes[:,1:idx_end-n_nodes]) # then stack the end and beginning of the lap together + #nodes_Y = hcat(nodes[2,idx_start:n_nodes],nodes[2,1:idx_end-n_nodes]) + else # if we are somewhere in the middle of the track + nodes_XY = nodes[:,idx_start:idx_end] # then just use the nodes from idx_start to end for interpolation + #nodes_Y = nodes[2,idx_start:idx_end] + end + + + nodes_X = vec(nodes_XY[1,:]) + nodes_Y = vec(nodes_XY[2,:]) + #println("length node xy= ",length(nodes_XY[1,:])) + + n_poly = round(Int,length(nodes_XY[1,:])) + #println("n_poly= ",n_poly) + + + + itp_matrix = zeros(n_poly,OrderXY+1) + + for ind=1:n_poly + for k=0:OrderXY + + itp_matrix[ind,OrderXY+1-k] = (s_start + (ind-1)*ds)^k + end + end + + itp_matrix_curv = zeros(n_poly,OrderThetaCurv+1) + + for ind=1:n_poly + for k=0:OrderThetaCurv + + itp_matrix_curv[ind,OrderThetaCurv+1-k] = (s_start + (ind-1)*ds)^k + end + end + + # println("size of nodes x= ",size(nodes_X)) + # println("size of itpmatrix= ",size(itp_matrix)) + # println("s start= ",s_start) + # println("s end= ",s_end) + + coeffY = itp_matrix\nodes_Y + coeffX = itp_matrix\nodes_X + + + b_curvature_vector = zeros(n_poly) + + Counter = 1 + + + for ind = 0:n_poly-1 + s_expression_der = zeros(OrderXY+1) + s_expression_2der = zeros(OrderXY+1) + s_poly = s_start + ind*ds + for k=0:OrderXY-1 + s_expression_der[OrderXY-k] = (k+1)*s_poly^k + end + for k=0:OrderXY-2 + s_expression_2der[OrderXY-1-k] = (2+k*(3+k))*s_poly^k + end + + dX = dot(coeffX,s_expression_der) + dY = dot(coeffY,s_expression_der) + ddX = dot(coeffX,s_expression_2der) + ddY = dot(coeffY,s_expression_2der) + + curvature = (dX*ddY-dY*ddX)/((dX^2+dY^2)^(3/2)) #standard curvature formula + + b_curvature_vector[Counter] = curvature + + Counter = Counter + 1 + end + + + + coeffCurv = itp_matrix_curv\b_curvature_vector + + s0 = s_ey_pred[i,1]+0.001 + + s_vec = zeros(OrderXY+1)::Array{Float64} + sdot_vec = zeros(OrderXY+1)::Array{Float64} + + for k = 1:OrderXY+1 + s_vec[k] = s_ey_pred[i,1]^(OrderXY-k+1) + + end + for k = 1:OrderXY + sdot_vec[k] = (OrderXY+1-k)* s_ey_pred[i,1]^(OrderXY-k) + end + + + XCurve = dot(coeffX,s_vec) + YCurve = dot(coeffY,s_vec) + + dX = dot(coeffX,sdot_vec) + dY = dot(coeffY,sdot_vec) + + + xyPathAngle = atan2(dY,dX) + + pred_sol_xy[2,i] = YCurve + s_ey_pred[i,2]*cos(xyPathAngle) + pred_sol_xy[1,i] = XCurve - s_ey_pred[i,2]*sin(xyPathAngle) + + + end + + + return pred_sol_xy +end diff --git a/eval_sim/mapping_francesco.jl b/eval_sim/mapping_francesco.jl new file mode 100644 index 00000000..7fcd9bb5 --- /dev/null +++ b/eval_sim/mapping_francesco.jl @@ -0,0 +1,80 @@ + +using PyPlot +using JLD +#using Optim + +# type Measurements{T} +# i::Int64 # measurement counter +# t::Array{Float64} # time data (when it was received by this recorder) +# t_msg::Array{Float64} # time that the message was sent +# z::Array{T} # measurement values +# end + +function data_analysis(code::AbstractString) + + log_path_record = "$(homedir())/open_loop/output-record-$code.jld" + d_rec = load(log_path_record) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + # t_cmd = d_rec["t_cmd"] + # t_no_cmd = d_rec["t_no_cmd"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure() + plot(v_opt) + + # half = round(length(v_opt)/2) + # println(half) + + #v_mean = mean(v_opt[half:end]) + + #v_mean = mean(v_opt[length(v_opt)/2:end]) + + #a = v_mean/4 + + a = v_opt[end]/4 + + println("a= ",a) + + figure(2) + + plot(cmd_opt[1],a, "*") + + + # println(t_cmd) + # println(t_no_cmd) + # println(t0) + + # figure() + # plot(cmd,v) + + # figure() + # plot(t,cmd) + + # figure() + # plot(t,v) + + # figure() + # plot(v) + + # figure() + # plot(t) +end \ No newline at end of file diff --git a/eval_sim/test_load.jl b/eval_sim/test_load.jl new file mode 100644 index 00000000..3a21b483 --- /dev/null +++ b/eval_sim/test_load.jl @@ -0,0 +1,217 @@ +using PyPlot +using JLD + +function test(path_acceleration::AbstractString,path_deceleration::AbstractString) + + + files_acc = readdir(path_acceleration) + files_dec = readdir(path_deceleration) + + a = zeros(length(files_acc) + length(files_dec)) + println("number of tests= ",length(a)) + + ## Loop for acceleration + + v_acceleration1 = [] #zeros(length(files)) + v_acceleration2 = [] + + commands_acc = [] #zeros(length(files)) + + for index = 1:length(files_acc) + + file = files_acc[index] + + log_path = "$(homedir())/open_loop/acceleration/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.>90] + cmd_opt = cmd[cmd.>90] + + figure(1) + + plot(v_opt) + grid("on") + + + # println("length= ",length(files)) + # println("v_opt= ",length(v_opt)) + # println("cmd_opt= ",length(cmd_opt)) + # println("v_opt[end]= ",v_opt[end]) + # println("a[i]= ",a[i]) + # println("v_opt[end]/4= ",v_opt[end]/4) + + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_acceleration1 = vcat(v_acceleration1,v_lms1) + v_acceleration2 = vcat(v_acceleration2,v_lms2) + a[index] = v_opt[end]/4 + commands_acc = vcat(commands_acc,cmd_opt[1:end-1]) + + + # println("a= ",a) + + + figure(2) + + plot(cmd_opt[1],a[index], "*") + grid("on") + + end + + ## Loop for deceleration + + v_deceleration1 = [] #zeros(length(files)) + v_deceleration2 = [] + + commands_dec = [] #zeros(length(files)) + + for index = 1:length(files_dec) + + file = files_dec[index] + + log_path = "$(homedir())/open_loop/deceleration/$file" + + d_rec=load(log_path) + + cmd_pwm_log = d_rec["cmd_pwm_log"] + vel_est = d_rec["vel_est"] + + t0 = max(cmd_pwm_log.t[1],vel_est.t[1]) + t_end = min(cmd_pwm_log.t[end],vel_est.t[end]) + + t = t0+0.1:.02:t_end-0.1 + v = zeros(length(t)) + cmd = zeros(length(t)) + + for i=1:length(t) + v[i] = vel_est.z[t[i].>vel_est.t,1][end] + #v[i] = pos_info.z[t[i].>pos_info.t,15][end] + cmd[i] = cmd_pwm_log.z[t[i].>cmd_pwm_log.t,1][end] + end + + v_opt = v[1:end] + cmd_opt = cmd[1:end] + v_opt = v[cmd.<100] + cmd_opt = cmd[cmd.<100] + + v_equal = 0 + flag = 0 + + for i = 2:length(v_opt) + + if v_opt[i]==v_opt[i-1] + v_equal = v_equal + 1 + else + v_equal = 0 + end + + if v_equal > 10 + flag = i + break + end + end + + v_opt = v_opt[1:flag] + cmd_opt = cmd_opt[1:flag] + + a[length(files_acc) + index] = (- v_opt[1])/4 + + # plot(cmd_opt[5],a[length(files_acc) + index], "*") + # grid("on") + + + + figure(3) + + plot(v_opt) + grid("on") + + + v_lms1 = zeros(length(v_opt)-1) + v_lms2 = zeros(length(v_opt)-1) + + + for j = 1:length(v_opt)-1 + + v_lms1[j] = v_opt[j+1] - v_opt[j] + v_lms2[j] = v_opt[j] + + end + + v_deceleration1 = vcat(v_deceleration1,v_lms1) + v_deceleration2 = vcat(v_deceleration2,v_lms2) + commands_dec = vcat(commands_dec,cmd_opt[1:end-1]) + + + + + end + + + + + + # Least Mean Square + + # println("length of v_acceleration1= ",length(v_acceleration1)) + # println("length of commands= ",length(commands)) + + + A = zeros(length(v_acceleration1)+length(v_deceleration1),5) + B = zeros(length(v_acceleration1)+length(v_deceleration1),1) + x = zeros(5,1) #[c1_+;c2_+,cM] + + A[1:length(commands_acc),1] = commands_acc + A[1:length(commands_acc),2] = 1 + A[1:length(v_acceleration2),3] = -v_acceleration2 + A[length(v_acceleration2)+1:end,3] = -v_deceleration2 + A[length(commands_acc)+1:end,4] = commands_dec + A[length(commands_acc)+1:end,5] = 1 + + B[1:length(v_acceleration1)] = v_acceleration1 + B[length(v_acceleration1)+1:end] = v_deceleration1 + + A = 0.02*A # 0.02 is the discretization + + + x = inv(A'*A)*A'*B + + println("the computed constants are: ", x) + + # x_axis = (findmin(commands)[1]):0.02:(findmax(commands)[1]) + # y_axis = x[1]*x_axis + x[2] + # plot(x_axis,y_axis) + + + + +end diff --git a/odroid_setup.sh b/odroid_setup.sh new file mode 100644 index 00000000..6201e3b0 --- /dev/null +++ b/odroid_setup.sh @@ -0,0 +1,2 @@ +export ROS_MASTER_URI=http://10.0.0.14:11311 +export ROS_IP=10.0.0.1 diff --git a/scripts/startup.sh b/scripts/startup.sh index f13ab2c3..16d1c109 100755 --- a/scripts/startup.sh +++ b/scripts/startup.sh @@ -1,7 +1,7 @@ #!/bin/bash # local start up script when opening bash session - +# set environment variable for python export WORKON_HOME=$HOME/.virtualenvs export VIRTUALENVWRAPPER_PYTHON=/usr/bin/python export VIRTUALENVWRAPPER_VIRTUALENV=/usr/local/bin/virtualenv @@ -9,11 +9,28 @@ export VIRTUALENVWRAPPER_VIRTUALENV_ARGS='--no-site-packages' source /usr/local/bin/virtualenvwrapper.sh workon barc -source /home/odroid/team_name.sh +# set environment variables for ROS +source $HOME/barc/workspace/devel/setup.bash +export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ -alias nanorst='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' -alias rebuild_system='source ~/barc/scripts/rebuild_system.sh' +# set environment variables for Amazon Web Server +source $HOME/team_name.sh -export ROS_HOME=$HOME/barc/workspace/src/barc/rosbag/ +# define commands +# * nanorst - resets the arduino nano from the command line (assuming the device is connected and on port /dev/ttyUSB0 +# * rebuild_system - rebuild all the ROS packages +alias flash_nano='cd ~/barc/arduino/.arduino_nano328_node; cp ../arduino_nano328_node/arduino_nano328_node.ino src/; ano clean; ano build -m nano328; ano upload -m nano328 -p /dev/ttyUSB0; roscd barc' +alias rebuild_system='source ~/barc/scripts/rebuild_system.sh' +alias tmux='tmux -2' +alias reset_wifi_rules='sudo rm /etc/udev/rules.d/70-persistent-net.rules' +alias set_init_ap='sudo cp $HOME/barc/scripts/accesspoint.conf /etc/init/accesspoint.conf' +# set configuration script for vim text editor cp ~/barc/scripts/vimrc ~/.vimrc + + +# added git branch listing (michael) +parse_git_branch() { + git branch 2> /dev/null | sed -e '/^[^*]/d' -e 's/* \(.*\)/ (\1)/' +} +export PS1="\u@\h \[\033[32m\]\w\[\033[33m\]\$(parse_git_branch)\[\033[00m\] $ " diff --git a/workspace/src/CMakeLists.txt b/workspace/src/CMakeLists.txt index 3703df4e..581e61db 120000 --- a/workspace/src/CMakeLists.txt +++ b/workspace/src/CMakeLists.txt @@ -1 +1 @@ -/opt/ros/indigo/share/catkin/cmake/toplevel.cmake \ No newline at end of file +/opt/ros/kinetic/share/catkin/cmake/toplevel.cmake \ No newline at end of file diff --git a/workspace/src/barc/CMakeLists.txt b/workspace/src/barc/CMakeLists.txt index 366e2c82..0e80eaae 100644 --- a/workspace/src/barc/CMakeLists.txt +++ b/workspace/src/barc/CMakeLists.txt @@ -47,10 +47,14 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES - Ultrasound.msg - Encoder.msg + pos_info.msg ECU.msg - Z_KinBkMdl.msg + Vel_est.msg + mpc_solution.msg + xy_prediction.msg + theta.msg + selected_states.msg + prediction.msg ) ## Generate services in the 'srv' folder diff --git a/workspace/src/barc/launch/barc_sim.launch b/workspace/src/barc/launch/barc_sim.launch new file mode 100644 index 00000000..31fd44b0 --- /dev/null +++ b/workspace/src/barc/launch/barc_sim.launch @@ -0,0 +1,68 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/barc_sim_feature_data_collecting.launch b/workspace/src/barc/launch/barc_sim_feature_data_collecting.launch new file mode 100644 index 00000000..33093392 --- /dev/null +++ b/workspace/src/barc/launch/barc_sim_feature_data_collecting.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/estimation_DynBkMdl.launch b/workspace/src/barc/launch/estimation_DynBkMdl.launch deleted file mode 100644 index cab1838a..00000000 --- a/workspace/src/barc/launch/estimation_DynBkMdl.launch +++ /dev/null @@ -1,42 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/estimation_KinBkMdl.launch b/workspace/src/barc/launch/estimation_KinBkMdl.launch deleted file mode 100644 index c9cfb374..00000000 --- a/workspace/src/barc/launch/estimation_KinBkMdl.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/closedLoop_mpc.launch b/workspace/src/barc/launch/follow_traj.launch similarity index 59% rename from workspace/src/barc/launch/closedLoop_mpc.launch rename to workspace/src/barc/launch/follow_traj.launch index 8298e38a..9083988b 100644 --- a/workspace/src/barc/launch/closedLoop_mpc.launch +++ b/workspace/src/barc/launch/follow_traj.launch @@ -1,10 +1,11 @@ + - - + + @@ -14,25 +15,38 @@ - - - - - + + + + + - + - - + + + + - - + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/gps-remote.launch b/workspace/src/barc/launch/gps-remote.launch new file mode 100644 index 00000000..24642d62 --- /dev/null +++ b/workspace/src/barc/launch/gps-remote.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/workspace/src/barc/launch/imu-remote.launch b/workspace/src/barc/launch/imu-remote.launch new file mode 100644 index 00000000..c5e08b01 --- /dev/null +++ b/workspace/src/barc/launch/imu-remote.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/low_level_mapping.launch b/workspace/src/barc/launch/low_level_mapping.launch new file mode 100755 index 00000000..b2b08019 --- /dev/null +++ b/workspace/src/barc/launch/low_level_mapping.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/multiple_physical.launch b/workspace/src/barc/launch/multiple_physical.launch new file mode 100755 index 00000000..9343a00f --- /dev/null +++ b/workspace/src/barc/launch/multiple_physical.launch @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/openLoop_circular.launch b/workspace/src/barc/launch/openLoop_circular.launch deleted file mode 100644 index 44d3d417..00000000 --- a/workspace/src/barc/launch/openLoop_circular.launch +++ /dev/null @@ -1,48 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/openLoop_straight.launch b/workspace/src/barc/launch/openLoop_straight.launch deleted file mode 100644 index 56142084..00000000 --- a/workspace/src/barc/launch/openLoop_straight.launch +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/open_loop_remote.launch b/workspace/src/barc/launch/open_loop_remote.launch new file mode 100644 index 00000000..7322c583 --- /dev/null +++ b/workspace/src/barc/launch/open_loop_remote.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch b/workspace/src/barc/launch/run_lmpc_remote.launch new file mode 100644 index 00000000..52a86790 --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_remote.launch @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote.launch.orig b/workspace/src/barc/launch/run_lmpc_remote.launch.orig new file mode 100644 index 00000000..7e53c1ae --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_remote.launch.orig @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote_shuqi.launch b/workspace/src/barc/launch/run_lmpc_remote_shuqi.launch new file mode 100644 index 00000000..4509f6cf --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_remote_shuqi.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/run_lmpc_remote_shuqi_feature_data_collecting.launch b/workspace/src/barc/launch/run_lmpc_remote_shuqi_feature_data_collecting.launch new file mode 100644 index 00000000..cfe2b9c5 --- /dev/null +++ b/workspace/src/barc/launch/run_lmpc_remote_shuqi_feature_data_collecting.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test.launch b/workspace/src/barc/launch/sensor_test.launch deleted file mode 100644 index ae8c0c29..00000000 --- a/workspace/src/barc/launch/sensor_test.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/barc/launch/sensor_test_barc.launch b/workspace/src/barc/launch/sensor_test_barc.launch new file mode 100644 index 00000000..ec09a023 --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_barc.launch @@ -0,0 +1,45 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/sensor_test_remote.launch b/workspace/src/barc/launch/sensor_test_remote.launch new file mode 100644 index 00000000..724ef606 --- /dev/null +++ b/workspace/src/barc/launch/sensor_test_remote.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + ` + + + + + + + diff --git a/workspace/src/barc/launch/test.launch b/workspace/src/barc/launch/test.launch new file mode 100755 index 00000000..20ddf968 --- /dev/null +++ b/workspace/src/barc/launch/test.launch @@ -0,0 +1,92 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_2.launch b/workspace/src/barc/launch/test_2.launch new file mode 100755 index 00000000..0401a7aa --- /dev/null +++ b/workspace/src/barc/launch/test_2.launch @@ -0,0 +1,74 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_multiple.launch b/workspace/src/barc/launch/test_multiple.launch new file mode 100755 index 00000000..6b0f1797 --- /dev/null +++ b/workspace/src/barc/launch/test_multiple.launch @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_multiple_physical_1.launch b/workspace/src/barc/launch/test_multiple_physical_1.launch new file mode 100755 index 00000000..f9038281 --- /dev/null +++ b/workspace/src/barc/launch/test_multiple_physical_1.launch @@ -0,0 +1,141 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_odroid.launch b/workspace/src/barc/launch/test_odroid.launch new file mode 100755 index 00000000..670f061d --- /dev/null +++ b/workspace/src/barc/launch/test_odroid.launch @@ -0,0 +1,119 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_odroid_2.launch b/workspace/src/barc/launch/test_odroid_2.launch new file mode 100644 index 00000000..fd97d89a --- /dev/null +++ b/workspace/src/barc/launch/test_odroid_2.launch @@ -0,0 +1,96 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/test_odroid_2_beacons.launch b/workspace/src/barc/launch/test_odroid_2_beacons.launch new file mode 100644 index 00000000..196a438b --- /dev/null +++ b/workspace/src/barc/launch/test_odroid_2_beacons.launch @@ -0,0 +1,118 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/track_setup.launch b/workspace/src/barc/launch/track_setup.launch new file mode 100755 index 00000000..5bc97cfc --- /dev/null +++ b/workspace/src/barc/launch/track_setup.launch @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/launch/track_setup_single.launch b/workspace/src/barc/launch/track_setup_single.launch new file mode 100755 index 00000000..86741645 --- /dev/null +++ b/workspace/src/barc/launch/track_setup_single.launch @@ -0,0 +1,75 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/workspace/src/barc/msg/ECU.msg b/workspace/src/barc/msg/ECU.msg index 2bffecbf..a37117f5 100644 --- a/workspace/src/barc/msg/ECU.msg +++ b/workspace/src/barc/msg/ECU.msg @@ -6,5 +6,6 @@ # # For modeling and state estimation, motors units are [N], and servo units are [rad] # For actuator signals, both have units of PWM angle [deg]. This relates to the duty cycle +Header header float32 motor float32 servo diff --git a/workspace/src/barc/msg/Encoder.msg b/workspace/src/barc/msg/Encoder.msg deleted file mode 100644 index 11fe3098..00000000 --- a/workspace/src/barc/msg/Encoder.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 FL -float32 FR -float32 BL -float32 BR diff --git a/workspace/src/barc/msg/Ultrasound.msg b/workspace/src/barc/msg/Ultrasound.msg deleted file mode 100644 index 2ffa3263..00000000 --- a/workspace/src/barc/msg/Ultrasound.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 front -float32 back -float32 right -float32 left diff --git a/workspace/src/barc/msg/Vel_est.msg b/workspace/src/barc/msg/Vel_est.msg new file mode 100644 index 00000000..69111051 --- /dev/null +++ b/workspace/src/barc/msg/Vel_est.msg @@ -0,0 +1,6 @@ +Header header +float32 vel_est +float32 vel_fl +float32 vel_fr +float32 vel_bl +float32 vel_br \ No newline at end of file diff --git a/workspace/src/barc/msg/Z_KinBkMdl.msg b/workspace/src/barc/msg/Z_KinBkMdl.msg deleted file mode 100644 index d76a6d56..00000000 --- a/workspace/src/barc/msg/Z_KinBkMdl.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 x -float32 y -float32 psi -float32 v diff --git a/workspace/src/barc/msg/mpc_solution.msg b/workspace/src/barc/msg/mpc_solution.msg new file mode 100644 index 00000000..7c03e33f --- /dev/null +++ b/workspace/src/barc/msg/mpc_solution.msg @@ -0,0 +1,14 @@ +# This is a message to hold data for pF/LMPC controller +Header header +float64[] z_x +float64[] z_y +float64[] z_vx +float64[] z_s +float64[] SS_x +float64[] SS_y +float64[] SS_vx +float64[] SS_s +float64[] z_fore_x +float64[] z_fore_y +float64[] z_iden_x +float64[] z_iden_y \ No newline at end of file diff --git a/workspace/src/barc/msg/pos_info.msg b/workspace/src/barc/msg/pos_info.msg new file mode 100644 index 00000000..b106badc --- /dev/null +++ b/workspace/src/barc/msg/pos_info.msg @@ -0,0 +1,17 @@ + +# This is a message to hold data with position and trajectory information +Header header +float32 s +float32 ey +float32 epsi +float32 v +float32 x +float32 y +float32 v_x +float32 v_y +float32 psi +float32 psiDot +float32 a_x +float32 a_y +float32 u_a +float32 u_df diff --git a/workspace/src/barc/msg/pos_info64.msg b/workspace/src/barc/msg/pos_info64.msg new file mode 100644 index 00000000..b80ec9b4 --- /dev/null +++ b/workspace/src/barc/msg/pos_info64.msg @@ -0,0 +1,17 @@ + +# This is a message to hold data with position and trajectory information +Header header +float64 s +float64 ey +float64 epsi +float64 v +float64 x +float64 y +float64 v_x +float64 v_y +float64 psi +float64 psiDot +float64 a_x +float64 a_y +float64 u_a +float64 u_df diff --git a/workspace/src/barc/msg/prediction.msg b/workspace/src/barc/msg/prediction.msg new file mode 100755 index 00000000..a5577dae --- /dev/null +++ b/workspace/src/barc/msg/prediction.msg @@ -0,0 +1,9 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] s +float64[] ey +float64[] epsi +float64[] psidot +float64[] vx +float64[] vy +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/msg/selected_states.msg b/workspace/src/barc/msg/selected_states.msg new file mode 100644 index 00000000..4eb73937 --- /dev/null +++ b/workspace/src/barc/msg/selected_states.msg @@ -0,0 +1,6 @@ +# This is a message to hold data of the selected states in x and y coords +Header header +float64[] x +float64[] y +float64[] s +float64[] ey \ No newline at end of file diff --git a/workspace/src/barc/msg/theta.msg b/workspace/src/barc/msg/theta.msg new file mode 100644 index 00000000..dfed806f --- /dev/null +++ b/workspace/src/barc/msg/theta.msg @@ -0,0 +1,5 @@ +# This is a message to communicate the agent's theta parameters +Header header +float64[] theta_vx +float64[] theta_vy +float64[] theta_psi_dot diff --git a/workspace/src/barc/msg/xy_prediction.msg b/workspace/src/barc/msg/xy_prediction.msg new file mode 100644 index 00000000..b1db55de --- /dev/null +++ b/workspace/src/barc/msg/xy_prediction.msg @@ -0,0 +1,8 @@ +# This is a message to communicate the agent's prediction +Header header +float64[] x +float64[] y +float64[] psi +float64[] acc +float64[] steering +int32 current_lap \ No newline at end of file diff --git a/workspace/src/barc/src/LMPC_node.jl b/workspace/src/barc/src/LMPC_node.jl new file mode 100755 index 00000000..1b876c62 --- /dev/null +++ b/workspace/src/barc/src/LMPC_node.jl @@ -0,0 +1,797 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, mpc_solution +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + +function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,z_est::Array{Float64,1},x_est::Array{Float64,1}) + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + # z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end + + # save current state in oldTraj + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = z_est + oldTraj.count[lapStatus.currentLap] += 1 +end + +function ST_callback(msg::pos_info,z_true::Array{Float64,1}) + z_true[:] = [msg.x,msg.y,msg.v_x,msg.v_y,msg.psi,msg.psiDot] + # println("z_true from LMPC node",round(z_true,4)) +end + +# This is the main function, it is called when the node is started. +function main() + println("Starting LMPC node.") + const BUFFERSIZE = 5000 + const LMPC_LAP = 30 + + const PF_FLAG = true # true:only pF, false:1 warm-up lap and LMPC + + const LMPC_FLAG = false # true:IDEN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_DYN_FLAG = false # true:DYN_LIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_KIN_FLAG = true # true:KIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + + const FEATURE_FLAG = false # true:8-shape, false:history (this requires the corresponding change in 3 palces) + const NORM_SPACE_FLAG = true # true:2-norm, false: space critirion + + const TI_TV_FLAG = true # true:TI, false:TV + + const GP_LOCAL_FLAG = false # true:local GPR + const GP_FULL_FLAG = false # true:full GPR + + const GP_HISTORY_FLAG = false # true: GPR data is from last laps, false: GP data is from data base. + + const N = 10 + const delay_df = 3 + const delay_a = 1 + + if PF_FLAG + file_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_name = "SYS_ID_TI" + else + file_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_name = "DYN_LIN" + elseif LMPC_KIN_FLAG + file_name = "KIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_name *= "_TI" + else + file_name *= "_TV" + end + end + end + if GP_LOCAL_FLAG + file_name *= "_LOCAL_GP" + elseif GP_FULL_FLAG + file_name *= "_FULL_GP" + end + + if get_param("sim_flag") + folder_name = "simulations" + else + folder_name = "experiments" + end + + + + PF_FLAG ? println("Path following") : println("Learning MPC") + FEATURE_FLAG ? println("Feature data: 8-shape database") : println("Feature data: History data base") + TI_TV_FLAG ? println("Time invariant SYS_ID") : println("Time variant SYS_ID") + println("N=$N, delay_df=$delay_df, delay_a=$delay_a") + + track_data = createTrack("MSC_lab") + track = Track(track_data) + track_fe = createTrack("feature") + track_f = Track(track_fe) + oldTraj = OldTrajectory() + posInfo = PosInfo(); posInfo.s_target=track.s; + lapStatus = LapStatus(1,1,false,false,0.3) + mpcCoeff = MpcCoeff() + mpcCoeff_dummy = MpcCoeff() + mpcSol = MpcSol() + modelParams = ModelParams() + mpcParams_pF = MpcParams() + mpcParams = MpcParams() + mpcParams_4s = MpcParams() + selectedStates = SelectedStates() + oldSS = SafeSetData() + # oldSS_true = SafeSetData() + # oldSS_true.oldSS = NaN*ones(BUFFERSIZE,6,10) # contains data from previous laps usefull to build the safe set + z_est = zeros(7) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_true = zeros(6) # (xDot, yDot, psiDot) + x_est = zeros(4) # (x, y, psi, v) + cmd = ECU() # CONTROL SIGNAL MESSAGE INITIALIZATION + mpcSol_to_pub = mpc_solution() # MPC SOLUTION PUBLISHING MESSAGE INITIALIZATION + InitializeParameters(mpcParams,mpcParams_4s,mpcParams_pF,modelParams,mpcSol, + selectedStates,oldSS,oldTraj,mpcCoeff,mpcCoeff_dummy, + LMPC_LAP,delay_df,delay_a,N,BUFFERSIZE) + z_prev = mpcSol.z + u_prev = mpcSol.u + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + d_f_lp = 0.0 + + # NODE INITIALIZATION + init_node("mpc_traj") + loop_rate = Rate(1/modelParams.dt) + # loop_rate = Rate(10.0) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + mpcSol_pub = Publisher("mpc_solution", mpc_solution, queue_size=1)::RobotOS.Publisher{barc.msg.mpc_solution}; acc_f = [0.0] + s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s2 = Subscriber("real_val", pos_info, ST_callback, (z_true,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + + num_lap = LMPC_LAP+1+max(selectedStates.Nl,selectedStates.feature_Nl) + log_cvx = zeros(BUFFERSIZE,mpcParams.N,6,num_lap) + log_cvy = zeros(BUFFERSIZE,mpcParams.N,4,num_lap) + log_cpsi = zeros(BUFFERSIZE,mpcParams.N,3,num_lap) + solHistory = SolHistory(BUFFERSIZE,mpcParams.N,6,num_lap) + selectHistory = zeros(BUFFERSIZE,num_lap,selectedStates.Nl*selectedStates.Np,6) + GP_vy_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + GP_psidot_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + selectFeatureHistory= zeros(BUFFERSIZE,num_lap,selectedStates.feature_Np,6) + statusHistory = Array{ASCIIString}(BUFFERSIZE,num_lap) + + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams) + mdl_kin = MpcModel_convhull_kin(mpcParams_4s,modelParams,selectedStates) + mdl_convhull = MpcModel_convhull_dyn_iden(mpcParams,modelParams,selectedStates) + mdl_kin_lin = MpcModel_convhull_kin_linear(mpcParams_4s,modelParams,selectedStates) + mdl_dyn_lin = MpcModel_convhull_dyn_linear(mpcParams,modelParams,selectedStates) + + if !PF_FLAG + # FUNCTION DUMMY CALLS: this is important to call all the functions that will be used before for initial compiling + data = load("$(homedir())/simulations/dummy_function/oldSS.jld") # oldSS.jld is a dummy data for initialization + oldSS_dummy = data["oldSS"] + lapStatus_dummy = LapStatus(1+max(selectedStates.feature_Nl,selectedStates.Nl),1,false,false,0.3) + selectedStates_dummy=find_SS(oldSS_dummy,selectedStates,5.2,z_prev,lapStatus_dummy,modelParams,mpcParams,track) + z = rand(1,6); u = rand(1,2) + data = load("$(homedir())/simulations/dummy_function/FeatureDataCollecting.jld") + feature_z = data["feature_z"] + feature_u = data["feature_u"] + (iden_z,iden_u)=find_feature_dist(feature_z,feature_u,z,u,selectedStates) + data = load("$(homedir())/simulations/dummy_function/path_following.jld") # oldSS.jld is a dummy data for initialization + solHistory_dummy = data["solHistory"] + # (~,~,~)=find_feature_space(solHistory_dummy,2*ones(1,6),rand(1,2),lapStatus_dummy,selectedStates,posInfo) + (c_Vx,c_Vy,c_Psi)=coeff_iden_dist(iden_z,iden_u) + (~,~,~)=solveMpcProblem_convhull_dyn_iden(mdl_convhull,mpcSol,mpcCoeff,rand(6),rand(mpcParams.N+1,6),rand(mpcParams.N,2),selectedStates_dummy,track,rand(mpcParams.N),rand(mpcParams.N)) + (~,~,~)=solveMpcProblem_convhull_dyn_linear(mdl_dyn_lin,mpcSol,mpcParams,modelParams,lapStatus,rand(mpcParams.N+1,6),rand(mpcParams.N,2),rand(mpcParams.N+1,6),rand(mpcParams.N,2),selectedStates_dummy,track,rand(mpcParams.N),rand(mpcParams.N)) + selectedStates_dummy.selStates=s6_to_s4(selectedStates_dummy.selStates) + (~,~,~)=solveMpcProblem_convhull_kin_linear(mdl_kin_lin,mpcSol,mpcParams_4s,modelParams,rand(mpcParams.N+1,4),rand(mpcParams.N,2),rand(mpcParams.N+1,4),rand(mpcParams.N,2),selectedStates_dummy,track) + (~,~,~)=solveMpcProblem_convhull_kin(mdl_kin,mpcSol,rand(4),rand(mpcParams.N+1,4),rand(mpcParams.N,2),selectedStates_dummy,track,rand(mpcParams.N),rand(mpcParams.N)) + (~,~,~)=car_pre_dyn(rand(1,6)+1,rand(mpcParams.N,2),track,modelParams,6) + (~,~,~)=find_SS_dist(solHistory,rand(1,6),rand(1,2),lapStatus,selectedStates) + end + + if FEATURE_FLAG + # FEATURE DATA READING + if get_param("feature_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld") + else + if get_param("sim_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld") + else + data = load("$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld") + end + end + feature_z = data["feature_z"] + feature_u = data["feature_u"] + println("Number of total feature points from 8-shape:",size(feature_z,1)) + end + + if GP_LOCAL_FLAG || GP_FULL_FLAG + # FEATURE DATA READING FOR GPR # THIS PART NEEDS TO BE COMMENTED OUT FOR THE FIRST TIME LMPC FOR GP DATA COLLECTING + if PF_FLAG + file_GP_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_GP_name = "SYS_ID_TI" + else + file_GP_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_GP_name = "DYN_LIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_GP_name *= "_TI" + else + file_GP_name *= "_TV" + end + end + end + if get_param("sim_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + else + data = load("$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + end + num_spare = 30 # THE NUMBER OF POINTS SELECTED FOR SPARE GP + feature_GP_z = data["feature_GP_z"] + feature_GP_u = data["feature_GP_u"] + feature_GP_vy_e = data["feature_GP_vy_e"] + feature_GP_psidot_e = data["feature_GP_psidot_e"] + feature_GP_z = feature_GP_z[1:num_spare:end,:] + feature_GP_u = feature_GP_u[1:num_spare:end,:] + feature_GP_vy_e = feature_GP_vy_e[1:num_spare:end] + feature_GP_psidot_e = feature_GP_psidot_e[1:num_spare:end] + + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + + # GP RELATED FUNCTION DUMMY CALLING + # regre(rand(1,6),rand(1,2),feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_full_vy(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + GP_full_psidot(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + else # THIS IS FOR COLLECTING FEATURE DATA FOR GPR + feature_GP_z = zeros(10000,6) + feature_GP_u = zeros(10000,2) + feature_GP_vy_e = zeros(10000) + feature_GP_psidot_e = zeros(10000) + k = 1 + end + + if PF_FLAG + lapStatus.currentLap = 1 + else + lapStatus.currentLap = 1+max(selectedStates.feature_Nl,selectedStates.Nl) + if get_param("sim_flag") + data = load("$(homedir())/simulations/path_following.jld") + else + data = load("$(homedir())/experiments/path_following.jld") + end + oldSS = data["oldSS"] + solHistory = data["solHistory"] + end + println(lapStatus) + println("track maximum curvature: ",track.max_curvature) + + println("Finished LMPC NODE initialization.") + counter = 0 + while ! is_shutdown() + if z_est[6] > 0 + + # CONTROL SIGNAL PUBLISHING + cmd.header.stamp = get_rostime() + mpcSol_to_pub.header.stamp = get_rostime() + # publish(pub, cmd) + # publish(mpcSol_pub, mpcSol_to_pub) + + # LAP SWITCHING + if lapStatus.nextLap + println("Finishing one lap at iteration ",lapStatus.currentIt) + if PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + # IN CONSISTANT WITH THE DATA SAVING PART: AVOIDING SAVING THE DATA FOR THE FIRST WARM UP LAP IN LMPC + # SAFE SET COST UPDATE + oldSS.oldCost[lapStatus.currentLap] = lapStatus.currentIt-1 + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + oldSS.cost2target[:,lapStatus.currentLap] = lapStatus.currentIt - oldSS.cost2target[:,lapStatus.currentLap] + end + + lapStatus.nextLap = false + + setvalue(mdl_pF.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_pF.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + + if z_prev[1,1]>posInfo.s_target + z_prev[:,1] -= posInfo.s_target + end + + lapStatus.currentLap += 1 + lapStatus.currentIt = 1 + + if GP_FULL_FLAG && GP_HISTORY_FLAG # USING DATA FROM PREVIOUS LAPS TO DO FULL GPR + # CONSTRUCT GP_related from solHistory + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + end # THIS PART IS STILL NOT READY + + if lapStatus.currentLap > num_lap # to save the data at the end before error pops up + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "oldTraj",oldTraj,"selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u,"feature_GP_vy_e",feature_GP_vy_e,"feature_GP_psidot_e",feature_GP_psidot_e) + end + end + end + + # MPC CONTROLLER OPTIMIZATION + if lapStatus.currentLap<=1+max(selectedStates.feature_Nl,selectedStates.Nl) # pF CONTROLLER + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + # println("z_curr:",round(z_curr,2)) + z_kin = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + # z_curr = xyFrame_to_trackFrame(z_true,track) + # z_kin = [z_curr[1],z_curr[2],z_curr[3],sqrt(z_curr[4]^2+z_curr[5]^2)] + # println(" state from LMPC nodes",round(mpcSol.z[2,:],2)) + # println("estimate state from LMPC nodes",round(z_curr,2)) + + (mpcSol.z,mpcSol.u,sol_status) = solveMpcProblem_pathFollow(mdl_pF,mpcParams_pF,modelParams,mpcSol,z_kin,z_prev,u_prev,track) + + else # LMPC CONTROLLER + # FOR QUICK LMPC STARTING, the next time, change the path following lap number to 1 and change the initial lapStatus to selectedStates. + if PF_FLAG + println("hahahahahahahaha") + save("$(homedir())/simulations/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + println("finish saving the PF data") + # save("$(homedir())/simulations/path_following.jld","oldSS",oldSS,"oldSS_true",oldSS_true,"solHistory",solHistory) + if get_param("sim_flag") + println("hahahahahahahaha") + else + save("$(homedir())/experiments/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + end + end + + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + # z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + # z_curr = xyFrame_to_trackFrame(z_true,track) + + # println("z_curr from LMPC node",z_curr) + + if LMPC_FLAG + ###################################################################### + ############### CHOICE 1: DO MPC ON SYS_ID MODEL ##################### + ###################################################################### + tic() + # PREPARE THE PREVIOUS SOLUTION FOR LMPC, WHEN SWITCHING FROM pF TO LMPC + if size(z_prev,2)==4 + z_prev = hcat(z_prev,zeros(size(z_prev,1),2)) + end + # println("z_prev is:",round(z_prev,3)) + # SYS_ID + if !TI_TV_FLAG + # TV SYS_ID + z_to_iden = vcat(z_curr',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i in 1:mpcParams.N + if FEATURE_FLAG + # SELECTING THE FEATURE POINTS FROM DATASET + (iden_z,iden_u,z_iden_plot)=find_feature_dist(feature_z,feature_u,z_to_iden[i,:],u_to_iden[i,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON 2-NORM CRITERION + (iden_z,iden_u,z_iden_plot)=find_SS_dist(solHistory,z_to_iden[i,:],u_to_iden[i,:],lapStatus,selectedStates) + else + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON SPATIAL CRITERION + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_to_iden[i,:],u_to_iden[i,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff.c_Vx[i,:],mpcCoeff.c_Vy[i,:],mpcCoeff.c_Psi[i,:])=coeff_iden_dist(iden_z,iden_u) + end + else + # TI SYS_ID + if FEATURE_FLAG + # SELECTING THE FEATURE POINTS FROM DATASET + (iden_z,iden_u,z_iden_plot)=find_feature_dist(feature_z,feature_u,z_curr',u_prev[2,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY + (iden_z,iden_u,z_iden_plot)=find_SS_dist(solHistory,z_curr',u_prev[2,:],lapStatus,selectedStates) + else + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_curr',u_prev[2,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff.c_Vx[1,:],mpcCoeff.c_Vy[1,:],mpcCoeff.c_Psi[1,:])=coeff_iden_dist(iden_z,iden_u) + for i in 2:mpcParams.N + mpcCoeff.c_Vx[i,:]=mpcCoeff.c_Vx[1,:] + mpcCoeff.c_Vy[i,:]=mpcCoeff.c_Vy[1,:] + mpcCoeff.c_Psi[i,:]=mpcCoeff.c_Psi[1,:] + end + # println("c_Vx",round(mpcCoeff.c_Vx[1,:],2)) + # println("c_Vy",round(mpcCoeff.c_Vy[1,:],2)) + # println("c_Psi",round(mpcCoeff.c_Psi[1,:],2)) + end + # GPR DISTURBANCE ID + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + z_to_iden = vcat(z_curr',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i = 1:mpcParams.N + if GP_LOCAL_FLAG + GP_e_vy[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy[i] = GP_full_vy(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot[i] = GP_full_psidot(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy[i] = 0 + GP_e_psidot[i] = 0 + end + end + # println(GP_e_vy) + # println(GP_e_psidot) + + # FEATURE POINTS VISUALIZATION + if FEATURE_FLAG + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track_f) + else + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track) + end + mpcSol_to_pub.z_iden_x = z_iden_x + mpcSol_to_pub.z_iden_y = z_iden_y + toc() # TIME FOR PREPARATION + # SAFESET POINT SELECTION + selectedStates=find_SS(oldSS,selectedStates,z_curr[1],z_prev,lapStatus,modelParams,mpcParams,track) + # println(selectedStates) + tic() + (mpcSol.z,mpcSol.u,sol_status)=solveMpcProblem_convhull_dyn_iden(mdl_convhull,mpcSol,mpcCoeff,z_curr,z_prev,u_prev,selectedStates,track,GP_e_vy,GP_e_psidot) + toc() # TIME FOR OPTIMIZATION + elseif LMPC_DYN_FLAG # USING LINEARIZED DYNAMIC MODEL FOR LMPC, 6 STATES LMPC CONTROLLER + ###################################################################### + ############### CHOICE 2: DO MPC ON DYN_LIN MODEL #################### + ###################################################################### + tic() + # PREPARE THE PREVIOUS SOLUTION FOR LMPC, WHEN SWITCHING FROM pF TO LMPC + if size(z_prev,2)==4 + z_prev = hcat(z_prev,zeros(size(z_prev,1),2)) + end + u_linear = vcat(u_prev[2:end,:],u_prev[end,:]) + (z_linear,~,~) = car_pre_dyn(z_curr,u_linear,track,modelParams,6) + + # SAFESET POINT SELECTION + # println(z_curr) + selectedStates=find_SS(oldSS,selectedStates,z_est[6],z_prev,lapStatus,modelParams,mpcParams,track) + # println(selectedStates) + + # GPR DISTURBANCE ID + for i = 1:mpcParams.N + if GP_LOCAL_FLAG + GP_e_vy[i] = regre(z_linear[i,:],u_linear[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot[i] = regre(z_linear[i,:],u_linear[i,:],feature_GP_psidot_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy[i] = GP_full_vy(z_linear[i,:],u_linear[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot[i] = GP_full_psidot(z_linear[i,:],u_linear[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy[i] = 0 + GP_e_psidot[i] = 0 + end + end + toc() + # println(GP_e_vy) + # println(GP_e_psidot) + tic() + (mpcSol.z,mpcSol.u,sol_status)=solveMpcProblem_convhull_dyn_linear(mdl_dyn_lin,mpcSol,mpcParams,modelParams,lapStatus,z_linear,u_linear,z_prev,u_prev,selectedStates,track,GP_e_vy,GP_e_psidot) + toc() + + elseif LMPC_KIN_FLAG + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + z_kin = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + z_to_iden = vcat(z_kin',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i = 1:mpcParams.N + if GP_LOCAL_FLAG + GP_e_vy[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy[i] = GP_full_vy(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot[i] = GP_full_psidot(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy[i] = 0 + GP_e_psidot[i] = 0 + end + end + # SAFESET SELECTION + selectedStates=find_SS(oldSS,selectedStates,z_curr[1],z_prev,lapStatus,modelParams,mpcParams_4s,track) + selectedStates.selStates=s6_to_s4(selectedStates.selStates) + # println(selectedStates) + (mpcSol.z,mpcSol.u,sol_status) = solveMpcProblem_convhull_kin(mdl_kin,mpcSol,z_kin,z_prev,u_prev,selectedStates,track,GP_e_vy,GP_e_psidot) + else + ###################################################################### + ############### CHOICE 3: DO MPC ON KIN_LIN MODEL #################### + ###################################################################### + tic() + z_linear = zeros(mpcParams_4s.N+1,4) + z_linear[1,:] = s6_to_s4(z_curr') + u_linear = vcat(u_prev[2:end,:],u_prev[end,:]) + z_dummy = copy(z_curr) # 6-dimension state is needed for forecasting forward + + if !TI_TV_FLAG + # TV SYS_ID + for i=1:size(z_linear,1)-1 + # LMS SYS ID + if FEATURE_FLAG + (iden_z,iden_u,z_iden_plot)=find_feature_dist(feature_z,feature_u,z_dummy',u_linear[i,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON 2-NORM CRITERION + (iden_z,iden_u,z_iden_plot)=find_SS_dist(solHistory,z_dummy',u_linear[i,:],lapStatus,selectedStates) + else + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON SPATIAL CRITERION + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_dummy',u_linear[i,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff_dummy.c_Vx[1,:],mpcCoeff_dummy.c_Vy[1,:],mpcCoeff_dummy.c_Psi[1,:])=coeff_iden_dist(iden_z,iden_u) + + mpcCoeff.c_Vx[i,:]=mpcCoeff_dummy.c_Vx[1,:] + mpcCoeff.c_Vy[i,:]=mpcCoeff_dummy.c_Vy[1,:] + mpcCoeff.c_Psi[i,:]=mpcCoeff_dummy.c_Psi[1,:] + + z_dummy=car_sim_iden_tv(z_dummy,u_linear[i,:],0.1,mpcCoeff_dummy,modelParams,track) + # GPR DISTURBANCE ID + if GP_LOCAL_FLAG + GP_e_vy = regre(z_dummy,u_linear[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot = regre(z_dummy,u_linear[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy = GP_full_vy(z_dummy,u_linear[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot = GP_full_psidot(z_dummy,u_linear[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy = 0 + GP_e_psidot = 0 + end + + z_dummy[5] += GP_e_vy + z_dummy[6] += GP_e_psidot + z_linear[i+1,:]=s6_to_s4(z_dummy') + end + else + # TI SYS_ID + if FEATURE_FLAG # LMS SYS ID + (iden_z,iden_u,z_iden_plot)=find_feature_dist(feature_z,feature_u,z_dummy',u_linear[1,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON 2-NORM CRITERION + (iden_z,iden_u,z_iden_plot)=find_SS_dist(solHistory,z_dummy',u_linear[1,:],lapStatus,selectedStates) + else + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON SPATIAL CRITERION + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_dummy',u_linear[1,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff_dummy.c_Vx[1,:],mpcCoeff_dummy.c_Vy[1,:],mpcCoeff_dummy.c_Psi[1,:])=coeff_iden_dist(iden_z,iden_u) + + for i = 1:size(mpcCoeff.c_Vx,1) + mpcCoeff.c_Vx[i,:]=mpcCoeff_dummy.c_Vx[1,:] + mpcCoeff.c_Vy[i,:]=mpcCoeff_dummy.c_Vy[1,:] + mpcCoeff.c_Psi[i,:]=mpcCoeff_dummy.c_Psi[1,:] + end + + for i=1:size(z_linear,1)-1 # GPR DISTURBANCE ID + if GP_LOCAL_FLAG + GP_e_vy = regre(z_dummy,u_linear[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot = regre(z_dummy,u_linear[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy = GP_full_vy(z_dummy,u_linear[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot = GP_full_psidot(z_dummy,u_linear[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy = 0 + GP_e_psidot = 0 + end + # THRESHOLDING + GP_e_vy = min(0.025,GP_e_vy) + GP_e_vy = max(-0.025,GP_e_vy) + GP_e_psidot = min(0.1,GP_e_psidot) + GP_e_psidot = max(-0.1,GP_e_psidot) + # println("z_dummy",round(z_dummy,2)) + z_dummy=car_sim_iden_tv(z_dummy,u_linear[i,:],0.1,mpcCoeff_dummy,modelParams,track) + # println("GP_e_vy",GP_e_vy) + z_dummy[5] += GP_e_vy[1] + z_dummy[6] += GP_e_psidot[1] + z_linear[i+1,:] = s6_to_s4(z_dummy') + end + end # end of TI/TV + + # FEATURE POINTS VISUALIZATION + if FEATURE_FLAG + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track_f) + else + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track) + end + mpcSol_to_pub.z_iden_x = z_iden_x + mpcSol_to_pub.z_iden_y = z_iden_y + + # SAFESET SELECTION + selectedStates=find_SS(oldSS,selectedStates,z_curr[1],z_prev,lapStatus,modelParams,mpcParams_4s,track) + selectedStates.selStates=s6_to_s4(selectedStates.selStates) + toc() + + tic() + (mpcSol.z,mpcSol.u,sol_status)=solveMpcProblem_convhull_kin_linear(mdl_kin_lin,mpcSol,mpcParams_4s,modelParams,z_linear,u_linear,z_prev,u_prev,selectedStates,track) + toc() + end # end of IF:IDEN_MODEL/DYN_LIN_MODEL/IDEN_KIN_LIN_MODEL + + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG && lapStatus.currentIt>1 + feature_GP_z[k,:] = solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,1,:] + feature_GP_u[k,:] = u_prev[1,:] + feature_GP_vy_e[k] = z_curr[5]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,5] + feature_GP_psidot_e[k] = z_curr[6]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,6] + k += 1 + end + + # BACK-UP FOR POSSIBLE NON-OPTIMAL SOLUTION + sol_status_dummy = "$sol_status" + if sol_status_dummy[1] != 'O' + mpcSol.u=copy(u_prev) + if LMPC_FLAG || LMPC_DYN_FLAG + (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,6) + else + (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,4) + end + end + end # end of IF:pF/LMPC + # tic() + # DATA WRITING AND COUNTER UPDATE + log_cvx[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vx + log_cvy[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vy + log_cpsi[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Psi + + n_state = size(mpcSol.z,2) + + mpcSol.a_x = mpcSol.u[1+mpcParams.delay_a,1] + mpcSol.d_f = mpcSol.u[1+mpcParams.delay_df,2] + + mpcSol.a_x = mpcSol.u[1,1] + mpcSol.d_f = mpcSol.u[1,2] + if length(mpcSol.df_his)==1 + mpcSol.df_his[1] = mpcSol.u[1+mpcParams.delay_df,2] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.df_his[1:end-1] = mpcSol.df_his[2:end] + mpcSol.df_his[end] = mpcSol.u[1+mpcParams.delay_df,2] + end + + if length(mpcSol.a_his)==1 + mpcSol.a_his[1] = mpcSol.u[1+mpcParams.delay_a,1] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.a_his[1:end-1] = mpcSol.a_his[2:end] + mpcSol.a_his[end] = mpcSol.u[1+mpcParams.delay_a,1] + end + + if PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) && counter == 10 + # println("saving history data") + + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,:,1:n_state]=mpcSol.z + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,4:6]=z_curr[4:6] # [z_est[1],z_est[2],z_est[3]] # THIS LINE IS REALLY IMPORTANT FOR SYS_ID FROM pF + # solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,4:6]=[z_true[3],z_true[4],z_true[6]] # THIS LINE IS REALLY IMPORTANT FOR SYS_ID FROM pF + solHistory.u[lapStatus.currentIt,lapStatus.currentLap,:,:]=mpcSol.u + + # SAFESET DATA SAVING BASED ON CONTROLLER'S FREQUENCY + oldSS.oldSS[lapStatus.currentIt,:,lapStatus.currentLap]=z_curr # [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + # oldSS_true.oldSS[lapStatus.currentIt,:,lapStatus.currentLap]=xyFrame_to_trackFrame(z_true,track) + oldSS.cost2target[lapStatus.currentIt,lapStatus.currentLap]=lapStatus.currentIt + counter = 0 + end + + statusHistory[lapStatus.currentIt,lapStatus.currentLap] = "$sol_status" + if !LMPC_DYN_FLAG && !LMPC_KIN_FLAG && !PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl) + selectFeatureHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = z_iden_plot + end + if (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + if !LMPC_FLAG && !LMPC_DYN_FLAG + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,1:4] = selectedStates.selStates + else + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = selectedStates.selStates + end + end + if !GP_LOCAL_FLAG || !GP_FULL_FLAG + GP_vy_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_vy + GP_psidot_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_psidot + end + + # VISUALIZATION COORDINATE CALCULATION FOR view_trajectory.jl NODE + (z_x,z_y) = trackFrame_to_xyFrame(mpcSol.z,track) + mpcSol_to_pub.z_x = z_x + mpcSol_to_pub.z_y = z_y + # println(selectedStates.selStates) + (SS_x,SS_y) = trackFrame_to_xyFrame(selectedStates.selStates,track) + mpcSol_to_pub.SS_x = SS_x + mpcSol_to_pub.SS_y = SS_y + mpcSol_to_pub.z_vx = mpcSol.z[:,4] + mpcSol_to_pub.SS_vx = selectedStates.selStates[:,4] + mpcSol_to_pub.z_s = mpcSol.z[:,1] + mpcSol_to_pub.SS_s = selectedStates.selStates[:,1] + # FORECASTING POINTS FROM THE DYNAMIC MODEL + + # if length(z_curr)==6 + # (z_fore,~,~) = car_pre_dyn_true(z_curr,mpcSol.u,track,modelParams,6) + # (z_fore_x,z_fore_y) = trackFrame_to_xyFrame(z_fore,track) + # mpcSol_to_pub.z_fore_x = z_fore_x + # mpcSol_to_pub.z_fore_y = z_fore_y + # end + + cmd.servo = convert(Float32,mpcSol.d_f) + cmd.motor = convert(Float32,mpcSol.a_x) + publish(pub, cmd) + publish(mpcSol_pub, mpcSol_to_pub) + # cmd.servo = convert(Float32,-0.2) + # cmd.motor = convert(Float32,0.0) + + + z_prev = copy(mpcSol.z) + u_prev = copy(mpcSol.u) + # toc() + println("$sol_status Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, " v: $(z_est[1])") + lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + counter += 1 + rossleep(loop_rate) + end # END OF THE WHILE LOOP + # THIS IS FOR THE LAST NO FINISHED LAP + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "oldTraj",oldTraj,"selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u, + "feature_GP_vy_e",feature_GP_vy_e,"track",track, + "feature_GP_psidot_e",feature_GP_psidot_e) + end + println("Exiting LMPC node. Saved data to $log_path.") +end + +if ! isinteractive() + main() +end + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s diff --git a/workspace/src/barc/src/LMPC_node_100.jl b/workspace/src/barc/src/LMPC_node_100.jl new file mode 100755 index 00000000..c168bed1 --- /dev/null +++ b/workspace/src/barc/src/LMPC_node_100.jl @@ -0,0 +1,512 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, mpc_solution +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + +function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,z_est::Array{Float64,1},x_est::Array{Float64,1}) + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + # z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end + + # save current state in oldTraj + oldTraj.oldTraj[oldTraj.count[lapStatus.currentLap],:,lapStatus.currentLap] = z_est + oldTraj.count[lapStatus.currentLap] += 1 +end + +function ST_callback(msg::pos_info,z_true::Array{Float64,1}) + z_true[:] = [msg.x,msg.y,msg.v_x,msg.v_y,msg.psi,msg.psiDot] + # println("z_true from LMPC node",round(z_true,4)) +end + +# This is the main function, it is called when the node is started. +function main() + println("Starting LMPC node.") + const BUFFERSIZE = 500 + const LMPC_LAP = 30 + + const PF_FLAG = false # true:only pF, false:1 warm-up lap and LMPC + + const LMPC_FLAG = false # true:IDEN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_DYN_FLAG = false # true:DYN_LIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_KIN_FLAG = true # true:KIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + + const FEATURE_FLAG = false # true:8-shape, false:history (this requires the corresponding change in 3 palces) + const NORM_SPACE_FLAG = true # true:2-norm, false: space critirion + + const TI_TV_FLAG = true # true:TI, false:TV + + const GP_LOCAL_FLAG = false # true:local GPR + const GP_FULL_FLAG = false # true:full GPR + + const GP_HISTORY_FLAG = false # true: GPR data is from last laps, false: GP data is from data base. + + const N = 5 + const delay_df = 1 + const delay_a = 1 + + if PF_FLAG + file_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_name = "SYS_ID_TI" + else + file_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_name = "DYN_LIN" + elseif LMPC_KIN_FLAG + file_name = "KIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_name *= "_TI" + else + file_name *= "_TV" + end + end + end + if GP_LOCAL_FLAG + file_name *= "_LOCAL_GP" + elseif GP_FULL_FLAG + file_name *= "_FULL_GP" + end + + if get_param("sim_flag") + folder_name = "simulations" + else + folder_name = "experiments" + end + + + + PF_FLAG ? println("Path following") : println("Learning MPC") + FEATURE_FLAG ? println("Feature data: 8-shape database") : println("Feature data: History data base") + TI_TV_FLAG ? println("Time invariant SYS_ID") : println("Time variant SYS_ID") + println("N=$N, delay_df=$delay_df, delay_a=$delay_a") + + track_data = createTrack("basic") + track = Track(track_data) + track_fe = createTrack("feature") + track_f = Track(track_fe) + oldTraj = OldTrajectory() + posInfo = PosInfo(); posInfo.s_target=track.s; + lapStatus = LapStatus(1,1,false,false,0.3) + mpcCoeff = MpcCoeff() + mpcCoeff_dummy = MpcCoeff() + mpcSol = MpcSol() + modelParams = ModelParams() + mpcParams_pF = MpcParams() + mpcParams = MpcParams() + mpcParams_4s = MpcParams() + selectedStates = SelectedStates() + oldSS = SafeSetData() + z_est = zeros(7) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_true = zeros(6) # (xDot, yDot, psiDot) + x_est = zeros(4) # (x, y, psi, v) + cmd = ECU() # CONTROL SIGNAL MESSAGE INITIALIZATION + mpcSol_to_pub = mpc_solution() # MPC SOLUTION PUBLISHING MESSAGE INITIALIZATION + InitializeParameters(mpcParams,mpcParams_4s,mpcParams_pF,modelParams,mpcSol, + selectedStates,oldSS,oldTraj,mpcCoeff,mpcCoeff_dummy, + LMPC_LAP,delay_df,delay_a,N,BUFFERSIZE) + z_prev = mpcSol.z + u_prev = mpcSol.u + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + + # NODE INITIALIZATION + init_node("mpc_traj") + # loop_rate = Rate(1/modelParams.dt) + loop_rate = Rate(50.0) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + mpcSol_pub = Publisher("mpc_solution", mpc_solution, queue_size=1)::RobotOS.Publisher{barc.msg.mpc_solution}; acc_f = [0.0] + s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s2 = Subscriber("real_val", pos_info, ST_callback, (z_true,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + + num_lap = LMPC_LAP+1+max(selectedStates.Nl,selectedStates.feature_Nl) + solHistory = SolHistory(BUFFERSIZE,mpcParams.N,6,num_lap) + selectHistory = zeros(BUFFERSIZE,num_lap,selectedStates.Nl*selectedStates.Np,6) + GP_vy_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + GP_psidot_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + statusHistory = Array{ASCIIString}(BUFFERSIZE,num_lap) + + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams) + mdl_kin = MpcModel_convhull_kin(mpcParams_4s,modelParams,selectedStates) + + if !PF_FLAG + # FUNCTION DUMMY CALLS: this is important to call all the functions that will be used before for initial compiling + data = load("$(homedir())/simulations/dummy_function/oldSS.jld") # oldSS.jld is a dummy data for initialization + oldSS_dummy = data["oldSS"] + lapStatus_dummy = LapStatus(1+max(selectedStates.feature_Nl,selectedStates.Nl),1,false,false,0.3) + selectedStates_dummy=find_SS(oldSS_dummy,selectedStates,5.2,z_prev,lapStatus_dummy,modelParams,mpcParams,track) + z = rand(1,6); u = rand(1,2) + selectedStates_dummy.selStates=s6_to_s4(selectedStates_dummy.selStates) + (~,~,~)=solveMpcProblem_convhull_kin(mdl_kin,mpcSol,rand(4),rand(mpcParams.N+1,4),rand(mpcParams.N,2),selectedStates_dummy,track,rand(mpcParams.N),rand(mpcParams.N)) + (~,~,~)=find_SS_dist(solHistory,rand(1,6),rand(1,2),lapStatus,selectedStates) + end + + if GP_LOCAL_FLAG || GP_FULL_FLAG + # FEATURE DATA READING FOR GPR # THIS PART NEEDS TO BE COMMENTED OUT FOR THE FIRST TIME LMPC FOR GP DATA COLLECTING + if PF_FLAG + file_GP_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_GP_name = "SYS_ID_TI" + else + file_GP_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_GP_name = "DYN_LIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_GP_name *= "_TI" + else + file_GP_name *= "_TV" + end + end + end + if get_param("sim_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + else + data = load("$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + end + num_spare = 30 # THE NUMBER OF POINTS SELECTED FOR SPARE GP + feature_GP_z = data["feature_GP_z"] + feature_GP_u = data["feature_GP_u"] + feature_GP_vy_e = data["feature_GP_vy_e"] + feature_GP_psidot_e = data["feature_GP_psidot_e"] + feature_GP_z = feature_GP_z[1:num_spare:end,:] + feature_GP_u = feature_GP_u[1:num_spare:end,:] + feature_GP_vy_e = feature_GP_vy_e[1:num_spare:end] + feature_GP_psidot_e = feature_GP_psidot_e[1:num_spare:end] + + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + + # GP RELATED FUNCTION DUMMY CALLING + # regre(rand(1,6),rand(1,2),feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_full_vy(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + GP_full_psidot(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + else # THIS IS FOR COLLECTING FEATURE DATA FOR GPR + feature_GP_z = zeros(10000,6) + feature_GP_u = zeros(10000,2) + feature_GP_vy_e = zeros(10000) + feature_GP_psidot_e = zeros(10000) + k = 1 + end + + if PF_FLAG + lapStatus.currentLap = 1 + else + lapStatus.currentLap = 1+max(selectedStates.feature_Nl,selectedStates.Nl) + if get_param("sim_flag") + data = load("$(homedir())/simulations/path_following.jld") + else + data = load("$(homedir())/experiments/path_following.jld") + end + oldSS = data["oldSS"] + solHistory = data["solHistory"] + end + println(lapStatus) + println("track maximum curvature: ",track.max_curvature) + + println("Finished LMPC NODE initialization.") + counter = 0 + while ! is_shutdown() + println(z_est[6]) + if z_est[6] >= 0 + tic() + # CONTROL SIGNAL PUBLISHING + cmd.header.stamp = get_rostime() + mpcSol_to_pub.header.stamp = get_rostime() + # publish(pub, cmd) + # publish(mpcSol_pub, mpcSol_to_pub) + + # LAP SWITCHING + if lapStatus.nextLap + println("Finishing one lap at iteration ",lapStatus.currentIt) + if PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + # IN CONSISTANT WITH THE DATA SAVING PART: AVOIDING SAVING THE DATA FOR THE FIRST WARM UP LAP IN LMPC + # SAFE SET COST UPDATE + oldSS.oldCost[lapStatus.currentLap] = lapStatus.currentIt-1 + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + oldSS.cost2target[:,lapStatus.currentLap] = lapStatus.currentIt - oldSS.cost2target[:,lapStatus.currentLap] + end + + lapStatus.nextLap = false + + setvalue(mdl_pF.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_pF.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_kin.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_kin.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + + if z_prev[1,1]>posInfo.s_target + z_prev[:,1] -= posInfo.s_target + end + + lapStatus.currentLap += 1 + lapStatus.currentIt = 1 + + if GP_FULL_FLAG && GP_HISTORY_FLAG # USING DATA FROM PREVIOUS LAPS TO DO FULL GPR + # CONSTRUCT GP_related from solHistory + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + end # THIS PART IS STILL NOT READY + + if lapStatus.currentLap > num_lap # to save the data at the end before error pops up + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "oldTraj",oldTraj,"selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u,"feature_GP_vy_e",feature_GP_vy_e,"feature_GP_psidot_e",feature_GP_psidot_e) + end + end + end + + # MPC CONTROLLER OPTIMIZATION + if lapStatus.currentLap<=1+max(selectedStates.feature_Nl,selectedStates.Nl) # pF CONTROLLER + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + z_kin = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + (mpcSol.z,mpcSol.u,sol_status) = solveMpcProblem_pathFollow(mdl_pF,mpcParams_pF,modelParams,mpcSol,z_kin,z_prev,u_prev,track) + else # LMPC CONTROLLER + # FOR QUICK LMPC STARTING, the next time, change the path following lap number to 1 and change the initial lapStatus to selectedStates. + if PF_FLAG + if get_param("sim_flag") + save("$(homedir())/simulations/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + else + save("$(homedir())/experiments/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + end + end + + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + if LMPC_KIN_FLAG + # tic() + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + z_kin = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + z_to_iden = vcat(z_kin',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i = 1:mpcParams.N + if GP_LOCAL_FLAG + GP_e_vy[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy[i] = GP_full_vy(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot[i] = GP_full_psidot(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy[i] = 0 + GP_e_psidot[i] = 0 + end + end + # SAFESET SELECTION + selectedStates=find_SS(oldSS,selectedStates,z_curr[1],z_prev,lapStatus,modelParams,mpcParams_4s,track) + selectedStates.selStates=s6_to_s4(selectedStates.selStates) + # println(selectedStates) + (mpcSol.z,mpcSol.u,sol_status) = solveMpcProblem_convhull_kin(mdl_kin,mpcSol,z_kin,z_prev,u_prev,selectedStates,track,GP_e_vy,GP_e_psidot) + println(mpcSol.u) + # toc() + + end # end of IF:IDEN_MODEL/DYN_LIN_MODEL/IDEN_KIN_LIN_MODEL + + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG && lapStatus.currentIt>1 + feature_GP_z[k,:] = solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,1,:] + feature_GP_u[k,:] = u_prev[1,:] + feature_GP_vy_e[k] = z_curr[5]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,5] + feature_GP_psidot_e[k] = z_curr[6]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,6] + k += 1 + end + + # BACK-UP FOR POSSIBLE NON-OPTIMAL SOLUTION + # sol_status_dummy = "$sol_status" + # if sol_status_dummy[1] != 'O' + # mpcSol.u=copy(u_prev) + # if LMPC_FLAG || LMPC_DYN_FLAG + # (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,6) + # else + # (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,4) + # end + # end + end # end of IF:pF/LMPC + # tic() + # DATA WRITING AND COUNTER UPDATE + # log_cvx[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vx + # log_cvy[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vy + # log_cpsi[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Psi + + n_state = size(mpcSol.z,2) + + mpcSol.a_x = mpcSol.u[1+mpcParams.delay_a,1] + mpcSol.d_f = mpcSol.u[1+mpcParams.delay_df,2] + + mpcSol.a_x = mpcSol.u[1,1] + mpcSol.d_f = mpcSol.u[1,2] + if length(mpcSol.df_his)==1 + mpcSol.df_his[1] = mpcSol.u[1+mpcParams.delay_df,2] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.df_his[1:end-1] = mpcSol.df_his[2:end] + mpcSol.df_his[end] = mpcSol.u[1+mpcParams.delay_df,2] + end + + if length(mpcSol.a_his)==1 + mpcSol.a_his[1] = mpcSol.u[1+mpcParams.delay_a,1] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.a_his[1:end-1] = mpcSol.a_his[2:end] + mpcSol.a_his[end] = mpcSol.u[1+mpcParams.delay_a,1] + end + + if (PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl))) + # println("saving history data") + if counter >= 10 + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,:,1:n_state]=mpcSol.z + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,4:6]=z_curr[4:6] # [z_est[1],z_est[2],z_est[3]] # THIS LINE IS REALLY IMPORTANT FOR SYS_ID FROM pF + # solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,4:6]=[z_true[3],z_true[4],z_true[6]] # THIS LINE IS REALLY IMPORTANT FOR SYS_ID FROM pF + solHistory.u[lapStatus.currentIt,lapStatus.currentLap,:,:]=mpcSol.u + + # SAFESET DATA SAVING BASED ON CONTROLLER'S FREQUENCY + oldSS.oldSS[lapStatus.currentIt,:,lapStatus.currentLap]=z_curr # [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + # oldSS_true.oldSS[lapStatus.currentIt,:,lapStatus.currentLap]=xyFrame_to_trackFrame(z_true,track) + oldSS.cost2target[lapStatus.currentIt,lapStatus.currentLap]=lapStatus.currentIt + counter = 0 + lapStatus.currentIt += 1 + end + end + + statusHistory[lapStatus.currentIt,lapStatus.currentLap] = "$sol_status" + if !LMPC_DYN_FLAG && !LMPC_KIN_FLAG && !PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl) + selectFeatureHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = z_iden_plot + end + if (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + if !LMPC_FLAG && !LMPC_DYN_FLAG + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,1:4] = selectedStates.selStates + else + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = selectedStates.selStates + end + end + if !GP_LOCAL_FLAG || !GP_FULL_FLAG + GP_vy_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_vy + GP_psidot_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_psidot + end + + # VISUALIZATION COORDINATE CALCULATION FOR view_trajectory.jl NODE + (z_x,z_y) = trackFrame_to_xyFrame(mpcSol.z,track) + mpcSol_to_pub.z_x = z_x + mpcSol_to_pub.z_y = z_y + # println(selectedStates.selStates) + (SS_x,SS_y) = trackFrame_to_xyFrame(selectedStates.selStates,track) + mpcSol_to_pub.SS_x = SS_x + mpcSol_to_pub.SS_y = SS_y + mpcSol_to_pub.z_vx = mpcSol.z[:,4] + mpcSol_to_pub.SS_vx = selectedStates.selStates[:,4] + mpcSol_to_pub.z_s = mpcSol.z[:,1] + mpcSol_to_pub.SS_s = selectedStates.selStates[:,1] + # FORECASTING POINTS FROM THE DYNAMIC MODEL + + # if length(z_curr)==6 + # (z_fore,~,~) = car_pre_dyn_true(z_curr,mpcSol.u,track,modelParams,6) + # (z_fore_x,z_fore_y) = trackFrame_to_xyFrame(z_fore,track) + # mpcSol_to_pub.z_fore_x = z_fore_x + # mpcSol_to_pub.z_fore_y = z_fore_y + # end + + cmd.servo = convert(Float32,mpcSol.d_f) + cmd.motor = convert(Float32,mpcSol.a_x) + publish(pub, cmd) + publish(mpcSol_pub, mpcSol_to_pub) + + z_prev = copy(mpcSol.z) + u_prev = copy(mpcSol.u) + toc() + println("$sol_status Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, " v: $(z_est[1])") + # lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + counter += 1 + println(counter) + # rossleep(loop_rate) + end # END OF THE WHILE LOOP + # THIS IS FOR THE LAST NO FINISHED LAP + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "oldTraj",oldTraj,"selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u, + "feature_GP_vy_e",feature_GP_vy_e,"track",track, + "feature_GP_psidot_e",feature_GP_psidot_e) + end + println("Exiting LMPC node. Saved data to $log_path.") +end + +if ! isinteractive() + main() +end + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s diff --git a/workspace/src/barc/src/LMPC_node_feature_data_collecting.jl b/workspace/src/barc/src/LMPC_node_feature_data_collecting.jl new file mode 100755 index 00000000..ac391f9f --- /dev/null +++ b/workspace/src/barc/src/LMPC_node_feature_data_collecting.jl @@ -0,0 +1,226 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, mpc_solution +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + +# This function is called whenever a new state estimate is received. +# It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) +function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,oldTraj::OldTrajectory,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data + # update mpc initial condition + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + + # check if lap needs to be switched + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.currentLap += 1 + lapStatus.currentIt = 1 + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end +end + +# function ST_callback(msg::pos_info,z_true::Array{Float64,1}) +# z_true[:] = [msg.x,msg.y,msg.v_x,msg.v_y,msg.psi,msg.psiDot] +# # println("z_true from LMPC node",round(z_true,4)) +# end + +# This is the main function, it is called when the node is started. +function main() + println("Starting LMPC_Feature_Collecting node.") + const BUFFERSIZE = 500 + const LMPC_LAP = 10 + const PF_FLAG = true # true:only pF, false:1 warm-up lap and LMPC + const LMPC_FLAG = true # true:IDEN_MODEL, false:IDEN_KIN_LIN_MODEL + const FEATURE_FLAG = false # true:8-shape, false:history (this requires the corresponding change in 3 palces) + const TI_TV_FLAG = true # true:TI, false:TV + const GP_LOCAL_FLAG = false # true:local GPR + const GP_FULL_FLAG = false # true:full GPR + const N = 10 + const delay_df = 3 + const delay_a = 1 + println("N=$N, delay_df=$delay_df, delay_a=$delay_a") + track_data = createTrack("feature") + track = Track(track_data) + oldTraj = OldTrajectory() + posInfo = PosInfo(); posInfo.s_target=track.s; + lapStatus = LapStatus(1,1,false,false,0.3) + mpcCoeff = MpcCoeff() + mpcCoeff_dummy = MpcCoeff() + mpcSol = MpcSol() + modelParams = ModelParams() + mpcParams_pF = MpcParams() + mpcParams = MpcParams() + mpcParams_4s = MpcParams() + selectedStates = SelectedStates() + oldSS = SafeSetData() + z_est = zeros(7) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_true = zeros(6) # (xDot, yDot, psiDot) + x_est = zeros(4) # (x, y, psi, v) + cmd = ECU() # CONTROL SIGNAL MESSAGE INITIALIZATION + mpcSol_to_pub = mpc_solution() # MPC SOLUTION PUBLISHING MESSAGE INITIALIZATION + InitializeParameters(mpcParams,mpcParams_4s,mpcParams_pF,modelParams,mpcSol, + selectedStates,oldSS,oldTraj,mpcCoeff,mpcCoeff_dummy, + LMPC_LAP,delay_df,delay_a,N,BUFFERSIZE) + println("track maximum curvature is: ",round(track.max_curvature,3)) + + # FEATURE DATA INITIALIZATION + v_ref = vcat([0.8],0.8:0.1:2.0) + + feature_z = zeros(100000,6,2) + feature_u = zeros(100000,2) + + # DATA LOGGING VARIABLE INITIALIZATION + k = 1 # counter initialization, k is the counter from the beginning of the experiment + + # MODEL INITIALIZATION + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams) + + # NODE INITIALIZATION + init_node("mpc_traj") + loop_rate = Rate(1/modelParams.dt) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + mpcSol_pub = Publisher("mpc_solution", mpc_solution, queue_size=1)::RobotOS.Publisher{barc.msg.mpc_solution} + # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: + acc_f = [0.0] + s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,oldTraj,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} + # s2 = Subscriber("real_val", pos_info, ST_callback, (z_true,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! + run_id = get_param("run_id") + println("Finished initialization.") + + mpcSol.df_his = zeros(mpcParams.delay_df) + mpcSol.z = zeros(mpcParams_pF.N+1,4) + mpcSol.u = zeros(mpcParams_pF.N,2) + for i in 2:mpcParams_pF.N+1 + mpcSol.z[i,:]=car_sim_kin(mpcSol.z[i-1,:],mpcSol.u[i-1,:],track,modelParams) + end + mpcSol.a_x = 0 + mpcSol.d_f = 0 + z_prev = mpcSol.z + u_prev = mpcSol.u + + while ! is_shutdown() + if z_est[6] > 0 + # CONTROL SIGNAL PUBLISHING + cmd.header.stamp = get_rostime() + mpcSol_to_pub.header.stamp = get_rostime() + publish(pub, cmd) + publish(mpcSol_pub, mpcSol_to_pub) + + # TRACK FEATURE DATA COLLECTING: it is important to put this at the beginning of the iteration to make the data consistant + if lapStatus.currentLap>1 + k = k + 1 # start counting from the second lap. + feature_z[k,:,1] = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + feature_z[k-1,:,2] = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + feature_u[k,1] = u_sol[2,1] # acceleration delay is from MPC itself + feature_u[k+mpcParams.delay_df-1,2] = u_sol[1+mpcParams.delay_df,2] # another 2 steps delay is from the system + end + + + # LAP SWITCHING + if lapStatus.nextLap + println("Finishing one lap at iteration ",lapStatus.currentIt) + lapStatus.nextLap = false + if mpcSol.z[1,1]>posInfo.s_target + # WARM START SWITCHING + setvalue(mdl_pF.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_pF.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + end + end + + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + # z_curr = xyFrame_to_trackFrame(z_true,track) + # z_curr = [z_curr[1],z_curr[2],z_curr[3],sqrt(z_curr[4]^2+z_curr[5]^2)] + + (z_sol,u_sol,sol_status)=solveMpcProblem_featureData(mdl_pF,mpcParams_pF,modelParams,mpcSol,z_curr,z_prev,u_prev,track,v_ref[lapStatus.currentLap]) + mpcSol.z = z_sol + mpcSol.u = u_sol + + # OPTIMIZATION + println("$sol_status, Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, ", v:", round(z_curr[4],2)) + # println("z_curr", round(z_curr,3)) + + mpcSol.a_x = u_sol[1+mpcParams_pF.delay_a,1] + mpcSol.d_f = u_sol[1+mpcParams_pF.delay_df,2] + if length(mpcSol.df_his)==1 + mpcSol.df_his[1] = u_sol[1+mpcParams_pF.delay_df,2] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.df_his[1:end-1] = mpcSol.df_his[2:end] + mpcSol.df_his[end] = u_sol[1+mpcParams_pF.delay_df,2] + end + + z_prev = z_sol + u_prev = u_sol + + cmd.motor = convert(Float32,mpcSol.a_x) + cmd.servo = convert(Float32,mpcSol.d_f) + + # VISUALIZATION COORDINATE CALCULATION FOR view_trajectory.jl NODE + (z_x,z_y) = trackFrame_to_xyFrame(z_sol,track) + mpcSol_to_pub.z_x = z_x + mpcSol_to_pub.z_y = z_y + + # TRACK FEATURE DATA COLLECTING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if lapStatus.currentLap==length(v_ref) && z_est[6] > track.s[end]-0.5 + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_z = feature_z[2+mpcParams.delay_df-1:k-1,:,:] + feature_u = feature_u[2+mpcParams.delay_df-1:k-1,:] + # DATA SAVING + save(log_path,"feature_z",feature_z,"feature_u",feature_u) + end + + lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + rossleep(loop_rate) + end # END OF THE WHILE LOOP + # DATA SAVING, trying to save as much as feature data as possible to do sys_id + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_z = feature_z[2+mpcParams.delay_df-1:k-1,:,:] + feature_u = feature_u[2+mpcParams.delay_df-1:k-1,:] + # DATA SAVING + save(log_path,"feature_z",feature_z,"feature_u",feature_u) +end + + +if ! isinteractive() + main() +end + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s diff --git a/workspace/src/barc/src/LMPC_node_feature_data_collecting_new_ID.jl b/workspace/src/barc/src/LMPC_node_feature_data_collecting_new_ID.jl new file mode 100755 index 00000000..8246215a --- /dev/null +++ b/workspace/src/barc/src/LMPC_node_feature_data_collecting_new_ID.jl @@ -0,0 +1,228 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, mpc_solution +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + +# This function is called whenever a new state estimate is received. +# It saves this estimate in oldTraj and uses it in the MPC formulation (see in main) +function SE_callback(msg::pos_info,acc_f::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,a_est,z_est::Array{Float64,1},x_est::Array{Float64,1}) # update current position and track data + # update mpc initial condition + z_est[:] = [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s,acc_f[1]] # the last variable is filtered acceleration + a_est[:] = [msg.a_x,msg.a_y] + x_est[:] = [msg.x,msg.y,msg.psi,msg.v] + + # check if lap needs to be switched + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.currentLap += 1 + lapStatus.currentIt = 1 + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end +end + +# function ST_callback(msg::pos_info,z_true::Array{Float64,1}) +# z_true[:] = [msg.x,msg.y,msg.v_x,msg.v_y,msg.psi,msg.psiDot] +# # println("z_true from LMPC node",round(z_true,4)) +# end + +# This is the main function, it is called when the node is started. +function main() + println("Starting LMPC_Feature_Collecting node.") + const BUFFERSIZE = 500 + const LMPC_LAP = 10 + const PF_FLAG = true # true:only pF, false:1 warm-up lap and LMPC + const LMPC_FLAG = true # true:IDEN_MODEL, false:IDEN_KIN_LIN_MODEL + const FEATURE_FLAG = false # true:8-shape, false:history (this requires the corresponding change in 3 palces) + const TI_TV_FLAG = true # true:TI, false:TV + const GP_LOCAL_FLAG = false # true:local GPR + const GP_FULL_FLAG = false # true:full GPR + const N = 10 + const delay_df = 3 + const delay_a = 1 + println("N=$N, delay_df=$delay_df, delay_a=$delay_a") + track_data = createTrack("feature") + track = Track(track_data) + oldTraj = OldTrajectory() + posInfo = PosInfo(); posInfo.s_target=track.s; + lapStatus = LapStatus(1,1,false,false,0.3) + mpcCoeff = MpcCoeff() + mpcCoeff_dummy = MpcCoeff() + mpcSol = MpcSol() + modelParams = ModelParams() + mpcParams_pF = MpcParams() + mpcParams = MpcParams() + mpcParams_4s = MpcParams() + selectedStates = SelectedStates() + oldSS = SafeSetData() + z_est = zeros(7) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + a_est = zeros(2) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_true = zeros(6) # (xDot, yDot, psiDot) + x_est = zeros(4) # (x, y, psi, v) + cmd = ECU() # CONTROL SIGNAL MESSAGE INITIALIZATION + mpcSol_to_pub = mpc_solution() # MPC SOLUTION PUBLISHING MESSAGE INITIALIZATION + InitializeParameters(mpcParams,mpcParams_4s,mpcParams_pF,modelParams,mpcSol, + selectedStates,oldSS,oldTraj,mpcCoeff,mpcCoeff_dummy, + LMPC_LAP,delay_df,delay_a,N,BUFFERSIZE) + println("track maximum curvature is: ",round(track.max_curvature,3)) + + # FEATURE DATA INITIALIZATION + v_ref = vcat([0.8],0.8:0.1:2.0) + + feature_z = zeros(100000,8,2) + feature_u = zeros(100000,2) + + # DATA LOGGING VARIABLE INITIALIZATION + k = 1 # counter initialization, k is the counter from the beginning of the experiment + + # MODEL INITIALIZATION + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams) + + # NODE INITIALIZATION + init_node("mpc_traj") + loop_rate = Rate(1/modelParams.dt) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + mpcSol_pub = Publisher("mpc_solution", mpc_solution, queue_size=1)::RobotOS.Publisher{barc.msg.mpc_solution} + # The subscriber passes arguments (coeffCurvature and z_est) which are updated by the callback function: + acc_f = [0.0] + s1 = Subscriber("pos_info", pos_info, SE_callback, (acc_f,lapStatus,posInfo,mpcSol,a_est,z_est,x_est),queue_size=50)::RobotOS.Subscriber{barc.msg.pos_info} + # s2 = Subscriber("real_val", pos_info, ST_callback, (z_true,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + # Note: Choose queue size long enough so that no pos_info packets get lost! They are important for system ID! + run_id = get_param("run_id") + println("Finished initialization.") + + mpcSol.df_his = zeros(mpcParams.delay_df) + mpcSol.z = zeros(mpcParams_pF.N+1,4) + mpcSol.u = zeros(mpcParams_pF.N,2) + for i in 2:mpcParams_pF.N+1 + mpcSol.z[i,:]=car_sim_kin(mpcSol.z[i-1,:],mpcSol.u[i-1,:],track,modelParams) + end + mpcSol.a_x = 0 + mpcSol.d_f = 0 + z_prev = mpcSol.z + u_prev = mpcSol.u + + while ! is_shutdown() + if z_est[6] > 0 + # CONTROL SIGNAL PUBLISHING + cmd.header.stamp = get_rostime() + mpcSol_to_pub.header.stamp = get_rostime() + publish(pub, cmd) + publish(mpcSol_pub, mpcSol_to_pub) + + # TRACK FEATURE DATA COLLECTING: it is important to put this at the beginning of the iteration to make the data consistant + if lapStatus.currentLap>1 + k = k + 1 # start counting from the second lap. + feature_z[k,:,1] = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3],a_est[1],a_est[2]] + feature_z[k-1,:,2] = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3],a_est[1],a_est[2]] + feature_u[k,1] = u_sol[2,1] # acceleration delay is from MPC itself + feature_u[k+mpcParams.delay_df-1,2] = u_sol[1+mpcParams.delay_df,2] # another 2 steps delay is from the system + end + + + # LAP SWITCHING + if lapStatus.nextLap + println("Finishing one lap at iteration ",lapStatus.currentIt) + lapStatus.nextLap = false + if mpcSol.z[1,1]>posInfo.s_target + # WARM START SWITCHING + setvalue(mdl_pF.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_pF.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + end + end + + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + # z_curr = xyFrame_to_trackFrame(z_true,track) + # z_curr = [z_curr[1],z_curr[2],z_curr[3],sqrt(z_curr[4]^2+z_curr[5]^2)] + + (z_sol,u_sol,sol_status)=solveMpcProblem_featureData(mdl_pF,mpcParams_pF,modelParams,mpcSol,z_curr,z_prev,u_prev,track,v_ref[lapStatus.currentLap]) + mpcSol.z = z_sol + mpcSol.u = u_sol + + # OPTIMIZATION + println("$sol_status, Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, ", v:", round(z_curr[4],2)) + # println("z_curr", round(z_curr,3)) + + mpcSol.a_x = u_sol[1+mpcParams_pF.delay_a,1] + mpcSol.d_f = u_sol[1+mpcParams_pF.delay_df,2] + if length(mpcSol.df_his)==1 + mpcSol.df_his[1] = u_sol[1+mpcParams_pF.delay_df,2] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.df_his[1:end-1] = mpcSol.df_his[2:end] + mpcSol.df_his[end] = u_sol[1+mpcParams_pF.delay_df,2] + end + + z_prev = z_sol + u_prev = u_sol + + cmd.motor = convert(Float32,mpcSol.a_x) + cmd.servo = convert(Float32,mpcSol.d_f) + + # VISUALIZATION COORDINATE CALCULATION FOR view_trajectory.jl NODE + (z_x,z_y) = trackFrame_to_xyFrame(z_sol,track) + mpcSol_to_pub.z_x = z_x + mpcSol_to_pub.z_y = z_y + + # TRACK FEATURE DATA COLLECTING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if lapStatus.currentLap==length(v_ref) && z_est[6] > track.s[end]-0.5 + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_z = feature_z[2+mpcParams.delay_df-1:k-1,:,:] + feature_u = feature_u[2+mpcParams.delay_df-1:k-1,:] + # DATA SAVING + save(log_path,"feature_z",feature_z,"feature_u",feature_u) + end + + lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + rossleep(loop_rate) + end # END OF THE WHILE LOOP + # DATA SAVING, trying to save as much as feature data as possible to do sys_id + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_z = feature_z[2+mpcParams.delay_df-1:k-1,:,:] + feature_u = feature_u[2+mpcParams.delay_df-1:k-1,:] + # DATA SAVING + save(log_path,"feature_z",feature_z,"feature_u",feature_u) +end + + +if ! isinteractive() + main() +end + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s diff --git a/workspace/src/barc/src/LMPC_node_new_ID.jl b/workspace/src/barc/src/LMPC_node_new_ID.jl new file mode 100755 index 00000000..cdf09c00 --- /dev/null +++ b/workspace/src/barc/src/LMPC_node_new_ID.jl @@ -0,0 +1,592 @@ +#!/usr/bin/env julia + +using RobotOS +@rosimport barc.msg: ECU, pos_info, mpc_solution +@rosimport geometry_msgs.msg: Vector3 +rostypegen() +using barc.msg +using geometry_msgs.msg +using JuMP +using Ipopt +using JLD + +# log msg +include("barc_lib/classes.jl") +include("barc_lib/LMPC/MPC_models.jl") +include("barc_lib/LMPC/functions.jl") +include("barc_lib/LMPC/solveMpcProblem.jl") +include("barc_lib/simModel.jl") + +function SE_callback(msg::pos_info,lapStatus::LapStatus,posInfo::PosInfo,mpcSol::MpcSol,a_est::Array{Float64,1},z_est::Array{Float64,1},x_est::Array{Float64,1}) + z_est[:]= [msg.v_x,msg.v_y,msg.psiDot,msg.epsi,msg.ey,msg.s] # the last variable is filtered acceleration + a_est[:]= [msg.a_x,msg.a_y] + x_est[:]= [msg.x,msg.y,msg.psi,msg.v] + + if z_est[6] <= lapStatus.s_lapTrigger && lapStatus.switchLap + lapStatus.nextLap = true + lapStatus.switchLap = false + elseif z_est[6] > lapStatus.s_lapTrigger + lapStatus.switchLap = true + end +end + +function ST_callback(msg::pos_info,z_true::Array{Float64,1}) + z_true[:] = [msg.x,msg.y,msg.v_x,msg.v_y,msg.psi,msg.psiDot] +end + +# This is the main function, it is called when the node is started. +function main() + println("Starting LMPC node.") + const BUFFERSIZE = 500 + const LMPC_LAP = 30 + + const PF_FLAG = false # true:only pF, false:1 warm-up lap and LMPC + + const LMPC_FLAG = true # true:IDEN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_DYN_FLAG = false # true:DYN_LIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + const LMPC_KIN_FLAG = false # true:KIN_MODEL, false:IDEN_KIN_LIN_MODEL(if all flags are false) + + const FEATURE_FLAG = true # true:8-shape, false:history (this requires the corresponding change in 3 palces) + const NORM_SPACE_FLAG = true # true:2-norm, false: space critirion + + const TI_TV_FLAG = false # true:TI, false:TV + + const GP_LOCAL_FLAG = false # true:local GPR + const GP_FULL_FLAG = false # true:full GPR + + const GP_HISTORY_FLAG = false # true: GPR data is from last laps, false: GP data is from data base. + + const N = 10 + const delay_df = 3 + const delay_a = 1 + + if PF_FLAG + file_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_name = "SYS_ID_TI" + else + file_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_name = "DYN_LIN" + elseif LMPC_KIN_FLAG + file_name = "KIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_name *= "_TI" + else + file_name *= "_TV" + end + end + end + if GP_LOCAL_FLAG + file_name *= "_LOCAL_GP" + elseif GP_FULL_FLAG + file_name *= "_FULL_GP" + end + + if get_param("sim_flag") + folder_name = "simulations" + else + folder_name = "experiments" + end + + PF_FLAG ? println("Path following") : println("Learning MPC") + FEATURE_FLAG ? println("Feature data: 8-shape database") : println("Feature data: History data base") + TI_TV_FLAG ? println("Time invariant SYS_ID") : println("Time variant SYS_ID") + println("N=$N, delay_df=$delay_df, delay_a=$delay_a") + + track_data = createTrack("basic") + track = Track(track_data) + track_fe = createTrack("feature") + track_f = Track(track_fe) + oldTraj = OldTrajectory() + posInfo = PosInfo(); posInfo.s_target=track.s; + lapStatus = LapStatus(1,1,false,false,0.3) + mpcCoeff = MpcCoeff() + mpcCoeff_dummy = MpcCoeff() + mpcSol = MpcSol() + modelParams = ModelParams() + mpcParams_pF = MpcParams() + mpcParams = MpcParams() + mpcParams_4s = MpcParams() + selectedStates = SelectedStates() + oldSS = SafeSetData() + # oldSS_true = SafeSetData() + # oldSS_true.oldSS = NaN*ones(BUFFERSIZE,6,10) # contains data from previous laps usefull to build the safe set + z_est = zeros(6) # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + a_est = zeros(2) + z_true = zeros(6) # (xDot, yDot, psiDot) + x_est = zeros(4) # (x, y, psi, v) + cmd = ECU() # CONTROL SIGNAL MESSAGE INITIALIZATION + mpcSol_to_pub = mpc_solution() # MPC SOLUTION PUBLISHING MESSAGE INITIALIZATION + InitializeParameters(mpcParams,mpcParams_4s,mpcParams_pF,modelParams,mpcSol, + selectedStates,oldSS,oldTraj,mpcCoeff,mpcCoeff_dummy, + LMPC_LAP,delay_df,delay_a,N,BUFFERSIZE) + z_prev = mpcSol.z + u_prev = mpcSol.u + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + d_f_lp = 0.0 + + # NODE INITIALIZATION + init_node("mpc_traj") + loop_rate = Rate(1/modelParams.dt) + pub = Publisher("ecu", ECU, queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + mpcSol_pub = Publisher("mpc_solution", mpc_solution, queue_size=1)::RobotOS.Publisher{barc.msg.mpc_solution} + s1 = Subscriber("pos_info", pos_info, SE_callback, (lapStatus,posInfo,mpcSol,a_est,z_est,x_est),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + s2 = Subscriber("real_val", pos_info, ST_callback, (z_true,),queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + + num_lap = LMPC_LAP+1+max(selectedStates.Nl,selectedStates.feature_Nl) + log_cvx = zeros(BUFFERSIZE,mpcParams.N,3,num_lap) + log_cvy = zeros(BUFFERSIZE,mpcParams.N,1,num_lap) + log_cpsi = zeros(BUFFERSIZE,mpcParams.N,1,num_lap) + solHistory = SolHistory(BUFFERSIZE,mpcParams.N,8,num_lap) + selectHistory = zeros(BUFFERSIZE,num_lap,selectedStates.Nl*selectedStates.Np,6) + GP_vy_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + GP_psidot_History = zeros(BUFFERSIZE,num_lap,mpcParams.N) + selectFeatureHistory= zeros(BUFFERSIZE,num_lap,selectedStates.feature_Np,6) + statusHistory = Array{ASCIIString}(BUFFERSIZE,num_lap) + + mdl_pF = MpcModel_pF(mpcParams_pF,modelParams) + mdl_convhull_simple = MpcModel_convhull_dyn_iden_simple(mpcParams,modelParams,selectedStates) + + if !PF_FLAG + # FUNCTION DUMMY CALLS: this is important to call all the functions that will be used before for initial compiling + data = load("$(homedir())/simulations/dummy_function/oldSS.jld") # oldSS.jld is a dummy data for initialization + oldSS_dummy = data["oldSS"] + lapStatus_dummy = LapStatus(1+max(selectedStates.feature_Nl,selectedStates.Nl),1,false,false,0.3) + selectedStates_dummy=find_SS(oldSS_dummy,selectedStates,5.2,z_prev,lapStatus_dummy,modelParams,mpcParams,track) + z = rand(1,6); u = rand(1,2) + data = load("$(homedir())/simulations/dummy_function/FeatureDataCollecting.jld") + feature_z = data["feature_z"] + feature_u = data["feature_u"] + (iden_z,iden_u)=find_feature_dist(feature_z,feature_u,z,u,selectedStates) + data = load("$(homedir())/simulations/dummy_function/path_following.jld") # oldSS.jld is a dummy data for initialization + solHistory_dummy = data["solHistory"] + # (~,~,~)=find_feature_space(solHistory_dummy,2*ones(1,6),rand(1,2),lapStatus_dummy,selectedStates,posInfo) + (c_Vx,c_Vy,c_Psi)=coeff_iden_dist(iden_z,iden_u) + (~,~,~)=solveMpcProblem_convhull_dyn_iden_simple(mdl_convhull_simple,mpcSol,mpcCoeff,rand(6),rand(mpcParams.N+1,6),rand(mpcParams.N,2),selectedStates_dummy,track,rand(mpcParams.N),rand(mpcParams.N)) + (~,~,~)=car_pre_dyn(rand(1,6)+1,rand(mpcParams.N,2),track,modelParams,6) + (~,~,~)=find_SS_dist_simple(solHistory,rand(1,6),rand(1,2),lapStatus,selectedStates) + end + + if FEATURE_FLAG + # FEATURE DATA READING + if get_param("feature_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld") + else + if get_param("sim_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureDataCollecting.jld") + else + data = load("$(homedir())/experiments/Feature_Data/FeatureDataCollecting.jld") + end + end + feature_z = data["feature_z"] + feature_u = data["feature_u"] + println("Number of total feature points from 8-shape:",size(feature_z,1)) + end + + if GP_LOCAL_FLAG || GP_FULL_FLAG + # FEATURE DATA READING FOR GPR # THIS PART NEEDS TO BE COMMENTED OUT FOR THE FIRST TIME LMPC FOR GP DATA COLLECTING + if PF_FLAG + file_GP_name = "PF" + else + if LMPC_FLAG # CONTROLLER FLAG + if TI_TV_FLAG + file_GP_name = "SYS_ID_TI" + else + file_GP_name = "SYS_ID_TV" + end + elseif LMPC_DYN_FLAG + file_GP_name = "DYN_LIN" + else + file_name = "SYS_ID_KIN_LIN" + if TI_TV_FLAG + file_GP_name *= "_TI" + else + file_GP_name *= "_TV" + end + end + end + if get_param("sim_flag") + data = load("$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + else + data = load("$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_GP_name).jld") + end + num_spare = 30 # THE NUMBER OF POINTS SELECTED FOR SPARE GP + feature_GP_z = data["feature_GP_z"] + feature_GP_u = data["feature_GP_u"] + feature_GP_vy_e = data["feature_GP_vy_e"] + feature_GP_psidot_e = data["feature_GP_psidot_e"] + feature_GP_z = feature_GP_z[1:num_spare:end,:] + feature_GP_u = feature_GP_u[1:num_spare:end,:] + feature_GP_vy_e = feature_GP_vy_e[1:num_spare:end] + feature_GP_psidot_e = feature_GP_psidot_e[1:num_spare:end] + + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + + # GP RELATED FUNCTION DUMMY CALLING + # regre(rand(1,6),rand(1,2),feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_full_vy(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + GP_full_psidot(rand(1,6),rand(1,2),GP_feature,GP_e_vy_prepare) + else # THIS IS FOR COLLECTING FEATURE DATA FOR GPR + feature_GP_z = zeros(10000,6) + feature_GP_u = zeros(10000,2) + feature_GP_vy_e = zeros(10000) + feature_GP_psidot_e = zeros(10000) + k = 1 + end + + if PF_FLAG + lapStatus.currentLap = 1 + else + lapStatus.currentLap = 1+max(selectedStates.feature_Nl,selectedStates.Nl) + if get_param("sim_flag") + data = load("$(homedir())/simulations/path_following.jld") + else + data = load("$(homedir())/experiments/path_following.jld") + end + oldSS = data["oldSS"] + solHistory = data["solHistory"] + end + println(lapStatus) + println("track maximum curvature: ",track.max_curvature) + + println("Finished LMPC NODE initialization.") + + while ! is_shutdown() + if z_est[6] > 0 + + # CONTROL SIGNAL PUBLISHING + cmd.header.stamp = get_rostime() + mpcSol_to_pub.header.stamp = get_rostime() + publish(pub, cmd) + publish(mpcSol_pub, mpcSol_to_pub) + + # LAP SWITCHING + if lapStatus.nextLap + println("Finishing one lap at iteration ",lapStatus.currentIt) + if PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + # IN CONSISTANT WITH THE DATA SAVING PART: AVOIDING SAVING THE DATA FOR THE FIRST WARM UP LAP IN LMPC + # SAFE SET COST UPDATE + oldSS.oldCost[lapStatus.currentLap] = lapStatus.currentIt-1 + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + oldSS.cost2target[:,lapStatus.currentLap] = lapStatus.currentIt - oldSS.cost2target[:,lapStatus.currentLap] + end + + lapStatus.nextLap = false + + setvalue(mdl_pF.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_pF.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull_simple.z_Ol[1:mpcParams.N,1],mpcSol.z[2:mpcParams.N+1,1]-posInfo.s_target) + setvalue(mdl_convhull_simple.z_Ol[mpcParams.N+1,1],mpcSol.z[mpcParams.N+1,1]-posInfo.s_target) + + if z_prev[1,1]>posInfo.s_target + z_prev[:,1] -= posInfo.s_target + end + + lapStatus.currentLap += 1 + lapStatus.currentIt = 1 + + if GP_FULL_FLAG && GP_HISTORY_FLAG # USING DATA FROM PREVIOUS LAPS TO DO FULL GPR + # CONSTRUCT GP_related from solHistory + GP_e_vy_prepare = GP_prepare(feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psi_dot_prepare = GP_prepare(feature_GP_psidot_e,feature_GP_z,feature_GP_u) + GP_feature = hcat(feature_GP_z,feature_GP_u) + end # THIS PART IS STILL NOT READY + + if lapStatus.currentLap > num_lap # to save the data at the end before error pops up + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u,"feature_GP_vy_e",feature_GP_vy_e,"feature_GP_psidot_e",feature_GP_psidot_e) + end + end + end + + # MPC CONTROLLER OPTIMIZATION + if lapStatus.currentLap<=1+max(selectedStates.feature_Nl,selectedStates.Nl) # pF CONTROLLER + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + a_curr = copy(a_est) + # println("z_curr:",round(z_curr,2)) + z_kin = [z_est[6],z_est[5],z_est[4],sqrt(z_est[1]^2+z_est[2]^2)] + (mpcSol.z,mpcSol.u,sol_status) = solveMpcProblem_pathFollow(mdl_pF,mpcParams_pF,modelParams,mpcSol,z_kin,z_prev,u_prev,track) + + else # LMPC CONTROLLER + if PF_FLAG + # save("$(homedir())/simulations/path_following.jld","oldSS",oldSS,"oldSS_true",oldSS_true,"solHistory",solHistory) + if get_param("sim_flag") + save("$(homedir())/simulations/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + else + save("$(homedir())/experiments/path_following.jld","oldSS",oldSS,"solHistory",solHistory) + end + end + + # (xDot, yDot, psiDot, ePsi, eY, s, acc_f) + z_curr = [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + a_curr = copy(a_est) + + if LMPC_FLAG + ###################################################################### + ############### CHOICE 1: DO MPC ON SYS_ID MODEL ##################### + ###################################################################### + tic() + # PREPARE THE PREVIOUS SOLUTION FOR LMPC, WHEN SWITCHING FROM pF TO LMPC + if size(z_prev,2)==4 + z_prev = hcat(z_prev,zeros(size(z_prev,1),2)) + end + # println("z_prev is:",round(z_prev,3)) + # SYS_ID + if !TI_TV_FLAG + # TV SYS_ID + z_to_iden = vcat(z_curr',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i in 1:mpcParams.N + if FEATURE_FLAG + # SELECTING THE FEATURE POINTS FROM DATASET + (iden_z,iden_u,z_iden_plot)=find_feature_dist_simple(feature_z,feature_u,z_to_iden[i,:],u_to_iden[i,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON 2-NORM CRITERION + (iden_z,iden_u,z_iden_plot)=find_SS_dist_simple(solHistory,z_to_iden[i,:],u_to_iden[i,:],lapStatus,selectedStates) + else + # SELECTING THE FEATURE POINTS FROM HISTORY BASED ON SPATIAL CRITERION + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_to_iden[i,:],u_to_iden[i,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff.c_Vx[i,:],mpcCoeff.c_Vy[i])=coeff_iden_dist_simple(iden_z,iden_u) + end + println(iden_z) + println(iden_u) + mpcCoeff.c_Psi = z_to_iden[:,6] + else + # TI SYS_ID + if FEATURE_FLAG + # SELECTING THE FEATURE POINTS FROM DATASET + (iden_z,iden_u,z_iden_plot)=find_feature_dist(feature_z,feature_u,z_curr',u_prev[2,:],selectedStates) + else + if NORM_SPACE_FLAG + # SELECTING THE FEATURE POINTS FROM HISTORY + (iden_z,iden_u,z_iden_plot)=find_SS_dist_simple(solHistory,z_curr',u_prev[2,:],lapStatus,selectedStates) + else + (iden_z,iden_u,z_iden_plot)=find_feature_space(solHistory,z_curr',u_prev[2,:],lapStatus,selectedStates,posInfo) + end + end + (mpcCoeff.c_Vx[1,:],mpcCoeff.c_Vy[1,:],mpcCoeff.c_Psi[1,:])=coeff_iden_dist_simple(iden_z,iden_u) + for i in 2:mpcParams.N + mpcCoeff.c_Vx[i,:]=mpcCoeff.c_Vx[1,:] + mpcCoeff.c_Vy[i,:]=mpcCoeff.c_Vy[1,:] + mpcCoeff.c_Psi[i,:]=mpcCoeff.c_Psi[1,:] + end + # println("c_Vx",round(mpcCoeff.c_Vx[1,:],2)) + # println("c_Vy",round(mpcCoeff.c_Vy[1,:],2)) + # println("c_Psi",round(mpcCoeff.c_Psi[1,:],2)) + end + # GPR DISTURBANCE ID + GP_e_vy = zeros(mpcParams.N) + GP_e_psidot = zeros(mpcParams.N) + z_to_iden = vcat(z_curr',z_prev[3:end,:]) + u_to_iden = vcat(u_prev[2:end,:],u_prev[end,:]) + for i = 1:mpcParams.N + if GP_LOCAL_FLAG + GP_e_vy[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + GP_e_psidot[i] = regre(z_to_iden[i,:],u_to_iden[i,:],feature_GP_vy_e,feature_GP_z,feature_GP_u) + elseif GP_FULL_FLAG + GP_e_vy[i] = GP_full_vy(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_vy_prepare) + GP_e_psidot[i] = GP_full_psidot(z_to_iden[i,:],u_to_iden[i,:],GP_feature,GP_e_psi_dot_prepare) + else + GP_e_vy[i] = 0 + GP_e_psidot[i] = 0 + end + end + # println(GP_e_vy) + # println(GP_e_psidot) + + # FEATURE POINTS VISUALIZATION + if FEATURE_FLAG + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track_f) + else + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(z_iden_plot,track) + end + mpcSol_to_pub.z_iden_x = z_iden_x + mpcSol_to_pub.z_iden_y = z_iden_y + toc() # TIME FOR PREPARATION + # SAFESET POINT SELECTION + selectedStates=find_SS(oldSS,selectedStates,z_curr[1],z_prev,lapStatus,modelParams,mpcParams,track) + # println(selectedStates) + tic() + (mpcSol.z,mpcSol.u,sol_status)=solveMpcProblem_convhull_dyn_iden_simple(mdl_convhull_simple,mpcSol,mpcCoeff,z_curr,z_prev,u_prev,selectedStates,track,GP_e_vy,GP_e_psidot) + toc() # TIME FOR OPTIMIZATION + end + + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG && lapStatus.currentIt>1 + feature_GP_z[k,:] = solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,1,1:6] + feature_GP_u[k,:] = u_prev[1,:] + feature_GP_vy_e[k] = z_curr[5]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,5] + feature_GP_psidot_e[k] = z_curr[6]-solHistory.z[lapStatus.currentIt-1,lapStatus.currentLap,2,6] + k += 1 + end + + # BACK-UP FOR POSSIBLE NON-OPTIMAL SOLUTION + sol_status_dummy = "$sol_status" + # if sol_status_dummy[1] != 'O' + # mpcSol.u=copy(u_prev) + # if LMPC_FLAG || LMPC_DYN_FLAG + # (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,6) + # else + # (mpcSol.z,~,~)=car_pre_dyn(z_curr,mpcSol.u,track,modelParams,4) + # end + # end + end # end of IF:pF/LMPC + # tic() + # DATA WRITING AND COUNTER UPDATE + log_cvx[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vx + log_cvy[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Vy + log_cpsi[lapStatus.currentIt,:,:,lapStatus.currentLap] = mpcCoeff.c_Psi + + n_state = size(mpcSol.z,2) + + mpcSol.a_x = mpcSol.u[1+mpcParams.delay_a,1] + mpcSol.d_f = mpcSol.u[1+mpcParams.delay_df,2] + + if length(mpcSol.df_his)==1 + mpcSol.df_his[1] = mpcSol.u[1+mpcParams.delay_df,2] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.df_his[1:end-1] = mpcSol.df_his[2:end] + mpcSol.df_his[end] = mpcSol.u[1+mpcParams.delay_df,2] + end + + if length(mpcSol.a_his)==1 + mpcSol.a_his[1] = mpcSol.u[1+mpcParams.delay_a,1] + else + # INPUT DELAY HISTORY UPDATE + mpcSol.a_his[1:end-1] = mpcSol.a_his[2:end] + mpcSol.a_his[end] = mpcSol.u[1+mpcParams.delay_a,1] + end + + if PF_FLAG || (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + # println("saving history data") + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,:,1:n_state]=mpcSol.z + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,4:6]=z_curr[4:6] + solHistory.z[lapStatus.currentIt,lapStatus.currentLap,1,7:8]=a_curr + solHistory.u[lapStatus.currentIt,lapStatus.currentLap,:,:]=mpcSol.u + + # SAFESET DATA SAVING BASED ON CONTROLLER'S FREQUENCY + oldSS.oldSS[lapStatus.currentIt,:,lapStatus.currentLap]=z_curr # [z_est[6],z_est[5],z_est[4],z_est[1],z_est[2],z_est[3]] + oldSS.cost2target[lapStatus.currentIt,lapStatus.currentLap]=lapStatus.currentIt + end + + statusHistory[lapStatus.currentIt,lapStatus.currentLap] = "$sol_status" + if !LMPC_DYN_FLAG && !LMPC_KIN_FLAG && !PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl) + selectFeatureHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = z_iden_plot + end + if (!PF_FLAG && lapStatus.currentLap > 1+max(selectedStates.feature_Nl,selectedStates.Nl)) + if !LMPC_FLAG && !LMPC_DYN_FLAG + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,1:4] = selectedStates.selStates + else + selectHistory[lapStatus.currentIt,lapStatus.currentLap,:,:] = selectedStates.selStates + end + end + if !GP_LOCAL_FLAG || !GP_FULL_FLAG + GP_vy_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_vy + GP_psidot_History[lapStatus.currentIt,lapStatus.currentLap,:] = GP_e_psidot + end + + # VISUALIZATION COORDINATE CALCULATION FOR view_trajectory.jl NODE + (z_x,z_y) = trackFrame_to_xyFrame(mpcSol.z,track) + mpcSol_to_pub.z_x = z_x + mpcSol_to_pub.z_y = z_y + # println(selectedStates.selStates) + (SS_x,SS_y) = trackFrame_to_xyFrame(selectedStates.selStates,track) + mpcSol_to_pub.SS_x = SS_x + mpcSol_to_pub.SS_y = SS_y + mpcSol_to_pub.z_vx = mpcSol.z[:,4] + mpcSol_to_pub.SS_vx = selectedStates.selStates[:,4] + mpcSol_to_pub.z_s = mpcSol.z[:,1] + mpcSol_to_pub.SS_s = selectedStates.selStates[:,1] + # FORECASTING POINTS FROM THE DYNAMIC MODEL + + if length(z_curr)==6 + (z_fore,~,~) = car_pre_dyn_true(z_curr,mpcSol.u,track,modelParams,6) + (z_fore_x,z_fore_y) = trackFrame_to_xyFrame(z_fore,track) + mpcSol_to_pub.z_fore_x = z_fore_x + mpcSol_to_pub.z_fore_y = z_fore_y + end + + cmd.servo = convert(Float32,mpcSol.d_f) + cmd.motor = convert(Float32,mpcSol.a_x) + + z_prev = copy(mpcSol.z) + u_prev = copy(mpcSol.u) + # toc() + println("$sol_status Current Lap: ", lapStatus.currentLap, ", It: ", lapStatus.currentIt, " v: $(z_est[1])") + lapStatus.currentIt += 1 + else + println("No estimation data received!") + end + rossleep(loop_rate) + end # END OF THE WHILE LOOP + # THIS IS FOR THE LAST NO FINISHED LAP + solHistory.cost[lapStatus.currentLap] = lapStatus.currentIt-1 + # DATA SAVING + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + log_path = "$(homedir())/$(folder_name)/LMPC-$(file_name)-$(run_time).jld" + save(log_path,"log_cvx",log_cvx,"log_cvy",log_cvy,"log_cpsi",log_cpsi,"GP_vy_History",GP_vy_History,"GP_psidot_History",GP_psidot_History, + "selectedStates",selectedStates,"oldSS",oldSS,"solHistory",solHistory, + "selectHistory",selectHistory,"selectFeatureHistory",selectFeatureHistory,"statusHistory",statusHistory, + "track",track,"modelParams",modelParams,"mpcParams",mpcParams) + # COLLECT ONE STEP PREDICTION ERROR FOR GPR + if !GP_LOCAL_FLAG && !GP_FULL_FLAG + run_time = Dates.format(now(),"yyyy-mm-dd-H:M") + if get_param("sim_flag") + log_path = "$(homedir())/simulations/Feature_Data/FeatureData_GP-$(file_name).jld" + else + log_path = "$(homedir())/experiments/Feature_Data/FeatureData_GP-$(file_name).jld" + end + # CUT THE FRONT AND REAR TAIL BEFORE SAVING THE DATA + feature_GP_z = feature_GP_z[1:k-1,:] + feature_GP_u = feature_GP_u[1:k-1,:] + feature_GP_vy_e = feature_GP_vy_e[1:k-1] + feature_GP_psidot_e = feature_GP_psidot_e[1:k-1] + save(log_path,"feature_GP_z",feature_GP_z,"feature_GP_u",feature_GP_u, + "feature_GP_vy_e",feature_GP_vy_e,"track",track, + "feature_GP_psidot_e",feature_GP_psidot_e) + end + println("Exiting LMPC node. Saved data to $log_path.") +end + +if ! isinteractive() + main() +end + +# zCurr[1] = v_x +# zCurr[2] = v_y +# zCurr[3] = psiDot +# zCurr[4] = ePsi +# zCurr[5] = eY +# zCurr[6] = s diff --git a/workspace/src/barc/src/Localization_helpers.py b/workspace/src/barc/src/Localization_helpers.py new file mode 100755 index 00000000..c17eea4b --- /dev/null +++ b/workspace/src/barc/src/Localization_helpers.py @@ -0,0 +1,302 @@ +""" +This module includes all functions that are necessary to convert the current absolute position +(x, y, psi) and the given racetrack to the current relative position (s, eY, ePsi) +This file was created by Shuqi Xu (shuqixu@berkeley.edu) +""" + +from numpy import hstack, arange, array, transpose, pi, sin, cos +from numpy import size, argmin, ceil +from numpy import ones, sqrt, sum + +class Track(object): + """ Object collecting estimated state data + Attributes: + addCurve(): + Helper function for track construction + create_feature_track(): + Create track to collect feature data + create_race_track(): + Create track to do racing + Localize(): + Localize the position on the current map + """ + def __init__(self,ds,width): + + # Track construction + self.nodes = None # track x-y array + self.nodes_bound1 = None # track bound x-y array + self.nodes_bound2 = None # track bound x-y array + self.track_s = None # track s array + self.track_idx = None # track idx array + self.curvature = None # track curvature array + self.theta = None # track tangent angle array + + # Track parameters + self.ds = ds # discretization distance + self.width = width # track width + self.n = 0 # number of nodes + + # POSITIONS RELATED VARIABLES + self.s = 0. + self.ey = 0. + self.epsi = 0. + self.x = 0. + self.y = 0. + + self.pos = [0.,0.] # current position + self.psi = 0. # current orientation + self.curv_curr = 0. # current track curvature + self.idx_curr = 0 # current track position + + + def addCurve(self, length, angle): + """ + Arguments: + theta: track tangent angle array, which has been constructed + curvature: track curvature array, which has been constructed + length: next section number of points + angle: next section radian + """ + d_theta = 0 + ds = self.ds + + curve = 2*sum(arange(1,length/2+1,1))+length/2 + + for i in range(0,length): + if i < length/2+1: + d_theta = d_theta + angle / curve + else: + d_theta = d_theta - angle / curve + + self.theta = hstack((self.theta,self.theta[-1] + d_theta)) + self.curvature = hstack((self.curvature,d_theta/ds)) + + def createFeatureTrack(self): + """Track construction for feature data collecting""" + + # TRACK PARAMETERS INITIALIZATION + ds = self.ds + width = self.width + self.theta = array([pi/4]) + self.curvature = array([0]) + + # X-Y TRACK/BOUND COORDINATE INITILIZATION + x = array([0]) + bound1_x = array([0]) + bound2_x = array([0]) + y = array([0]) + bound1_y = array([0]) + bound2_y = array([0]) + bound1_x = x + width/2*cos(self.theta[0]+pi/2) + bound2_x = x - width/2*cos(self.theta[0]+pi/2) + bound1_y = y + width/2*sin(self.theta[0]+pi/2) + bound2_y = y - width/2*sin(self.theta[0]+pi/2) + + # TRACK DESIGN + v = 2.5 + max_a = 7.6; + R = v**2/max_a + max_c = 1/R + angle = (pi+pi/2)-0.105 + R_kin = 0.8 + num_kin = int(round(angle/ ( ds/R_kin ) * 2)) + num = max(int(round(angle/ ( ds/R ) * 2)),num_kin) + num = int(ceil(num*1.0)) + + # TRACK CONSTRUCTION + self.addCurve(num,-angle) + self.addCurve(num,angle) + + for i in range(1,size(self.theta)): + x_next = x[-1] + cos(self.theta[i])*ds + y_next = y[-1] + sin(self.theta[i])*ds + bound1_x_next = x_next + width/2*cos(self.theta[i]+pi/2) + bound2_x_next = x_next - width/2*cos(self.theta[i]+pi/2) + bound1_y_next = y_next + width/2*sin(self.theta[i]+pi/2) + bound2_y_next = y_next - width/2*sin(self.theta[i]+pi/2) + x = hstack((x, x_next)) + y = hstack((y, y_next)) + bound1_x = hstack((bound1_x, x_next+ width/2*cos(self.theta[i]+pi/2))) + bound2_x = hstack((bound2_x, x_next- width/2*cos(self.theta[i]+pi/2))) + bound1_y = hstack((bound1_y, y_next+ width/2*sin(self.theta[i]+pi/2))) + bound2_y = hstack((bound2_y, y_next- width/2*sin(self.theta[i]+pi/2))) + + self.nodes = array([x,y]) + self.nodes_bound1 = array([bound1_x,bound1_y]) + self.nodes_bound2 = array([bound2_x,bound2_y]) + self.n = size(x) + self.track_s = array([ds*i for i in range(self.n)]) + self.track_idx = array([i for i in range(self.n)]) + + print "number of nodes: %i"%self.n, "length: %f"%((self.n)*ds) + + def createRaceTrack(self): + """Track construction for feature data collecting""" + + # TRACK PARAMETERS INITIALIZATION + ds = self.ds + width = self.width + self.theta = array([0]) + self.curvature = array([0]) + + # X-Y TRACK/BOUND COORDINATE INITILIZATION + x = array([0]) + bound1_x = array([0]) + bound2_x = array([0]) + y = array([0]) + bound1_y = array([0]) + bound2_y = array([0]) + bound1_x = x + width/2*cos(self.theta[0]+pi/2) + bound2_x = x - width/2*cos(self.theta[0]+pi/2) + bound1_y = y + width/2*sin(self.theta[0]+pi/2) + bound2_y = y - width/2*sin(self.theta[0]+pi/2) + + # TRACK DATA CALCULATION + # track_data=[[80,0], + # [120,-pi/2], + # [80,0], + # [220,-pi*0.85], + # [105,pi/15], + # [300,pi*1.15], + # [240,-pi*0.865], + # [100,0], + # [120,-pi/2], + # [153,0], + # [120,-pi/2], + # [211,0]] + + # track for room 3110 + # num = 100 # 60 + # track_data=[[int(ceil(2*80)) ,0], + # [int(ceil(2*num)), pi/2], + # [int(ceil(2*(80+47))) ,0], + # [int(ceil(2*num)), pi/2], + # [int(ceil(2*50)) ,0], + # [int(ceil(2*num)), pi/2], + # [int(ceil(2*4)) , 0], + # [int(ceil(2*num)), -pi/2], + # [int(ceil(2*30)) ,0], + # [int(ceil(2*num)), pi/2], + # [int(ceil(2*4)) ,0], + # [int(ceil(2*num)), pi/2], + # [int(ceil(2*(71+48))) ,0]] + + # Basic track for experiment + # track_data = [[int(ceil(3*60)), 0], + # [int(ceil(3*80)), pi/2], + # [int(ceil(3*20)), 0], + # [int(ceil(3*80)), pi/2], + # [int(ceil(3*40)), -pi/10], + # [int(ceil(3*60)), pi/5], + # [int(ceil(3*40)), -pi/10], + # [int(ceil(3*80)), pi/2], + # [int(ceil(3*20)), 0], + # [int(ceil(3*80)), pi/2], + # [int(ceil(3*75)), 0]] + + # track_data = [[int(ceil(2.8*40)), 0], + # [int(ceil(2.8*120)), -pi/2], + # [int(ceil(2.8*5)), 0], + # [int(ceil(2.8*120)), -pi/2], + # [int(ceil(2.8*80)), 0], + # [int(ceil(2.8*120)), -pi/2], + # [int(ceil(2.8*5)), 0], + # [int(ceil(2.8*120)), -pi/2], + # [int(ceil(2.8*40)), 0]] + + # TRACK FOR MSC EXPERIMENT ROOM + track_data = [[int(ceil(1.5*3*10)), 0], + [int(ceil(1.5*3*120)), pi], + [int(ceil(1.5*3*20)), 0], + [int(ceil(1.5*3*120)), pi], + [int(ceil(1.5*3*10)), 0]] + + # TRACK CONSTRUCTION + for i in range(len(track_data)): + num = track_data[i][0] + angle = track_data[i][1] + self.addCurve(num,angle) + + for i in range(1,size(self.theta)): + x_next = x[-1] + cos(self.theta[i])*ds + y_next = y[-1] + sin(self.theta[i])*ds + bound1_x_next = x_next + width/2*cos(self.theta[i]+pi/2) + bound2_x_next = x_next - width/2*cos(self.theta[i]+pi/2) + bound1_y_next = y_next + width/2*sin(self.theta[i]+pi/2) + bound2_y_next = y_next - width/2*sin(self.theta[i]+pi/2) + x = hstack((x, x_next)) + y = hstack((y, y_next)) + bound1_x = hstack((bound1_x, x_next+ width/2*cos(self.theta[i]+pi/2))) + bound2_x = hstack((bound2_x, x_next- width/2*cos(self.theta[i]+pi/2))) + bound1_y = hstack((bound1_y, y_next+ width/2*sin(self.theta[i]+pi/2))) + bound2_y = hstack((bound2_y, y_next- width/2*sin(self.theta[i]+pi/2))) + + self.nodes = array([x,y]) + self.nodes_bound1 = array([bound1_x,bound1_y]) + self.nodes_bound2 = array([bound2_x,bound2_y]) + self.n = size(x) + self.track_s = array([ds*i for i in range(self.n)]) + self.track_idx = array([i for i in range(self.n)]) + + print "number of nodes: %i"%self.n, "length : %f"%((self.n)*ds) + + def Localize(self,x,y,psi): + """ Localization on the constructed track + Arguements: + 1. x (global x-y) + 2. y (global x-y) + 3. psi (global x-y) + """ + + self.pos = array([x,y]) + self.psi = psi + self.x = x + self.y = y + + # 1. Localize s + l_ran = 1 # LOCALIZATION RANGE: useful when track is overlaping + # if self.s > self.track_s[-1]-l_ran: + # idx_candidate_1 = (self.track_s+l_ran>=self.track_s[-1]) + # idx_candidate_2 = (self.track_s<=l_ran) + # x_candidate = hstack((self.nodes[0,:][idx_candidate_1],self.nodes[0,:][idx_candidate_2])) + # y_candidate = hstack((self.nodes[1,:][idx_candidate_1],self.nodes[1,:][idx_candidate_2])) + # nodes_candidate = array([x_candidate,y_candidate]) + # s_candidate = hstack((self.track_s[idx_candidate_1],self.track_s[idx_candidate_2])) + # idx_curr_candidate = hstack((self.track_idx[idx_candidate_1],self.track_idx[idx_candidate_2])) + # else: + + + dist_s = self.track_s - self.s + # idx_candidate = (dist_s>=0) & (dist_s-10000 + x_candidate = self.nodes[0,:][idx_candidate] + y_candidate = self.nodes[1,:][idx_candidate] + nodes_candidate = array([x_candidate,y_candidate]) + s_candidate = self.track_s[idx_candidate] + idx_curr_candidate = self.track_idx[idx_candidate] + + + + + dist = sum((self.pos*ones([len(x_candidate),2])-nodes_candidate.transpose())**2,1) + idx_min = argmin(dist) + self.s = s_candidate[idx_min] + self.idx_curr = idx_curr_candidate[idx_min] + + dist_bound1 = (self.pos[0]-self.nodes_bound1[0,self.idx_curr])**2 + (self.pos[1]-self.nodes_bound1[1,self.idx_curr])**2 + dist_bound2 = (self.pos[0]-self.nodes_bound2[0,self.idx_curr])**2 + (self.pos[1]-self.nodes_bound2[1,self.idx_curr])**2 + + # 2. Localize ey + ey = dist[idx_min]**0.5 + if dist_bound1>dist_bound2: + self.ey = -ey + else: + self.ey = ey + + # 3. Localize epsi + self.epsi = (self.psi - self.theta[self.idx_curr]+pi)%(2*pi)-pi + + # 4. Localize curvature + self.curv_curr = self.curvature[self.idx_curr] + + return self.s, self.ey, self.epsi diff --git a/workspace/src/barc/src/README.md b/workspace/src/barc/src/README.md new file mode 100644 index 00000000..9334e211 --- /dev/null +++ b/workspace/src/barc/src/README.md @@ -0,0 +1,56 @@ +Source code for LMPC +==================== + +1. barc_record.jl +================= +This node records data that might be interesting for later evaluation. It subscribes to topics, records received messages into arrays and saves these arrays into a .jld file when the ros master is shut down. It uses log_functions.jl in barc_lib to import the callback functions. + +2. barc_simulator_dyn.jl +======================== +This node simulates a dynamic bicycle model. It subscribes to the ECU commands of the MPC and publishes simulated "raw" measurement data: GPS, IMU and Encoders. It also logs its "real" states into a .jld file, this can be interesting to experiment with Kalman filters to find a good estimator (since you can compare "real" simulation data with your estimation data). +It uses the simModel.jl in barc_lib. + +3. controller_low_level.py +========================== +This is the low level controller that maps acceleration and steering angle commands (in SI units) to PWM signals that are needed by the motor and steering servo. The mapping is linear and has been found by multiple open-loop measurements. It might vary from BARC to BARC. + +4. controller_MPC_traj.jl +========================= +This is a simple MPC path following controller that uses a given track. + +5. debug_localization.py +======================== +This is a very short script you can use to debug and analyze the mapping from x/y coordinates to s/e_y/e_psi. You can use it to find an optimal approximation length for the curvature. + +6. filtering.py +=============== +Basic filtering classes, not used in LMPC but might be helpful for future use. + +7. LMPC_node.jl +=============== +This is the main node of LMPC. It receives data from the estimator, records it and does system ID and LMPC to find optimal inputs. + +8. Localization_helpers.py +========================== +This contains a class that is used to map x-y-coordinates to s-ey-epsi coordinates. It also includes the shape of the track (in create_track()). Tracks are created by putting together curves using the function add_curve(track,length,angle_of_curve). This makes sure that there are no jumps in curvature (but there are jumps in the 1st derivative of the curvature!). +Mapping x-y to s-ey: +1. set_pos() sets the class variables to the current position/heading +2. find_s() calculates s, ey, epsi and the curvature coefficients according to its settings (polynomial degree, length of approximation) +3. read variables (s,ey, ...) manually from class (look at state estimation node to get an idea) + +9. observers.py +=============== +Contains the function for the EKF and other necessary functions. + +10. open_loop.jl +================ +Can do open loop experiments with this and send either ECU or ECU_pwm commands. + +11. state_estimation_SensorKinematicModel.py +============================================ +State estimation node. Uses a combination of a kinematic bicycle model and a kinematic motion model in an EKF (without the bicycle assumption) to calculate states. It also prefilters GPS data to make sure holes and outliers are not used. + +12. system_models.py +==================== +This contains the model that is used by the Kalman filter in state estimation. + diff --git a/workspace/src/barc/src/StateEstimatorClean.py b/workspace/src/barc/src/StateEstimatorClean.py new file mode 100755 index 00000000..6b9c1b60 --- /dev/null +++ b/workspace/src/barc/src/StateEstimatorClean.py @@ -0,0 +1,754 @@ +#!/usr/bin/env python +""" + File name: state_estimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- +import sys +sys.path.append(sys.path[0]+'/ControllersObject') +sys.path.append(sys.path[0]+'/Utilities') +import rospy +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import os +import pdb + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator_2/delay_a") + df_delay = rospy.get_param("state_estimator_2/delay_df") + loop_rate = 50.0 + + + Q = eye(8) + Q[0,0] = 0.01 # x + Q[1,1] = 0.01 # y + Q[2,2] = 0.01 # vx + Q[3,3] = 0.01 # vy + Q[4,4] = 1.0 # ax + Q[5,5] = 1.0 # ay + Q[6,6] = 10.0 # psi + Q[7,7] = 10.0 # psiDot + R = eye(7) + R[0,0] = 20.0 # x + R[1,1] = 20.0 # y + R[2,2] = 0.1 # vx + R[3,3] = 10.0 # ax + R[4,4] = 10.0 # ay + R[5,5] = 0.1 # psiDot + R[6,6] = 0.01 # vy + thReset = 1.5 + + # Q_noVy = eye(8) + # Q_noVy[0,0] = 0.01 # x + # Q_noVy[1,1] = 0.01 # y + # Q_noVy[2,2] = 0.01 # vx + # Q_noVy[3,3] = 0.01 # vy + # Q_noVy[4,4] = 1.0 # ax + # Q_noVy[5,5] = 1.0 # ay + # Q_noVy[6,6] = 10.0 # psi + # Q_noVy[7,7] = 10.0 # psidot + # # Q[8,8] = 0.0 # psiDot in the model + # R_noVy = eye(6) + # R_noVy[0,0] = 20.0 # x + # R_noVy[1,1] = 20.0 # y + # R_noVy[2,2] = 0.1 # vx + # R_noVy[3,3] = 10.0 # ax + # R_noVy[4,4] = 30.0 # ay + # R_noVy[5,5] = 0.10 # psiDot + # R[6,6] = 0.001 # vy + # thReset_noVy = 0.8 + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + + est = Estimator(t0,loop_rate,a_delay,df_delay,Q, R, thReset) + + estMsg = pos_info() + + saved_x_est = [] + saved_y_est = [] + saved_vx_est = [] + saved_vy_est = [] + saved_psi_est = [] + saved_psiDot_est = [] + saved_ax_est = [] + saved_ay_est = [] + saved_switch = [] + + while not rospy.is_shutdown(): + + if (est.vx_est + 0.0 * np.abs(est.psiDot_est) ) > 0.8 or (np.abs(est.psiDot_est) > 1.0): + flagVy = True + # print "================ Not using vy! ==============" + else: + flagVy = False + # print "================ Using vy! ==============" + + est.estimateState(imu,gps,enc,ecu,est.ekf,flagVy) + + estMsg.header.stamp = rospy.get_rostime() + + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + + # Save estimator output. + # NEED TO DO IT HERE AS THERE ARE MULTIPLE ESTIMATOR RUNNING IN PARALLEL + saved_x_est.append(estMsg.x) + saved_y_est.append(estMsg.y) + saved_vx_est.append(estMsg.v_x) + saved_vy_est.append(estMsg.v_y) + saved_psi_est.append(estMsg.psi) + saved_psiDot_est.append(estMsg.psiDot) + saved_ax_est.append(estMsg.a_x) + saved_ay_est.append(estMsg.a_y) + saved_switch.append(int(flagVy)) + + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_data/estimator_output.npz") + np.savez(pathSave,yaw_est_his = saved_psi_est, + psiDot_est_his = saved_psiDot_est, + x_est_his = saved_x_est, + y_est_his = saved_y_est, + vx_est_his = saved_vx_est, + vy_est_his = saved_vy_est, + ax_est_his = saved_ax_est, + ay_est_his = saved_ay_est, + gps_time = est.gps_time, + imu_time = est.imu_time, + enc_time = est.enc_time, + inp_x_his = est.x_his, + inp_y_his = est.y_his, + inp_v_meas_his = est.v_meas_his, + inp_ax_his = est.ax_his, + inp_ay_his = est.ay_his, + inp_psiDot_his = est.psiDot_his, + inp_a_his = est.inp_a_his, + inp_df_his = est.inp_df_his, + flagVy = saved_switch) + + pathSave = os.path.join(homedir,"barc_data/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + gps_time = gps.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R,thReset): + """ Initialization + Arguments: + t0: starting measurement time + """ + self.thReset = thReset + + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() + + self.x_est_his = [] + self.y_est_his = [] + self.vx_est_his = [] + self.vy_est_his = [] + self.v_est_his = [] + self.ax_est_his = [] + self.ay_est_his = [] + self.yaw_est_his = [] + self.psiDot_est_his = [] + self.time_his = [] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [] + self.y_his = [] + self.v_meas_his = [] + self.ax_his = [] + self.ay_his = [] + self.psiDot_his = [] + self.inp_a_his = [] + self.inp_df_his = [] + + self.gps_time = [] + self.enc_time = [] + self.imu_time = [] + + self.oldGPS_x = 0.0 + self.oldGPS_y = 0.0 + + # ecu command update + def estimateState(self, imu, gps, enc, ecu, KF, flagVy): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + # u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + dist = np.sqrt(( self.x_est - gps.x )**2 + ( self.y_est - gps.y )**2) + + if flagVy == False: + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, bta * enc.v_meas]) + else: + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + + if np.abs(imu.psiDot) < self.thReset: + self.z[3] = 0.0 + + KF(y,u, flagVy) + + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(gps.x) + self.y_his.append(gps.y) + self.v_meas_his.append(enc.v_meas) + self.ax_his.append(imu.ax) + self.ay_his.append(imu.ay) + self.psiDot_his.append(imu.psiDot) + self.inp_a_his.append(u[0]) + self.inp_df_his.append(u[1]) + + self.gps_time.append(gps.curr_time) + self.imu_time.append(imu.curr_time) + self.enc_time.append(enc.curr_time) + # SAVE output KF given the above measurements + self.saveHistory() + + def ekf(self, y, u, flagVy): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + numericalDiffActive = True + + xDim = self.z.size # dimension of the state + + mx_kp1, Aa = self.f(self.z, u) # predict next state + An = self.numerical_jac(self.f, self.z, u, flagVy) # linearize process model about current state + + if numericalDiffActive == True: + A = An + else: + A = Aa + + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + + my_kp1, Ha = self.h(mx_kp1, u, flagVy) # predict future output + Hn = self.numerical_jac(self.h, mx_kp1, u, flagVy) # linearize measurement model about predicted next state + + if numericalDiffActive == True: + H = Hn + else: + H = Ha + + P12 = dot(P_kp1, H.T) # cross covariance + + if flagVy == False: + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + else: + K = dot(P12, inv( dot(H,P12) + self.R[:-1,:-1])) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + + if flagVy == False: + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + else: + self.P = dot(dot(K,self.R[:-1,:-1]),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u, flagVy): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y, _ = func(x,u, flagVy) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi, _ = func(xp, u, flagVy) + xp[i] = x[i] - eps/2.0 + ylo, _ = func(xp, u, flagVy) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u, flagVy=True): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + + jac = np.array([[1.0, 0.0, dt*cos(z[6]), -dt*sin(z[6]), 0.0, 0.0, dt*(-sin(z[6])*z[2]-cos(z[6])*z[3]), 0.0], + [0.0, 1.0, dt*sin(z[6]), dt*cos(z[6]), 0.0, 0.0, dt*( cos(z[6])*z[2]-sin(z[6])*z[3]), 0.0], + [0.0, 0.0, 1.0, dt*z[7], dt, 0.0, 0.0, dt*z[3]], + [0.0, 0.0, -dt*z[7], 1.0, 0.0, dt, 0.0,-dt*z[2]], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, dt], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) + + return np.array(zNext), jac + + def h(self, x, u, flagVy): + """ This is the measurement model to the kinematic<->sensor model above """ + if flagVy == False: + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + + jac = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,], + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,]]) + else: + y = [0]*6 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + + jac = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) + + return np.array(y), jac + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + + +# ======================================================================================================================================== +# ======================================================= SENSOR CLASSES ================================================================= +# ======================================================================================================================================== + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() + + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.prev_time = self.curr_time + + self.saveHistory() + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + # rospy.Subscriber('hedge_pos', hedge_pos, self.gps_callback, queue_size=1) + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + + # GPS measurement history + self.x_his = np.array([]) + self.y_his = np.array([]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([]) + self.curr_time = rospy.get_rostime().to_sec() + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() + + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # n_intplt = 20 + # if size(self.x_his,0) > n_intplt: # do interpolation when there is enough points + # x_intplt = self.x_his[-n_intplt:] + # y_intplt = self.y_his[-n_intplt:] + # t_intplt = self.time_his[-n_intplt:] + # t_matrix = vstack([t_intplt**2, t_intplt, ones(sz)]).T + # self.c_X = linalg.lstsq(t_matrix, x_intplt)[0] + # self.c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + # self.x = polyval(self.c_X, self.curr_time) + # self.y = polyval(self.c_Y, self.curr_time) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + v_est = self.v_rr + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/Tools/estimator_playback.py b/workspace/src/barc/src/Tools/estimator_playback.py new file mode 100644 index 00000000..b65a2ca4 --- /dev/null +++ b/workspace/src/barc/src/Tools/estimator_playback.py @@ -0,0 +1,707 @@ +import os +import sys +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src")) +from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import pdb + +def approximate_yaw(x, y, time, time_now): + index = np.argmin(abs(time - time_now)) + + x_0 = x[index - 1] + y_0 = y[index - 1] + + x_1 = x[index] + y_1 = y[index] + + argument_y = y_1 - y_0 + argument_x = x_1 - x_0 + # angle = np.arctan(argument_y / argument_x) + + angle = 0.0 + + if argument_x > 0.0: + angle = np.arctan(argument_y / argument_x) + + if argument_y >= 0.0 and argument_x < 0.0: + angle = np.pi + np.arctan(argument_y / argument_x) + + if argument_y < 0.0 and argument_x < 0.0: + angle = - np.pi + np.arctan(argument_y / argument_x) + + if argument_y > 0.0 and argument_x == 0.0: + angle = np.pi / 2.0 + + if argument_y < 0.0 and argument_x == 0.0: + angle = - np.pi / 2.0 + + if angle < 0.0: + angle += 2.0 * np.pi + + # angle = np.arctan2(argument_y, argument_x) + + return angle + +def interpolate_yaw(yaw, gps_time, time_now): + seconds_used = 1.0 + + start = np.argmax(gps_time >= time_now - seconds_used) + end = max(np.argmax(gps_time >= time_now) - 1, 0) + + print start + print end + + if start == end: + return yaw[start] + + t_gps = gps_time[start : end] - gps_time[start] + yaw_hist = yaw[start : end] + + print yaw.shape + + t_matrix = np.vstack([t_gps**2, t_gps, np.ones(end - start)]).T + + print t_matrix.shape + print yaw_hist.shape + c_yaw = np.linalg.lstsq(t_matrix, yaw_hist)[0] + + yaw_interpolated = np.polyval(c_yaw, time_now - gps_time[start]) + + return yaw_interpolated + +def main(): + # node initialization + a_delay = 0.0 + df_delay = 0.0 + loop_rate = 50.0 + + """ + Q = eye(8) + Q[0,0] = 0.01 # x + Q[1,1] = 0.01 # y + Q[2,2] = 0.01 # vx + Q[3,3] = 0.01 # vy + Q[4,4] = 1.0 # ax + Q[5,5] = 1.0 # ay + Q[6,6] = 0.0001 # psi + Q[7,7] = 1.0 # psidot + R = eye(7) + R[0,0] = 1.0 # x + R[1,1] = 1.0 # y + R[2,2] = 0.1 # vx + R[3,3] = 10.0 # ax + R[4,4] = 10.0 # ay + R[5,5] = 1.0 # psiDot + R[6,6] = 2.0 # yaw + """ + + Q = eye(8) + Q[0,0] = 0.01 # x + Q[1,1] = 0.01 # y + Q[2,2] = 0.01 # vx + Q[3,3] = 0.01 # vy + Q[4,4] = 0.03 # ax + Q[5,5] = 0.05 # ay + Q[6,6] = 0.05 # psi + Q[7,7] = 0.01 # psidot + R = eye(7) + R[0,0] = 0.5 # x + R[1,1] = 0.5 # y + R[2,2] = 1.0 # vx + R[3,3] = 2.0 # ax + R[4,4] = 8.0 # ay + R[5,5] = 0.1 # psiDot + R[6,6] = 100.0 # yaw + + imu = ImuClass(0.0) + gps = GpsClass(0.0) + enc = EncClass(0.0) + ecu = EcuClass(0.0) + est = Estimator(0.0,loop_rate,a_delay,df_delay,Q,R) + + # track = Track(0.01,1.0) + # track.createRaceTrack() + + estMsg = pos_info() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + npz_output = np.load(pathSave) + KF_x_his = npz_output["KF_x_his"] + KF_y_his = npz_output["KF_y_his"] + KF_v_meas_his = npz_output["KF_v_meas_his"] + KF_ax_his = npz_output["KF_ax_his"] + KF_ay_his = npz_output["KF_ay_his"] + KF_psiDot_his = npz_output["KF_psiDot_his"] + KF_a_his = npz_output["KF_a_his"] + KF_df_his = npz_output["KF_df_his"] + estimator_time = npz_output["estimator_time"] + print "Finish loading data from", pathSave + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + print "Finish loading data from", pathSave + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + npz_gps = np.load(pathSave) + x_his = npz_gps["x_his"] + y_his = npz_gps["y_his"] + x_ply_his = npz_gps["x_ply_his"] + y_ply_his = npz_gps["y_ply_his"] + gps_time = npz_gps["gps_time"] + # gps_angle = npz_gps["angle_his"] + gps_ply_time= npz_gps["gps_ply_time"] + print "Finish loading data from", pathSave + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + v_meas_his = npz_enc["v_meas_his"] + enc_time = npz_enc["enc_time"] + print "Finish loading data from", pathSave + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + npz_ecu = np.load(pathSave) + a_his = npz_ecu["a_his"] + df_his = npz_ecu["df_his"] + ecu_time = npz_ecu["ecu_time"] + print "Finish loading data from", pathSave + + gps_t, indices = np.unique(gps_time, return_index=True) + gps_yaw = zeros(len(x_his[indices])-1) + for i in range(1,len(x_his[indices])): + if gps_t[i] > enc_time[np.argmax(v_meas_his>0.1)] : + gps_yaw[i-1] = approximate_yaw(x_his[indices], y_his[indices], gps_t, gps_t[i]) + gps_yaw = np.unwrap(gps_yaw) + + # gps_angle = gps_angle * np.pi / 180.0 - np.pi / 2.0 + + for i in range(len(KF_a_his)): + # READ SENSOR DATA + time_now = estimator_time[i] + gps.x = KF_x_his[i] + gps.y = KF_y_his[i] + gps.angle = gps_yaw[max(np.argmin(gps_t <= time_now) - 1, 0)] + imu.ax = KF_ax_his[i] + imu.ay = KF_ay_his[i] + imu.psiDot = KF_psiDot_his[i] + enc.v_meas = KF_v_meas_his[i] + ecu.a = KF_a_his[i] + ecu.df = KF_df_his[i] + + # est.angle = interpolate_yaw(gps_yaw, gps_time, time_now) + + + est.estimateState(imu,gps,enc,ecu,est.ekf) + est.saveHistory() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging2/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + angle_est_his = est.angle_est_his, + KF_x_his = KF_x_his, + KF_y_his = KF_y_his, + KF_v_meas_his = KF_v_meas_his, + KF_ax_his = KF_ax_his, + KF_ay_his = KF_ay_his, + KF_psiDot_his = KF_psiDot_his, + KF_a_his = KF_a_his, + KF_df_his = KF_df_his, + estimator_time = estimator_time) + + pathSave = os.path.join(homedir,"barc_debugging2/estimator_imu.npz") + np.savez(pathSave,psiDot_his = psiDot_his, + roll_his = roll_his, + pitch_his = pitch_his, + yaw_his = yaw_his, + ax_his = ax_his, + ay_his = ay_his, + imu_time = imu_time) + + pathSave = os.path.join(homedir,"barc_debugging2/estimator_gps.npz") + np.savez(pathSave,x_his = x_his, + y_his = y_his, + x_ply_his = x_ply_his, + y_ply_his = y_ply_his, + gps_t = gps_t, + # gps_angle = gps_angle, + gps_yaw = gps_yaw, + gps_time = gps_time, + gps_ply_time = gps_ply_time) + + pathSave = os.path.join(homedir,"barc_debugging2/estimator_enc.npz") + np.savez(pathSave,v_fl_his = v_fl_his, + v_fr_his = v_fr_his, + v_rl_his = v_rl_his, + v_rr_his = v_rr_his, + v_meas_his = v_meas_his, + enc_time = enc_time) + + pathSave = os.path.join(homedir,"barc_debugging2/estimator_ecu.npz") + np.savez(pathSave,a_his = a_his, + df_his = df_his, + ecu_time = ecu_time) + + print "Finishing saveing state estimation data" + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + """ + dt = 1.0 / loop_rate + self.rate = 50.0 + L_f = 0.125 + L_r = 0.125 + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDot_drift_est = 0.0 + self.angle = 0.0 + + self.x_est_his = [] + self.y_est_his = [] + self.vx_est_his = [] + self.vy_est_his = [] + self.v_est_his = [] + self.ax_est_his = [] + self.ay_est_his = [] + self.yaw_est_his = [] + self.psiDot_est_his = [] + self.time_his = [] + self.angle_est_his = [] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [0.0] + self.y_his = [0.0] + self.v_meas_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.psiDot_his = [0.0] + self.a_his = [0.0] + self.df_his = [0.0] + self.angle_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, gps.angle]) + # y = np.array([gps.x_ply, gps.y_ply, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + KF(y,u) + + """ + if abs(self.y[5]) < 0.3: + self.z[3] = 0.0 + """ + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(y[0]) + self.y_his.append(y[1]) + self.v_meas_his.append(y[2]) + self.ax_his.append(y[3]) + self.ay_his.append(y[4]) + self.psiDot_his.append(y[5]) + self.a_his.append(u[0]) + self.df_his.append(u[1]) + + self.angle_his.append(y[6]) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ekfMultiRate(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + + idx = [] + if self.x_his[-1] == y[0]: + idx.append(0) + if self.y_his[-1] == y[1]: + idx.append(1) + if self.v_meas_his[-1] == y[2]: + idx.append(2) + if self.ax_his[-1] == y[3]: + idx.append(3) + if self.ay_his[-1] == y[4]: + idx.append(4) + if self.psiDot_his[-1] == y[5]: + idx.append(5) + + if len(idx) == 6: + print "No measurement data received!" + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = mx_kp1 + else: + H = np.delete(H,(idx),axis=0) + R = np.delete(self.R,(idx),axis=0) + R = np.delete(R,(idx),axis=1) + y = np.delete(y,(idx),axis=0) + my_kp1 = np.delete(my_kp1,(idx),axis=0) + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[6] # psi + return np.array(y) + + def saveHistory(self): + # self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + self.angle_est_his.append(self.angle) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + # GPS measurement + self.x = 0.0 + self.y = 0.0 + self.x_ply = 0.0 + self.y_ply = 0.0 + self.angle = 0.0 + + # GPS measurement history + self.x_his = np.array([]) + self.y_his = np.array([]) + self.x_ply_his = np.array([0.0]) + self.y_ply_his = np.array([0.0]) + self.angle_his = np.array([0.0]) + + +class EncClass(object): + + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + +class EcuClass(object): + + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + +if __name__ == '__main__': + main() diff --git a/workspace/src/barc/src/Tools/plotClosedLoop.py b/workspace/src/barc/src/Tools/plotClosedLoop.py new file mode 100644 index 00000000..1758fd49 --- /dev/null +++ b/workspace/src/barc/src/Tools/plotClosedLoop.py @@ -0,0 +1,171 @@ +import numpy as np +import os +import sys +import pdb +import matplotlib.pyplot as plt +homedir = os.path.expanduser("~") +import sys +sys.path.append(sys.path[0]+'/../Utilities') +import matplotlib.patches as patches + + +def main(): + + # map = Map("oval") + # FIGURE 1 plotting of estimator output data + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_data/estimator_output.npz") + npz_output = np.load(pathSave) + x_est_his = npz_output["x_est_his"] + y_est_his = npz_output["y_est_his"] + vx_est_his = npz_output["vx_est_his"] + vy_est_his = npz_output["vy_est_his"] + ax_est_his = npz_output["ax_est_his"] + ay_est_his = npz_output["ay_est_his"] + psiDot_est_his = npz_output["psiDot_est_his"] + yaw_est_his = npz_output["yaw_est_his"] + gps_time_his = npz_output["gps_time"] + imu_time_his = npz_output["imu_time"] + enc_time_his = npz_output["enc_time"] + inp_x_his = npz_output["inp_x_his"] + inp_y_his = npz_output["inp_y_his"] + inp_v_meas_his = npz_output["inp_v_meas_his"] + inp_ax_his = npz_output["inp_ax_his"] + inp_ay_his = npz_output["inp_ay_his"] + inp_psiDot_his = npz_output["inp_psiDot_his"] + inp_a_his = npz_output["inp_a_his"] + inp_df_his = npz_output["inp_df_his"] + + pathSave = os.path.join(homedir,"barc_data/estimator_imu.npz") + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + + pathSave = os.path.join(homedir,"barc_data/estimator_gps.npz") + npz_gps = np.load(pathSave) + x_his = npz_gps["x_his"] + y_his = npz_gps["y_his"] + gps_time = npz_gps["gps_time"] + + pathSave = os.path.join(homedir,"barc_data/estimator_enc.npz") + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + enc_time = npz_enc["enc_time"] + + pathSave = os.path.join(homedir,"barc_data/estimator_ecu.npz") + npz_ecu = np.load(pathSave) + a_his = npz_ecu["a_his"] + df_his = npz_ecu["df_his"] + ecu_time = npz_ecu["ecu_time"] + + # Plot Trajectory + plt.axis("equal") + plt.plot(x_est_his,y_est_his,color="green") + plt.plot(x_his,y_his,color="blue") + plt.legend() + + plt.show() + # Acc + plt.subplot(311) + plt.plot(range(0,len(inp_ax_his)), inp_ax_his, '-or') + plt.plot(range(0,len(ax_est_his)), ax_est_his, '-sb') + + plt.subplot(312) + plt.plot(range(0,len(inp_ay_his)), inp_ay_his, '-or') + plt.plot(range(0,len(ay_est_his)), ay_est_his, '-sb') + + plt.subplot(313) + plt.plot(range(0,len(inp_psiDot_his)), inp_psiDot_his, '-or') + plt.plot(range(0,len(psiDot_est_his)), psiDot_est_his, '-sb') + + + + # Check Redundancy + plt.figure(10) + plt.plot(range(0,len(gps_time_his)-1), gps_time_his[1:]-gps_time_his[:-1], '--sk') + plt.ylabel('delta gps time') + + plt.figure(11) + plt.plot(range(0,len(imu_time_his)-1), imu_time_his[1:]-imu_time_his[:-1], '--sk') + plt.ylabel('delta imu time') + + plt.figure(12) + plt.plot(range(0,len(enc_time_his)-1), enc_time_his[1:]-enc_time_his[:-1], '--sk') + plt.ylabel('delta enc time') + + plt.show() + +def plotTrack(map): + fig = plt.figure("track x-y plot") + Points = np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])) + Points1 = np.zeros((Points, 2)) + Points2 = np.zeros((Points, 2)) + Points0 = np.zeros((Points, 2)) + for i in range(0, int(Points)): + Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) + Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) + Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) + + plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') + plt.plot(Points0[:, 0], Points0[:, 1], '--') + plt.plot(Points1[:, 0], Points1[:, 1], '-b') + plt.plot(Points2[:, 0], Points2[:, 1], '-b') + + return fig + + +def getCar_x_y(x, y, psi, l, w): + car_x = [ x + l * np.cos(psi) - w * np.sin(psi), x + l*np.cos(psi) + w * np.sin(psi), + x - l * np.cos(psi) + w * np.sin(psi), x - l * np.cos(psi) - w * np.sin(psi)] + car_y = [ y + l * np.sin(psi) + w * np.cos(psi), y + l * np.sin(psi) - w * np.cos(psi), + y - l * np.sin(psi) - w * np.cos(psi), y - l * np.sin(psi) + w * np.cos(psi)] + + return car_x, car_y + + +def _initializeFigure_xy(map): + xdata = []; ydata = [] + fig = plt.figure(figsize=(12,8)) + plt.ion() + axtr = plt.axes() + + Points = np.floor(10 * (map.PointAndTangent[-1, 3] + map.PointAndTangent[-1, 4])) + Points1 = np.zeros((Points, 2)) + Points2 = np.zeros((Points, 2)) + Points0 = np.zeros((Points, 2)) + for i in range(0, int(Points)): + Points1[i, :] = map.getGlobalPosition(i * 0.1, map.halfWidth) + Points2[i, :] = map.getGlobalPosition(i * 0.1, -map.halfWidth) + Points0[i, :] = map.getGlobalPosition(i * 0.1, 0) + + plt.plot(map.PointAndTangent[:, 0], map.PointAndTangent[:, 1], 'o') + plt.plot(Points0[:, 0], Points0[:, 1], '--') + plt.plot(Points1[:, 0], Points1[:, 1], '-b') + plt.plot(Points2[:, 0], Points2[:, 1], '-b') + + line_est, = axtr.plot(xdata, ydata, '-b') + line_gps, = axtr.plot(xdata, ydata, '-og') + + v = np.array([[ 1., 1.], + [ 1., -1.], + [-1., -1.], + [-1., 1.]]) + + rec = patches.Polygon(v, alpha=0.5,closed=True, fc='r', ec='k',zorder=10) + axtr.add_patch(rec) + + plt.show() + + return fig, axtr, line_est, line_gps, rec + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/workspace/src/barc/src/Tools/simple_debugging.py b/workspace/src/barc/src/Tools/simple_debugging.py new file mode 100644 index 00000000..cdc74fec --- /dev/null +++ b/workspace/src/barc/src/Tools/simple_debugging.py @@ -0,0 +1,514 @@ +# simple debugging +# GP data plotting to check if they are biased +# -> julia simple_debugging.jl GP SYS_ID_KIN_LIN_TI +# plot the newest recorded data +# -> julia simple_debugging.jl record +# plot the newest trajectory data +# -> julia simple_debugging.jl trajectory +# plot the newest recorded data and trajectory +# -> julia simple_debugging.jl both + +using JLD +using PyPlot +using PyCall +using QuickHull + +@pyimport matplotlib.patches as patch +@pyimport matplotlib.transforms as transforms +@pyimport matplotlib.animation as animation +@pyimport matplotlib as mpl +@pyimport matplotlib.collections as collections +include("./library.jl") +# FLAG FOR PLOTTING THE SELECTED POINTS +const select_flag = false +folder_name = "simulations" + +# find the newest experiment time until now +file_names_all = readdir("$(homedir())/$(folder_name)/") +file_names = [] +for i = 1:length(file_names_all) + if file_names_all[i][1:4] == "LMPC" + push!(file_names,file_names_all[i]) + end +end +file_times = zeros(length(file_names)) +for i = 1:length(file_names) + file_times[i] = ctime("$(homedir())/$(folder_name)/$(file_names[i])") +end +(~,idx) = findmax(file_times) +println("Load data from $(homedir())/$(folder_name)/$(file_names[idx])") + +data = load("$(homedir())/$(folder_name)/$(file_names[idx])") +z = data["z"] +u = data["u"] +SS_z = data["SS_z"] +sysID_z = data["sysID_z"] +sysID_u = data["sysID_u"] +log_cvx = data["log_cvx"] +log_cvy = data["log_cvy"] +log_cpsi = data["log_cpsi"] +GP_vy = data["GP_vy"] +GP_psiDot = data["GP_psiDot"] +track = data["track"] +cost = data["cost"] + + +# STATE AND SYS_ID PARAMETERS PLOT FOR LAPS DONE +if ARGS[1] == "record" || ARGS[1]=="both" + figure("Record-$(file_names[idx])") # PLOT OF STATE + i = 1; plot_way = [3,2] + for i in 1:length(cost) + i == 1 ? current_x = 0 : current_x = sum(cost[1:i-1]) + x_len = Int(cost[i]) + subplot(3,2,1) + plot(current_x+1:current_x+x_len,z[1:x_len,i,1,4],color="blue") + plot([current_x,current_x],[0,2.5],color="grey",linestyle="--") + ylabel("vx") + + subplot(3,2,3) + plot(current_x+1:current_x+x_len,z[1:x_len,i,1,5],color="blue") + plot([current_x,current_x],[-0.3,0.3],color="grey",linestyle="--") + ylabel("vy") + + subplot(3,2,5) + plot(current_x+1:current_x+x_len,z[1:x_len,i,1,6],color="blue") + plot([current_x,current_x],[-3,3],color="grey",linestyle="--") + ylabel("psi_dot") + + # subplot(3,2,5) + # plot(current_x+1:current_x+x_len,solHistory.u[1:x_len,i,1,1],color="blue",label="a") + # plot([current_x,current_x],[-3,3],color="grey",linestyle="--") + # ylabel("psi_dot") + end + # PLOT OF SYS_ID PARAMETERS + # figure(figsize=(15,10)) + for i in 1:length(cost) + i == 1 ? current_x = 0 : current_x = sum(cost[1:i-1]) + x_len = Int(cost[i]) + subplot(3,2,2) + for j=1:size(log_cvx,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="blue",label="cvx_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="red",label="cvx_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="green",label="cvx_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,i,j],color="green") + end + end + plot([current_x,current_x],[-1,1],color="grey",linestyle="--") + end + legend(); ylabel("cvx") + + subplot(3,2,4) + for j=1:size(log_cvy,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="blue",label="cvy_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="red",label="cvy_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="green",label="cvy_$j") + elseif j == 4 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="orange",label="cvy_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="green") + elseif j == 4 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,i,j],color="orange") + end + end + plot([current_x,current_x],[-1,1],color="grey",linestyle="--") + end + legend(); ylabel("cvy") + + subplot(3,2,6) + for j=1:size(log_cpsi,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="blue",label="cpsi_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="red",label="cpsi_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="green",label="cpsi_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,i,j],color="green") + end + end + plot([current_x,current_x],[-4,4],color="grey",linestyle="--") + end + legend(); ylabel("cpsi") + end +end + +# TRAJECTORY PLOT FOR ALL THE LAPS DONE +if ARGS[1] == "trajectory" || ARGS[1]=="both" + i = 1 + fig = figure("Trajectory-$(file_names[idx])") + axs = fig[:add_subplot](1, 1, 1); line = nothing + plot(track.xy[:, 1], track.xy[:, 2], color="grey",alpha=0.4) + plot(track.bound1xy[:, 1], track.bound1xy[:, 2], color="red") + plot(track.bound2xy[:, 1], track.bound2xy[:, 2], color="red") + v_min = 3; v_max = 3; + println(cost) + for i in 1:length(cost) + x_len = Int(cost[i]) + v = sqrt(z[1:x_len,i,1,4].^2 + z[1:x_len,i,1,5].^2) + state = reshape(z[1:x_len,i,1,:],x_len,6) + (x,y) = trackFrame_to_xyFrame(state,track) + + points = reshape([x y], (size(x)[1], 1, 2)) + points_1 = points[1 : end - 1, :, :] + points_2 = points[2 : end, :, :] + segments = cat(2, points_1, points_2) + + # v_min = min(findmin(v)[1],v_min) + # v_max = max(findmax(v)[1],v_max) + v_min = findmin(v)[1] + v_max = findmax(v)[1] + # max should max(V_MAX), min = 0 + norm = plt[:Normalize](v_min, v_max) + + lc = collections.LineCollection(segments, cmap="jet", norm=norm) + # Set the values used for colormapping + lc[:set_array](v) + lc[:set_linewidth](2) + line = axs[:add_collection](lc) + end + axis("equal") + # grid("on") + fig[:colorbar](line, ax=axs) + xlabel("x [m]") + ylabel("y [m]") +end +show() + +#= +function set_limits(ax, solHistory::SolHistory,index::Int64) + i = 1; min_val = 0; max_val = 0 + while solHistory.cost[i]>10 + min_val = min(findmin(solHistory.z[:, i, :, index])[1],min_val) + max_val = max(findmax(solHistory.z[:, i, :, index])[1],max_val) + i+=1 + if i>length(solHistory.cost) + break + end + end + ax[:set_ylim]([min_val - 0.1, max_val + 0.1]) +end + +function update_limits(ax, x_pred::Array{Float64}, x_true::Array{Float64,1}, x_select::Array{Float64,1}) + ax[:set_xlim]([min(findmin(x_pred)[1],findmin(x_true)[1],findmin(x_select)[1]) - 0.1, + max(findmax(x_pred)[1],findmax(x_true)[1],findmax(x_select)[1]) + 0.1]) + # println("pre",findmax(x_pred)[1]) + # println(findmax(x_true)[1]) + # println(findmax(x_select)[1]) +end + +function plot_prediction(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + prediction, = ax[:plot](x, y, color=c, marker="*") + return prediction +end + +function update_prediction(plt_h, x::Array{Float64}, y::Array{Float64}, track::Track) + plt_h[:set_data](x, y) +end + +function plot_selection(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull = patch.Polygon(conv_hull, fill=true, color=c, alpha=0.2) + ax[:add_artist](convex_hull) + + plt_hull_bounds, = ax[:plot](conv_hull[:, 1], conv_hull[:, 2], color=c, linestyle="-", lw=2.0, alpha=0.3) + plt_states, = ax[:plot](x, y, color=c, marker="o", alpha=0.3) + + return convex_hull, plt_hull_bounds, plt_states +end + +function update_selection(convex_hull, plt_hull_bounds, plt_states, x::Array{Float64,1}, y::Array{Float64,1}) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull[:set_xy](conv_hull) + plt_hull_bounds[:set_data](conv_hull[:, 1], conv_hull[:, 2]) + plt_states[:set_data](x, y) +end + +function plot_closed_loop(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + closed_loop, = ax[:plot](x, y, color=c, linestyle="--", lw=2) + return closed_loop +end + +function update_closed_loop(closed_loop_plt, x::Array{Float64,1}, y::Array{Float64,1}, bufferSize::Int64) + x_full = vcat(x,NaN*ones(bufferSize-size(x,1))) + y_full = vcat(y,NaN*ones(bufferSize-size(y,1))) + closed_loop_plt[:set_data](x_full, y_full) +end + +if ARGS[1] == "prediction" + plt[:ion]() + + # Create figure and subplots + fig = figure("Prediction-$(file_names[idx])",figsize=(20,10)) + ax_s_ey = fig[:add_subplot](2, 3, 1) + xlabel("s [m]") + ylabel("e_y [m]") + + ax_s_epsi = fig[:add_subplot](2, 3, 2) + xlabel("s [m]") + ylabel("e_psi [rad]") + + ax_s_psidot = fig[:add_subplot](2, 3, 3) + xlabel("s [m]") + ylabel("psi_dot [rad / s]") + + ax_s_vx = fig[:add_subplot](2, 3, 4) + xlabel("s [m]") + ylabel("v_x [m / s]") + + ax_s_vy = fig[:add_subplot](2, 3, 5) + xlabel("s [m]") + ylabel("v_y [m / s]") + + ax_s_c = fig[:add_subplot](2, 3, 6) + xlabel("s [m]") + ylabel("curvature [1 / m]") + + ax = [ax_s_ey, ax_s_epsi, ax_s_vx, ax_s_vy, ax_s_psidot] + + # PLOT FINISHING LINE + for axe in ax + axe[:plot]([0,0],[-10,10],color="black", linestyle="-") + axe[:plot]([track.s,track.s],[-10,10],color="black", linestyle="-") + end + # PLOT THE BOUNDARY FOR STRAIGHT LINE AND CURVE + for axe in ax + for i = 1:size(track.curvature,1)-1 + if (track.curvature[i] == 0 && track.curvature[i+1] != 0) + axe[:plot]([i-1,i-1]*track.ds,[-10,10],color="grey", linestyle="-") + elseif (track.curvature[i] != 0 && track.curvature[i+1] == 0) + axe[:plot]([i-1,i-1]*track.ds,[-10,10],color="grey", linestyle="-.") + end + end + end + # PLOT THE TRACK BOUNDARY ON e_y + x = (track.idx-1)*track.ds + y = track.w/2*ones(size(x,1)) + ax_s_ey[:plot](x, -y, color="grey", linestyle="--") + ax_s_ey[:plot](x,0*y, color="grey", linestyle="--") + ax_s_ey[:plot](x, y, color="grey", linestyle="--") + + # SET THE Y-LIMITS FOR EACH SUBPLOT + for i = 1 : 5 + set_limits(ax[i], solHistory, i+1) + end + # ax_s_vy[:set_ylim]([-0.15, 0.15]) + ax_s_c[:set_ylim]([-1.2,1.2]) + + # INITIALIZE THE PLOTING FOR MPC PREDICTION + pre_hs = [] + for i = 1 : 5 + pre_h = plot_prediction(ax[i], NaN*ones(size(solHistory.z,3)), NaN*ones(size(solHistory.z,3)), "blue") + pre_hs = [pre_hs; pre_h] + end + + # INITIALIZE THE PLOTTING FOR TRUE PREDICTION WHICH NEEDS TO BE TURNED OFF DURING EXPERIMENT + true_hs = [] + for i = 1 : 5 + true_h = plot_prediction(ax[i], NaN*ones(size(solHistory.z,3)), NaN*ones(size(solHistory.z,3)), "black") + true_hs = [true_hs; true_h] + end + + # INITIALIZE THE PLOTTING FOR SAFESET + conv_hulls = [] + hull_bounds = [] + plt_states = [] + for i = 1 : 5 + conv_hull, hull_bound, plt_state = plot_selection(ax[i], NaN*ones(size(selectedStates.selStates[:, 1])), + NaN*ones(size(selectedStates.selStates[:, 1])), "blue") + conv_hulls = [conv_hulls; conv_hull] + hull_bounds = [hull_bounds; hull_bound] + plt_states = [plt_states; plt_state] + end + + # INITIALIZE THE PLOT FOR THE CLOSE LOOP TRAJECTORY + closed_loop_plts = []; bufferSize = 500 + for i = 1 : 5 + closed_loop_plt = plot_closed_loop(ax[i], NaN*ones(bufferSize), NaN*ones(bufferSize), "grey") + closed_loop_plts = [closed_loop_plts; closed_loop_plt] + end + + # INITIALIZE THE PLOT FOR CURVATURE + curvature_plt, = ax_s_c[:plot](NaN*ones(mpcParams.N+1), NaN*ones(mpcParams.N+1), color="blue", marker="o") + + fig_select = figure("Selected points-$(file_names[idx])",figsize=(20,10)) + ax_track_select = fig_select[:add_subplot](1, 1, 1) + ax_track_select[:plot](track.bound1xy[:,1],track.bound1xy[:,2],color="black") + ax_track_select[:plot](track.bound2xy[:,1],track.bound2xy[:,2],color="black") + select_plt, = ax_track_select[:plot](NaN*ones(size(selectedStates.selStates[:, 1])), + NaN*ones(size(selectedStates.selStates[:, 1])),"go",alpha=0.3) + car_plt, = ax_track_select[:plot](NaN,NaN,"ko",markersize=5,alpha=0.3) + + xlabel("x [m]") + ylabel("y [m]") + axis("equal") + + # UPDATE AND SAVE THE PLOT FOR EVERY LMPC LAPS + GP_flag = false + # if length(ARGS) == 2 + # lap = 1 + # elseif ARGS[3] == "LMPC" + # lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + # elseif ARGS[3] == "GP" + # lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + # GP_vy_plt, = ax_s_vy[:plot](NaN*ones(mpcParams.N),NaN*ones(mpcParams.N),"ro") + # GP_psidot_plt, = ax_s_psidot[:plot](NaN*ones(mpcParams.N),NaN*ones(mpcParams.N),"ro") + # GP_flag = true + # end + lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + + + while solHistory.cost[lap] > 10 + lapStatus.currentLap = lap + + # UPDATE THE PLOT OF THE CURRENT LAP CLOSE LOOP TRAJECTORY + for i = 1 : 5 + update_closed_loop(closed_loop_plts[i], solHistory.z[1:Int(solHistory.cost[lap]),lap,1,1], + solHistory.z[1:Int(solHistory.cost[lap]),lap,1,i+1], bufferSize) + end + + if lap <= 1 + max(selectedStates.Nl,selectedStates.feature_Nl) + # UPDATE THE PLOT OF THE CURRENT LAP CLOSE LOOP TRAJECTORY + for j = 1:Int(solHistory.cost[lap]) + # UPDATE THE PLOTTING DATA + for i = 1 : 5 + update_prediction(pre_hs[i], solHistory.z[j,lap,:,1], solHistory.z[j,lap,:,1+i], track) + update_prediction(true_hs[i], true_state[:,1], true_state[:,i+1], track) + update_limits(ax[i],solHistory.z[j,lap,:,1],true_state[:,1],NaN*ones(mpcParams.N+1)) + end + + # UPDATE THE CURVATURE PLOT + curvature=curvature_prediction(reshape(solHistory.z[j,lap,:,:],size(solHistory.z[j,lap,:,:],3),size(solHistory.z[j,lap,:,:],4)),track) + update_prediction(curvature_plt,solHistory.z[j,lap,:,1],curvature,track) + ax_s_c[:set_xlim]([findmin(solHistory.z[j,lap,:,1])[1]-0.1, findmax(solHistory.z[j,lap,:,1])[1] + 0.1]) + + fig[:savefig]("$(homedir())/$(folder_name)/animations/lap$(lap)_iteration$(j).png", dpi=100) + println("lap$(lap)_iteration$(j)") + end + else + for j = 1:Int(solHistory.cost[lap]) + # SIMULATE THE TRUE MODEL, WHICH SHOULD BE TURNED OFF FOR EXPERIMENTS + true_state, ~, ~ = car_pre_dyn(reshape(solHistory.z[j,lap,1,:],1,size(solHistory.z,4)), + reshape(solHistory.u[j,lap,:,:],size(solHistory.u,3),size(solHistory.u,4)),track,modelParams,6) + + # UPDATE THE PLOTTING DATA + for i = 1 : 5 + update_selection(conv_hulls[i], hull_bounds[i], plt_states[i], + reshape(selectHistory[j,lap,:,1],size(selectHistory[j,lap,:,1],3)), + reshape(selectHistory[j,lap,:,1+i],size(selectHistory[j,lap,:,1],3))) + update_prediction(pre_hs[i], solHistory.z[j,lap,:,1], solHistory.z[j,lap,:,1+i], track) + update_prediction(true_hs[i], true_state[:,1], true_state[:,i+1], track) + update_limits(ax[i],solHistory.z[j,lap,:,1],true_state[:,1],reshape(selectHistory[j,lap,:,1],size(selectHistory,3))) + + if select_flag + # FEATURE DATA PLOTTING UPDATE + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(reshape(selectFeatureHistory[j,lap,:,:],size(selectFeatureHistory,3),size(selectFeatureHistory,4)),track) + update_prediction(select_plt,z_iden_x,z_iden_y,track) + + # CAR POSITION PLOT UPDATE + (car_x,car_y)=trackFrame_to_xyFrame(reshape(solHistory.z[j,lap,1,:],1,6),track) + update_prediction(car_plt,car_x,car_y,track) + end + end + + if GP_flag + GP_vy = solHistory.z[j,lap,2:end,5]+GP_vy_History[j,lap,:] + GP_psidot = solHistory.z[j,lap,2:end,6]+GP_psidot_History[j,lap,:] + update_prediction(GP_vy_plt, solHistory.z[j,lap,2:end,1], GP_vy, track) + update_prediction(GP_psidot_plt, solHistory.z[j,lap,2:end,1], GP_psidot, track) + end + + # UPDATE THE CURVATURE PLOT + curvature=curvature_prediction(reshape(solHistory.z[j,lap,:,:],size(solHistory.z[j,lap,:,:],3),size(solHistory.z[j,lap,:,:],4)),track) + update_prediction(curvature_plt,solHistory.z[j,lap,:,1],curvature,track) + ax_s_c[:set_xlim]([findmin(solHistory.z[j,lap,:,1])[1]-0.1, findmax(solHistory.z[j,lap,:,1])[1] + 0.1]) + ax_s_c[:set_title](statusHistory[j,lap]) + + fig[:savefig]("$(homedir())/$(folder_name)/animations/lap$(lap)_iteration$(j).png", dpi=100) + if select_flag + fig_select[:savefig]("$(homedir())/$(folder_name)/animations/lap$(lap)_iteration$(j)_select.png", dpi=100) + end + println("lap$(lap)_iteration$(j)") + end + end + lap += 1 + if lap>length(solHistory.cost) + break + end + end +end + +if ARGS[1] == "GP" + data = load("$(homedir())/$(folder_name)/Feature_Data/FeatureData_GP-$(ARGS[2]).jld") + feature_GP_z = data["feature_GP_z"] + feature_GP_vy_e = data["feature_GP_vy_e"] + feature_GP_psidot_e = data["feature_GP_psidot_e"] + + fig = figure("GPR-$(file_names[idx])",figsize=(20,10)) + min_val = minimum(feature_GP_vy_e) + max_val = maximum(feature_GP_vy_e) + ax_s_eVy = fig[:add_subplot](2, 1, 1) + ax_s_eVy[:plot](feature_GP_vy_e,".") + for i = 1:length(feature_GP_vy_e)-1 + if feature_GP_z[i,1] > feature_GP_z[i+1,1] + ax_s_eVy[:plot]([i,i],[min_val,max_val],color="grey",linestyle="--") + end + end + xlabel("s [m]") + ylabel("e_vy [m/s]") + + min_val = minimum(feature_GP_psidot_e) + max_val = maximum(feature_GP_psidot_e) + ax_s_ePsidot = fig[:add_subplot](2, 1, 2) + ax_s_ePsidot[:plot](feature_GP_psidot_e,".") + for i = 1:length(feature_GP_vy_e)-1 + if feature_GP_z[i,1] > feature_GP_z[i+1,1] + ax_s_ePsidot[:plot]([i,i],[min_val,max_val],color="grey",linestyle="--") + end + end + xlabel("s [m]") + ylabel("e_Psi_dot [rad/s]") +end +show() +=# diff --git a/workspace/src/barc/src/Tools/simple_debugging2.py b/workspace/src/barc/src/Tools/simple_debugging2.py new file mode 100644 index 00000000..9e86a162 --- /dev/null +++ b/workspace/src/barc/src/Tools/simple_debugging2.py @@ -0,0 +1,218 @@ +import numpy as np +import os +import sys +import pdb +import matplotlib.pyplot as plt +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src")) +from Localization_helpers import Track +# l = Track(0.01,0.8) +# l.createRaceTrack() + +homedir = os.path.expanduser("~") +pathSave = os.path.join(homedir,"barc_debugging2/estimator_output.npz") +npz_output = np.load(pathSave) +x_est_his = npz_output["x_est_his"] +y_est_his = npz_output["y_est_his"] +vx_est_his =npz_output["vx_est_his"] +vy_est_his =npz_output["vy_est_his"] +ax_est_his =npz_output["ax_est_his"] +ay_est_his =npz_output["ay_est_his"] +psiDot_est_his =npz_output["psiDot_est_his"] +yaw_est_his =npz_output["yaw_est_his"] +# angle_est_his = npz_output["angle_est_his"] +KF_x_his = npz_output["KF_x_his"] +KF_y_his = npz_output["KF_y_his"] +KF_v_meas_his = npz_output["KF_v_meas_his"] +KF_ax_his = npz_output["KF_ax_his"] +KF_ay_his = npz_output["KF_ay_his"] +KF_psiDot_his = npz_output["KF_psiDot_his"] +KF_a_his = npz_output["KF_a_his"] +KF_df_his = npz_output["KF_df_his"] +estimator_time = npz_output["estimator_time"] +print "Finish loading data from", pathSave + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_imu.npz") +npz_imu = np.load(pathSave) +psiDot_his = npz_imu["psiDot_his"] +roll_his = npz_imu["roll_his"] +pitch_his = npz_imu["pitch_his"] +yaw_his = npz_imu["yaw_his"] +ax_his = npz_imu["ax_his"] +ay_his = npz_imu["ay_his"] +imu_time = npz_imu["imu_time"] +print "Finish loading data from", pathSave + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_gps.npz") +npz_gps = np.load(pathSave) +x_his = npz_gps["x_his"] +y_his = npz_gps["y_his"] +x_ply_his = npz_gps["x_ply_his"] +y_ply_his = npz_gps["y_ply_his"] +gps_t = npz_gps["gps_t"] +gps_yaw = npz_gps["gps_yaw"] +# gps_angle = npz_gps["gps_angle"] +gps_time = npz_gps["gps_time"] +gps_ply_time= npz_gps["gps_ply_time"] +print "Finish loading data from", pathSave + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_enc.npz") +npz_enc = np.load(pathSave) +v_fl_his = npz_enc["v_fl_his"] +v_fr_his = npz_enc["v_fr_his"] +v_rl_his = npz_enc["v_rl_his"] +v_rr_his = npz_enc["v_rr_his"] +enc_time = npz_enc["enc_time"] +print "Finish loading data from", pathSave + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_ecu.npz") +npz_ecu = np.load(pathSave) +a_his = npz_ecu["a_his"] +df_his = npz_ecu["df_his"] +ecu_time = npz_ecu["ecu_time"] +print "Finish loading data from", pathSave + +""" +# FIGURE 1 plotting of estimator data +# pdb.set_trace() +fig = plt.figure("Estimator") +ax1 = fig.add_subplot(3,1,1,ylabel="yaw_estimation") +ax1.plot(estimator_time,yaw_est_his,label="yaw_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(3,1,2,ylabel="v estimation") +ax2.plot(estimator_time,vx_est_his,label="vx_est") +ax2.plot(estimator_time,vy_est_his,label="vy_est") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(3,1,3,ylabel="acc & psidot estimation") +ax3.plot(estimator_time,ax_est_his,label="ax") +ax3.plot(estimator_time,ay_est_his,label="ay") +ax3.plot(estimator_time,psiDot_est_his,label="psiDot") +ax3.legend() +ax3.grid() +""" + +# FIGURE 1 plotting of estimator data +# pdb.set_trace() +fig = plt.figure("Estimator") +ax1 = fig.add_subplot(3,1,1,ylabel="yaw_estimation") +ax1.plot(estimator_time[7800 : 8200],yaw_est_his[7800 : 8200],label="yaw_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(3,1,2,ylabel="v estimation") +ax2.plot(estimator_time[7800 : 8200],vx_est_his[7800 : 8200],label="vx_est") +ax2.plot(estimator_time[7800 : 8200],vy_est_his[7800 : 8200],label="vy_est") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(3,1,3,ylabel="acc & psidot estimation") +ax3.plot(estimator_time[7800 : 8200],ax_est_his[7800 : 8200],label="ax") +ax3.plot(estimator_time[7800 : 8200],ay_est_his[7800 : 8200],label="ay") +ax3.plot(estimator_time[7800 : 8200],psiDot_est_his[7800 : 8200],label="psiDot") +ax3.legend() +ax3.grid() + + + +# FIGURE 2 plotting of IMU data +num_plot = 3 +fig = plt.figure("Imu") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="IMU yaw") +ax1.plot(imu_time,yaw_his, label="yaw") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="IMU acc & psidot") +ax2.plot(imu_time,psiDot_his,label="psiDot") +ax2.plot(imu_time,ax_his,label="ax") +ax2.plot(imu_time,ay_his,label="ay") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,3,ylabel="pitch & roll angle") +ax3.plot(imu_time,roll_his,label="roll angle") +ax3.plot(imu_time,pitch_his,label="pitch angle") +ax3.legend() +ax3.grid() + +# GPS comparison +num_plot = 2 +fig = plt.figure("GPS") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="x") +ax1.plot(gps_time, x_his, label="x") +ax1.plot(gps_ply_time, x_ply_his, label="x_ply") +ax1.plot(estimator_time, x_est_his, label="x_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="y") +ax2.plot(gps_time, y_his, label="y") +ax2.plot(gps_ply_time, y_ply_his, label="y_ply") +ax2.plot(estimator_time, y_est_his, label="y_est") +ax2.legend() +ax2.grid() + +# ecu plot +fig = plt.figure("input") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(ecu_time, df_his, "-", label="cmd.df") +ax4.plot(ecu_time, a_his, "--", label="cmd.a") +ax4.legend() +ax4.grid() + +# enc plot +fig = plt.figure("encoder") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(enc_time, v_fl_his, "--", label="fl") +ax4.plot(enc_time, v_fr_his, "--", label="fr") +ax4.plot(enc_time, v_rl_his, "-", label="rl") +ax4.plot(enc_time, v_rr_his, "-", label="rr") +ax4.legend() +ax4.grid() + +# trajectory +fig = plt.figure("track x-y plot") +ax1 = fig.add_subplot(1,1,1,ylabel="track x-y plot") +# ax1.plot(l.nodes[0],l.nodes[1],color="grey",linestyle="--", alpha=0.3) +# ax1.plot(l.nodes_bound1[0],l.nodes_bound1[1],color="red",alpha=0.3) +# ax1.plot(l.nodes_bound2[0],l.nodes_bound2[1],color="red",alpha=0.3) +ax1.axis("equal") +# ax1.plot(x_his,y_his,"b.-",label="raw",alpha=0.3) +ax1.plot(KF_x_his[7800 : 8200],KF_y_his[7800 : 8200],"b.-",label="raw",alpha=0.3) + +ax1.plot(x_est_his[7800 : 8200],y_est_his[7800 : 8200],label="est",color="green",linewidth=2,alpha=0.5) + +# ax1.plot(x_ply_his,y_ply_his,label="ply",color="red",linewidth=2,alpha=0.5) +ax1.legend() + +# raw data and estimation data comparison +num_plot = 3 +fig = plt.figure("raw data and est data comparison") +ax2 = fig.add_subplot(num_plot,1,1,ylabel="ax") +ax2.plot(imu_time, ax_his, ".", label="ax_meas") +ax2.plot(estimator_time, ax_est_his, label="ax_est") +# ax2.plot(ecu_time, a_his, "--", label="cmd.acc") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,2,ylabel="ay") +ax3.plot(imu_time, ay_his, ".", label="ay_meas") +ax3.plot(estimator_time, ay_est_his, label="ay_est") +# ax3.plot(ecu_time, df_his, "--", label="cmd.df") +ax3.legend() +ax3.grid() + +# gps_angle = np.unwrap(gps_angle) +ax4 = fig.add_subplot(num_plot,1,3,ylabel="psidot") +ax4.plot(imu_time, psiDot_his, ".", label="psidot_meas") +ax4.plot(estimator_time,psiDot_est_his,label="psidot_est") +ax4.plot(imu_time, yaw_his, ".", label="yaw_meas") +ax4.plot(estimator_time,yaw_est_his,label="yaw_est") +ax4.plot(gps_t[:-1], gps_yaw,label="yaw_gps") +# ax4.plot(gps_time, gps_angle,label="angle_gps") +# ax4.plot(estimator_time, angle_est_his,label="interpolated yaw") + + + + +# ax4.plot(estimator_time, df_his, "--", label="cmd.df") +ax4.legend() +ax4.grid() + +plt.show() diff --git a/workspace/src/barc/src/agent.jl b/workspace/src/barc/src/agent.jl new file mode 100755 index 00000000..0996ff7d --- /dev/null +++ b/workspace/src/barc/src/agent.jl @@ -0,0 +1,1125 @@ +#!/usr/bin/env julia + +#= + File name: agent.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +using Distances + +include("track.jl") +include("transformations.jl") +include("filesystem_helpers.jl") + + +type Agent + # index to identify the agent + index::Int64 + + # parameters + l_front::Float64 + l_rear::Float64 + width::Float64 + + dt::Float64 + + input_lower_bound::Array{Float64} + input_upper_bound::Array{Float64} + state_s_lower_bound::Array{Float64} + state_s_upper_bound::Array{Float64} + + v_max::Float64 + max_slip_angle::Float64 + + mass::Float64 + mu::Float64 + g::Float64 + I_z::Float64 + B::Float64 + C::Float64 + + # dynamic or kinematic model + is_dynamic::Bool + + # states and inputs + states_xy::Array{Float64} + states_s::Array{Float64} + states_true_s::Array{Float64} + + inputs::Array{Float64} + current_lap::Int64 + current_iteration::Int64 + current_input::Array{Float64} + + final_s::Array{Float64} + final_xy::Array{Float64} + final_input::Array{Float64} + + optimal_inputs::Array{Float64} + predicted_xy::Array{Float64} + predicted_true_xy::Array{Float64} + predicted_s::Array{Float64} + prev_predicted_s::Array{Float64} + + # num_laps::Int64 + iterations_needed::Array{Int64} + # trajectories + # act as memory of all old states + trajectories_s::Array{Float64} + trajectories_xy::Array{Float64} + previous_inputs::Array{Float64} + # selected states + # act as memory of selected old states + selected_states_s::Array{Float64} + selected_states_xy::Array{Float64} + selected_states_cost::Array{Float64} + selected_laps::Array{Int64} + closest_indeces::Array{Int64} + + all_selected_states::Dict{Int64,Array{Tuple{Int64,UnitRange{Int64}},1}} + all_predictions::Array{Float64} + + num_loaded_laps::Int64 + color::AbstractString + weights_states::Array{Float64} + + updated_state::Bool + xy_predictions::xy_prediction + + state_initialized::Bool + blocked::Bool + + theta_vx::Array{Float64} + theta_vy::Array{Float64} + theta_psi_dot::Array{Float64} + + t_vx_log::Array{Float64} + t_vy_log::Array{Float64} + t_psidot_log::Array{Float64} + + time_log::Array{Float64} + + t_counter::Int64 + + thetas::theta + selection::selected_states + + dynamic_states::Array{Float64} + next_dynamics::Array{Float64} + loaded_sys_id_data::Bool + + time_inputs::Float64 + time_states::Float64 + + num_sessions::Int64 + current_session::Int64 + dynamics::Array{Float64} + counter::Array{Int64} + + acc::Float64 + + delay_a::Int64 + delay_df::Int64 + + not_for_selection::Array{Int64} + not_for_dynamics::Array{Int64} + + leading::Bool + + Agent() = new() +end + +function init!(agent::Agent, index::Int64, track::Track, + maximum_num_iterations::Int64, horizon::Int64, num_laps::Int64, + num_considered_states::Int64, num_loaded_laps::Int64, + v_max::Float64, color::AbstractString) + node_name = get_name() + agent.index = get_param(node_name * "/index") + # agent.index = index + agent.dt = 0.1 # TODO: define somewhere else!? maybe config file? + + agent.l_front = 0.125 + agent.l_rear = 0.125 + agent.width = 0.1 + + agent.input_lower_bound = [(- 0.6) (- pi / 3)] + agent.input_upper_bound = [0.6 (pi / 3)] + agent.state_s_lower_bound = [(- Inf) (- Inf) (- Inf) (- Inf) 0.0 0.0] + agent.state_s_upper_bound = [Inf Inf Inf Inf Inf Inf] + + agent.v_max = v_max + agent.max_slip_angle = 10 + + agent.mass = get_param(node_name * "/mass") + println("mass: ", agent.mass) + # agent.mass = 1.98 + agent.mu = 0.85 + agent.g = 9.81 + agent.I_z = 0.03 + agent.B = 6.0 + agent.C = 1.6 + + agent.is_dynamic = true + + # TODO: iterations_needed here or in simulator? + agent.iterations_needed = zeros(num_laps + num_loaded_laps) + agent.trajectories_s = zeros(num_laps + num_loaded_laps, + maximum_num_iterations, 6) + agent.trajectories_xy = zeros(num_laps + num_loaded_laps, + maximum_num_iterations, 6) + agent.previous_inputs = zeros(num_laps + num_loaded_laps, + maximum_num_iterations, 2) + + agent.selected_states_s = zeros(num_considered_states, 6) + agent.selected_states_xy = zeros(num_considered_states, 6) + agent.selected_states_cost = zeros(num_considered_states) + agent.selected_laps = zeros(NUM_CONSIDERED_LAPS) + agent.closest_indeces = zeros(NUM_CONSIDERED_LAPS) + + # agent.all_selected_states = Dict([i => (0, 1 : 10) for i = 1 : 6000]) + agent.all_selected_states = Dict([i => [(j, 1 : 10) for j = 1 : NUM_CONSIDERED_LAPS] for i = 1 : 6000]) + agent.all_predictions = zeros(6000, horizon + 1, 6) + + agent.num_loaded_laps = 0 + + agent.current_session = 1 + agent.num_sessions = 1 + agent.counter = ones(agent.num_sessions) + agent.dynamics = zeros(agent.num_sessions, 6000, 5) + + if MODE == "learning" + filename = ascii(get_most_recent(node_name[2 : end], "path_following", + INITIALIZATION_TYPE)) + # filename = "/home/lukas/simulations/" * get_name() * + # "_path_following_" * INITIALIZATION_TYPE * ".jld" + # filename = "/home/lukas/simulations/lmpc_test.jld" + println("LOADING: ", filename) + load_trajectories!(agent, filename) + elseif MODE == "racing" + #= + filename = ascii(get_most_recent(node_name[2 : end], "path_following", + INITIALIZATION_TYPE)) + println("LOADING: ", filename) + load_trajectories!(agent, filename) + =# + + filename = ascii(get_most_recent(node_name[2 : end], "path_following", + INITIALIZATION_TYPE)) + filename_center = ascii(get_most_recent(node_name[2 : end], "lmpc", + "center")) + filename_inner = ascii(get_most_recent(node_name[2 : end], "lmpc", + "inner")) + filename_outer = ascii(get_most_recent(node_name[2 : end], "lmpc", + "outer")) + println("LOADING: ", filename) + println("LOADING: ", filename_center) + println("LOADING: ", filename_inner) + println("LOADING: ", filename_outer) + + # files = [filename; filename_center] + files = [filename; filename_center; filename_inner; filename_outer] + load_trajectories!(agent, files) + end + + # x-y state: z_xy = [x y v_x v_y psi psi_dot] + agent.states_xy = zeros(maximum_num_iterations, 6) + # s-e_y state: z_s = [s e_y e_psi psi_dot v_x v_y] + agent.states_s = zeros(maximum_num_iterations, 6) + # input: u = [acceleration steering_angle] + agent.inputs = zeros(maximum_num_iterations, 2) + + # current state and input + agent.current_iteration = 1 + agent.current_input = zeros(1, 2) + agent.final_input = zeros(1, 2) + agent.final_s = zeros(1, 6) + agent.final_xy = zeros(1, 6) + + agent.optimal_inputs = zeros(horizon, 2) + agent.predicted_xy = zeros(horizon + 1, 6) + # agent.predicted_true_xy = zeros(horizon + 1, 6) + agent.predicted_s = zeros(horizon + 1, 6) + agent.prev_predicted_s = zeros(horizon + 1, 6) + + # set the velocity for the first state unequal to zero + # initial_psi = agent.states_xy[1, 5] + # agent.states_xy[1, 3 : 4] = 0.4 * [cos(initial_psi) sin(initial_psi)] + # make sure the states for xy and s are initialized equally! + # agent.states_s[1, :] = xy_to_s(track, agent.states_xy[1, :]) + # agent.states_true_s[1, 5] = 0.4 + + agent.color = get_param(node_name * "/color") + println("color: ", agent.color) + # agent.color = color + agent.current_lap = 1 + agent.weights_states = ones(num_considered_states) + + agent.updated_state = false + agent.xy_predictions = xy_prediction() + + agent.state_initialized = true + agent.blocked = false + + agent.theta_vx = zeros(Float64, 3) + agent.theta_vy = zeros(Float64, 4) + agent.theta_psi_dot = zeros(Float64, 3) + + agent.t_vx_log = zeros(6000, 3) + agent.t_vy_log = zeros(6000, 4) + agent.t_psidot_log = zeros(6000, 3) + + agent.time_log = zeros(6000) + + agent.t_counter = 0 + + agent.thetas = theta() + agent.selection = selected_states() + + agent.dynamic_states = zeros(1, 5) + agent.next_dynamics = zeros(1, 3) + agent.loaded_sys_id_data = false + + agent.time_inputs = 0.0 + agent.time_states = 0.0 + + agent.delay_a = get_param(node_name * "/delay_a") + agent.delay_df = get_param(node_name * "/delay_df") + + # agent.num_sessions = 1 + # agent.current_session = 1 + # TODO: Determine a good amount of saved dynamics. + # agent.dynamics = zeros(agent.num_sessions, 2000, 5) + # agent.counter = [1] + + agent.acc = 0.0 + agent.leading = false +end + +function get_state_xy(agent::Agent, iteration::Int64) + return agent.states_xy[iteration, :] +end + +function get_state_s(agent::Agent, iteration::Int64) + return agent.states_s[iteration, :] +end + +function get_current_s(agent::Agent) + return agent.states_s[agent.current_iteration, :] +end + +function get_current_xy(agent::Agent) + return agent.states_xy[agent.current_iteration, :] +end + +function get_state_true_s(agent::Agent, iteration::Int64) + return agent.states_true_s[iteration, :] +end + +function set_state_xy!(agent::Agent, xy_state::Array{Float64}, track::Track) + # TODO: only sets the initial state + agent.states_xy[1, :] = xy_state + agent.states_s[1, :] = xy_to_s(track, xy_state) +end + +function set_current_xy!(agent::Agent, xy_state::Array{Float64}, track::Track) + iteration = agent.current_iteration + agent.states_xy[iteration, :] = xy_state + agent.states_s[iteration, :] = xy_to_s(track, xy_state) +end + +function set_state_s!(agent::Agent, s_state::Array{Float64}, track::Track) + # TODO: only sets the initial state + s_state = get_s_coord(track, s_state) + agent.states_xy[1, :] = s_to_xy(track, s_state) + agent.states_s[1, :] = s_state +end + +function set_current_s!(agent::Agent, s_state::Array{Float64}, track::Track) + s_state = get_s_coord(track, s_state) + iteration = agent.current_iteration + agent.states_xy[iteration, :] = s_to_xy(track, s_state) + agent.states_s[iteration, :] = s_state +end + +function save_trajectories(agent::Agent, filename::ASCIIString) + println("SAVING") + num_recorded_laps = agent.num_loaded_laps + agent.current_lap - 1 + jldopen(filename, "w") do file + JLD.write(file, "trajectories_s", agent.trajectories_s[1 : num_recorded_laps, : , :]) + JLD.write(file, "trajectories_xy", agent.trajectories_xy[1 : num_recorded_laps, :, :]) + JLD.write(file, "previous_inputs", agent.previous_inputs[1 : num_recorded_laps, :, :]) + JLD.write(file, "dynamics", agent.dynamics) + JLD.write(file, "all_selected_states", agent.all_selected_states) + JLD.write(file, "all_predictions", agent.all_predictions) + JLD.write(file, "theta_vx", agent.t_vx_log) + JLD.write(file, "theta_vy", agent.t_vy_log) + JLD.write(file, "theta_psi_dot", agent.t_psidot_log) + JLD.write(file, "time", agent.time_log) + end +end + +function load_trajectories!(agent::Agent, filename::ASCIIString) + load_trajectories!(agent, [filename]) +end + +function load_trajectories!(agent::Agent, filenames::Array{ASCIIString}) + agent.num_sessions = length(filenames) + 1 + num_previously_loaded = 0 + agent.current_session = agent.num_sessions + agent.dynamics = zeros(agent.num_sessions, 6000, 5) + agent.counter = zeros(agent.num_sessions) + agent.counter[end] = 1 + + agent.not_for_selection = zeros(agent.num_sessions - 1) + agent.not_for_dynamics = zeros((agent.num_sessions - 1) * 2 + 1) + + num_laps = 0 + for filename in filenames + Data = load(filename) + cutoff = 0 + if contains(filename, "lmpc") + cutoff += 5 # Number of path following laps + end + num_laps += size(Data["trajectories_s"])[1] - cutoff + end + + num_laps += NUM_LAPS + + agent.trajectories_s = zeros(num_laps, MAXIMUM_NUM_ITERATIONS, 6) + agent.trajectories_xy = zeros(num_laps, MAXIMUM_NUM_ITERATIONS, 6) + agent.previous_inputs = zeros(num_laps, MAXIMUM_NUM_ITERATIONS, 2) + agent.iterations_needed = zeros(num_laps) + + index = 1 + for filename in filenames + println(filename) + Data = load(filename) + cutoff = 1 + if contains(filename, "lmpc") + cutoff += 5 # Number of path following laps + end + num_loaded_laps = size(Data["trajectories_s"])[1] + num_previously_loaded - cutoff + 1 + agent.trajectories_s[num_previously_loaded + 1 : num_loaded_laps, :, :] = Data["trajectories_s"][cutoff : end, :, :] + agent.trajectories_xy[num_previously_loaded + 1 : num_loaded_laps, :, :] = Data["trajectories_xy"][cutoff : end, :, :] + agent.previous_inputs[num_previously_loaded + 1 : num_loaded_laps, :, :] = Data["previous_inputs"][cutoff : end, :, :] + previous_session_dynamics = Data["dynamics"] + + agent.dynamics[index, :, :] = previous_session_dynamics[end, :, :] + agent.counter[index] = findmin(sumabs(agent.dynamics[index, :, :], 3))[2] + + agent.not_for_selection[index] = num_loaded_laps + agent.not_for_dynamics[(index - 1) * 2 + 1] = num_previously_loaded + 1 + agent.not_for_dynamics[(index - 1) * 2 + 2] = num_loaded_laps + + determine_needed_iterations!(agent, num_previously_loaded + 1 : num_loaded_laps) + num_previously_loaded = num_loaded_laps + index += 1 + end + + agent.not_for_dynamics[end] = num_previously_loaded + 1 + + println(agent.iterations_needed) + println(agent.not_for_selection) + println(agent.not_for_dynamics) + # NUM_LOADED_LAPS = num_previously_loaded + agent.num_loaded_laps = num_previously_loaded + end + + + +#= + function load_trajectories!(agent::Agent, filenames::Array{ASCIIString}) + num_previously_loaded = 0 + for filename in filenames + Data = load(filename) + num_loaded_laps = size(Data["trajectories_s"])[1] + num_previously_loaded + println(num_loaded_laps) + agent.trajectories_s[num_previously_loaded + 1: num_loaded_laps, :, :] = Data["trajectories_s"] + agent.trajectories_xy[num_previously_loaded + 1: num_loaded_laps, :, :] = Data["trajectories_xy"] + num_previously_loaded = num_loaded_laps + end + + determine_needed_iterations!(agent) + end +=# + +function select_states(agent::Agent, track::Track) + num_recorded_laps = NUM_LOADED_LAPS + agent.current_lap - 1 + + iteration = agent.current_iteration + prev_s = agent.predicted_s[1, 1] + current_s = agent.states_s[iteration, 1] + + # Check if we're already in the next lap + if abs(prev_s - current_s) >= 0.5 * track.total_length && iteration > 1 + num_recorded_laps += 1 + end + + possible_trajectory_list = nothing + closest_index = zeros(Int64, num_recorded_laps) + + horizon = size(agent.optimal_inputs)[1] + num_considered_states = size(agent.selected_states_s)[1] + num_buffer = NUM_STATES_BUFFER + + selected_laps = zeros(Int64, NUM_CONSIDERED_LAPS) + + needed_iteration_array = zeros(num_recorded_laps) + + # find the fastest lap + min_iterations = [size(agent.states_xy)[1], 0] + + # Do not select from the first overall lap, because there are no states, + # which could be appended in the beginning + for i = 1 : num_recorded_laps + if i in agent.not_for_selection + continue + end + + if agent.iterations_needed[i] == 0 && i == num_recorded_laps + needed_iterations = iteration + else + needed_iterations = agent.iterations_needed[i] + end + + if agent.trajectories_s[i, num_buffer + 1, 1] <= 2 * agent.dt * agent.v_max + needed_iteration_array[i] = needed_iterations + else + needed_iteration_array[i] = needed_iterations + Int64(ceil(2 * agent.dt * agent.v_max)) + end + + # println("needed_iterations: ", needed_iterations) + if needed_iterations == 0 + continue + end + + index_closest_state = findmin(abs(agent.trajectories_s[i, num_buffer + 1 : needed_iterations + num_buffer, 1] + - agent.states_s[iteration, 1]))[2] + num_buffer + + index_closest_state += SELECTION_SHIFT + closest_index[i] = Int64(index_closest_state) + + # determine reachability from current state + current_vel = sqrt(agent.states_s[iteration, 5]^2 + agent.states_s[iteration, 6]^2) + max_diff_vel = horizon * agent.dt * agent.input_upper_bound[1] + + #= + if MODE == "racing" && agent.current_lap == 2 + max_diff_vel = 0.3 + end + =# + + recorded_vel_ahead = sqrt(agent.trajectories_s[i, index_closest_state + horizon, 5]^2 + + agent.trajectories_s[i, index_closest_state + horizon, 6]^2) + + + # if MODE == "racing" + # v_max = 1.5 + # if any(agent.trajectories_s[i, index_closest_state + (0 : horizon - 1), 5] .> v_max) + # continue + # end + # end + + if current_vel - max_diff_vel <= recorded_vel_ahead && current_vel + + max_diff_vel >= recorded_vel_ahead + + travel_distance = horizon * agent.dt * (current_vel ) + + if travel_distance > abs(agent.trajectories_s[i, index_closest_state, 1] - agent.states_s[iteration, 1]) + if possible_trajectory_list == nothing + possible_trajectory_list = [i] + else + append!(possible_trajectory_list, [i]) + end + end + end + end + + if num_recorded_laps <= NUM_CONSIDERED_LAPS + possible_trajectory_list = collect(1 : num_recorded_laps) + possible_trajectory_list[1] = 2 + end + + if possible_trajectory_list == nothing + possible_trajectory_list = collect(num_recorded_laps : - 1 : 1) + for i = 1 : length(possible_trajectory_list) + if possible_trajectory_list[i] in agent.not_for_selection + possible_trajectory_list[i] = 2 + end + end + end + + @assert possible_trajectory_list != nothing + + sorted_iterations = sortperm(needed_iteration_array) + num_possible_trajectories = length(possible_trajectory_list) + + i, j = 1, 1 + while j <= NUM_CONSIDERED_LAPS + if num_possible_trajectories <= NUM_CONSIDERED_LAPS && j >= num_possible_trajectories + selected_laps[j] = possible_trajectory_list[end] + j += 1 + elseif sorted_iterations[i] in possible_trajectory_list + selected_laps[j] = sorted_iterations[i] + j += 1 + end + i += 1 + end + + if MODE == "learning" + if !(num_recorded_laps in selected_laps) + dummy_list = [num_recorded_laps] + append!(dummy_list, selected_laps[1 : NUM_CONSIDERED_LAPS - 1]) + possible_trajectory_list = dummy_list + end + end + + # Make sure, that certain laps are not selected. + if 1 in selected_laps && NUM_LOADED_LAPS > 0 + for i = 1 : size(selected_laps, 1) + if selected_laps[i] == 1 + selected_laps[i] = 2 + end + end + end + + if NUM_LOADED_LAPS > 0 && NUM_LOADED_LAPS in selected_laps + for i = 1 : size(selected_laps, 1) + if selected_laps[i] == NUM_LOADED_LAPS + selected_laps[i] = 2 + end + end + end + + println("selected_laps: ", selected_laps) + + min_iteration = round(Int64, findmin(agent.iterations_needed[selected_laps])[1]) + + counter = 1 + iteration_number = 1 + for i in selected_laps + agent.selected_laps[iteration_number] = i + agent.selected_states_s[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + squeeze(agent.trajectories_s[i, + closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1, :], + 1) + agent.selected_states_xy[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + squeeze(agent.trajectories_xy[i, + closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1, :], + 1) + + + # Lukas' cost + agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + collect((round(Int64, NUM_HORIZONS * horizon) : - 1 : 1) + agent.iterations_needed[i] - min_iteration) + + + + # # 1. new cost + # cost = collect(0 : - 1 : - (round(Int64, NUM_HORIZONS * horizon) - 1)) + agent.iterations_needed[i] - closest_index[i] + # if any(agent.predicted_s[:, 1] .> track.total_length) + # if i < NUM_LOADED_LAPS + agent.current_lap - 1 + # cost += agent.iterations_needed[i + 1] + # else + # # find state where finish line is crossed + # finish_line_crossed_idx = findmax(agent.predicted_s .> track.total_length)[2] + # cost += agent.current_iteration + finish_line_crossed_idx - 1 + # end + # end + # agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = cost + + #= + # 2. new cost + cost = collect(0 : - 1 : - (round(Int64, NUM_HORIZONS * horizon) - 1)) + agent.iterations_needed[i] - closest_index[i] + safe_set = squeeze(agent.trajectories_s[i, + closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1, :], + 1) + beyond_finish_line = safe_set[:, 1] .> track.total_length + cost[beyond_finish_line] = 0 + agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = cost + =# + + # Ugo's cost + #= + agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + collect(0 : - 1 : - (round(Int64, NUM_HORIZONS * horizon) - 1)) + agent.iterations_needed[i] - closest_index[i] + =# + + agent.all_selected_states[agent.counter[end]][iteration_number] = (i, closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1) + + counter += round(Int64, NUM_HORIZONS * horizon) + iteration_number += 1 + end + + agent.closest_indeces = closest_index[agent.selected_laps] + + for i = 1 : num_considered_states + if abs(agent.selected_states_s[i, 2]) > TRACK_WIDTH / 2 + agent.selected_states_cost[i] += 20 + end + end + + laps_for_sysid = selected_laps[1 : 2] + identify_system!(agent, laps_for_sysid) +end + + +function select_states_smartly(agent::Agent, adv_e_y::Float64, adv_s::Array{Float64}, track::Track) + num_recorded_laps = NUM_LOADED_LAPS + agent.current_lap - 1 + println("AGENT $(agent.index) CURRENT LAP: ", agent.current_lap) + println("AGENT $(agent.index) NUM RECORDED LAPS: ", num_recorded_laps) + println("AGENT $(agent.index) NUM LOADED LAPS: ", NUM_LOADED_LAPS) + + iteration = agent.current_iteration + prev_s = agent.predicted_s[1, 1] + current_s = agent.states_s[iteration, 1] + + # Check if we're already in the next lap + #= + if abs(prev_s - current_s) >= 0.5 * track.total_length && iteration > 1 + num_recorded_laps += 1 + end + =# + + horizon = size(agent.optimal_inputs)[1] + num_considered_states = size(agent.selected_states_s)[1] + num_buffer = NUM_STATES_BUFFER + + selected_laps = zeros(Int64, NUM_CONSIDERED_LAPS) + + needed_iteration_array = zeros(num_recorded_laps) + + possible_trajectory_list = nothing + avg_e_y = nothing + closest_index = zeros(Int64, num_recorded_laps) + + overtake_on_left = true + + # find the fastest lap + min_iterations = [size(agent.states_xy)[1], 0] + + current_e_y = agent.states_s[iteration, 2] + current_vel = agent.states_s[iteration, 5] + + adv_s_1 = adv_s[1] + adv_s_2 = adv_s[2] + + alpha = 4.0 + + if adv_e_y > current_e_y + if adv_e_y + track.width / 2.0 > alpha * agent.width + # add states on that side + # determine the interval + ey_max = min(adv_e_y, current_e_y + alpha / 2.0 * agent.width) + ey_min = - track.width / 2.0 + + interval = [ey_min ey_max] + overtake_on_left = true + else + ey_max = track.width / 2.0 + ey_min = adv_e_y + alpha / 2.0 * agent.width # max(adv_e_y - alpha * agent.width, - track.width) + interval = [ey_min ey_max] + overtake_on_left = false + end + else + if track.width / 2.0 - adv_e_y > alpha * agent.width + # add states on the other side + # determine the interval + ey_max = track.width / 2.0 + ey_min = max(adv_e_y, current_e_y - alpha / 2.0 * agent.width) + interval = [ey_min ey_max] + overtake_on_left = false + else + ey_max = adv_e_y - alpha / 2.0 * agent.width + ey_min = - track.width / 2.0 + interval = [ey_min ey_max] + overtake_on_left = true + end + end + + for i = 1 : num_recorded_laps + if i in agent.not_for_selection + continue + end + + if agent.iterations_needed[i] == 0 && i == num_recorded_laps + needed_iterations = iteration + else + needed_iterations = agent.iterations_needed[i] + end + + if agent.trajectories_s[i, num_buffer + 1, 1] <= 2 * agent.dt * agent.v_max + needed_iteration_array[i] = needed_iterations + else + needed_iteration_array[i] = needed_iterations + Int64(ceil(2 * agent.dt * agent.v_max)) + end + + index_closest_state = findmin(abs(agent.trajectories_s[i, num_buffer + 1 : needed_iterations + num_buffer, 1] + - agent.states_s[iteration, 1]))[2] + num_buffer + + index_closest_state += SELECTION_SHIFT + closest_index[i] = Int64(index_closest_state) + + # determine reachability from current state + current_vel = sqrt(agent.states_s[iteration, 5]^2 + agent.states_s[iteration, 6]^2) + max_diff_vel = horizon * agent.dt * agent.input_upper_bound[1] + recorded_vel_ahead = sqrt(agent.trajectories_s[i, index_closest_state + horizon, 5]^2 + + agent.trajectories_s[i, index_closest_state + horizon, 6]^2) + + # v_max = 1.5 + # if any(agent.trajectories_s[i, index_closest_state + (0 : horizon - 1), 5] .> v_max) + # continue + # end + + if current_vel - max_diff_vel <= recorded_vel_ahead && current_vel + + max_diff_vel >= recorded_vel_ahead + + travel_distance = horizon * agent.dt * current_vel + + if travel_distance > abs(agent.trajectories_s[i, index_closest_state, 1] - agent.states_s[iteration, 1]) + visited_s = agent.trajectories_s[i, index_closest_state : index_closest_state + 2 * horizon - 1, 1] + adv_index_1 = findmin(abs(visited_s - adv_s_1))[2] + adv_index_2 = findmin(abs(visited_s - adv_s_2))[2] + visited_e_ys = agent.trajectories_s[i, index_closest_state : index_closest_state + 2 * horizon - 1, 2] + visited_e_ys = visited_e_ys[:, adv_index_1 : adv_index_2, :] + mean_e_y = mean(visited_e_ys) + + tolerance = 0.0 + + if overtake_on_left + if any(visited_e_ys .> interval[2] + tolerance) + continue + end + else + if any(visited_e_ys .< interval[1] - tolerance) + continue + end + end + + if possible_trajectory_list == nothing + possible_trajectory_list = [i] + avg_e_y = [mean_e_y] + else + append!(possible_trajectory_list, [i]) + append!(avg_e_y, [mean_e_y]) + end + end + end + end + + if avg_e_y == nothing + # select_states(agent) + # println("STANDARD SELECTION") + laps_per_init = round(Int64,round(Int64, NUM_LOADED_LAPS - 5) / 3) + distance_to_pf = abs([EY_INNER, EY_CENTER, EY_OUTER] - adv_e_y) + selected_init = findmax(distance_to_pf)[2] + + if selected_init == 1 + selected_laps = 5 + 1 * laps_per_init + collect(1 : NUM_CONSIDERED_LAPS) + elseif selected_init == 2 + selected_laps = 5 + 0 * laps_per_init + collect(1 : NUM_CONSIDERED_LAPS) + elseif selected_init == 3 + selected_laps = 5 + 2 * laps_per_init + collect(1 : NUM_CONSIDERED_LAPS) + end + + # selected_laps = [2, 3, 4, 2] + println("CONSERVATIVE SELECTION") + else + println("SMART SELECTION") + num_possible_trajectories = length(possible_trajectory_list) + + sorted_iterations = sortperm(needed_iteration_array) + i, j = 1, 1 + while j <= NUM_CONSIDERED_LAPS + if num_possible_trajectories <= NUM_CONSIDERED_LAPS && j >= num_possible_trajectories + selected_laps[j] = possible_trajectory_list[end] + j += 1 + elseif sorted_iterations[i] in possible_trajectory_list + selected_laps[j] = sorted_iterations[i] + j += 1 + end + i += 1 + end + end + + #= + sorted_e_y = sortperm(avg_e_y) + min_max_e_y = [possible_trajectory_list[sorted_e_y[1]]; + possible_trajectory_list[sorted_e_y[end]]] + + if num_possible_trajectories >= NUM_CONSIDERED_LAPS + if overtake_on_left + possible_trajectory_list = possible_trajectory_list[sorted_e_y[1 : NUM_CONSIDERED_LAPS - 1]] + else + possible_trajectory_list = possible_trajectory_list[sorted_e_y[end : - 1 : end - (NUM_CONSIDERED_LAPS - 2)]] + end + else + possible_trajectory_list = possible_trajectory_list[randperm(num_possible_trajectories)] + end + + while length(possible_trajectory_list) < NUM_CONSIDERED_LAPS + append!(possible_trajectory_list, [possible_trajectory_list[randperm(length(possible_trajectory_list))[1]]]) + end + + =# + + # append!(possible_trajectory_list, [findmin(agent.iterations_needed)[2]]) + min_iteration = round(Int64, findmin(agent.iterations_needed[selected_laps])[1]) + + println("selection: ", selected_laps) + + counter = 1 + iteration_number = 1 + for i in selected_laps + agent.selected_laps[iteration_number] = i + println("Closest index: ", closest_index) + agent.selected_states_s[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + squeeze(agent.trajectories_s[i, + closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1, :], + 1) + agent.selected_states_xy[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + squeeze(agent.trajectories_xy[i, + closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1, :], + 1) + #= + agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + collect((round(Int64, NUM_HORIZONS * horizon) : - 1 : 1) + agent.iterations_needed[i] - min_iteration) + =# + + agent.selected_states_cost[counter : counter + round(Int64, NUM_HORIZONS * horizon) - 1, :] = + collect(0 : - 1 : - (round(Int64, NUM_HORIZONS * horizon) - 1)) + agent.iterations_needed[i] - closest_index[i] + + agent.all_selected_states[agent.counter[end]][iteration_number] = (i, closest_index[i] : closest_index[i] + round(Int64, NUM_HORIZONS * horizon) - 1) + + counter += round(Int64, NUM_HORIZONS * horizon) + iteration_number += 1 + end + + agent.closest_indeces = closest_index[agent.selected_laps] + + for i = 1 : num_considered_states + if abs(agent.selected_states_s[i, 2]) > TRACK_WIDTH / 2 + agent.selected_states_cost[i] += 20 + end + end + + laps_for_sysid = selected_laps[1 : 2] + identify_system!(agent, laps_for_sysid) +end + +function identify_system!(agent::Agent, laps::Array{Int64}) + num_states = 50 # number of states used for system identification + num_previous = SYS_ID_BEFORE + num_after = SYS_ID_AFTER + + num_buffer = NUM_STATES_BUFFER + + # Create the current dynamic state: [v_x v_y psi_dot acc steering] + current_iteration = agent.current_iteration + + # Mapping: [s, e_y, e_psi, psi_dot, v_x, v_y] --> [v_x v_y psi_dot] + mapping = [5, 6, 4] + # println(agent.states_s[current_iteration]) + # println(agent.current_input) + current_s = agent.states_s[current_iteration, 1] + v_x, v_y, psi_dot = agent.states_s[current_iteration, mapping] + + # TODO: Add Delay + # acc, steering = agent.current_input + previous_lap = agent.current_lap - 1 + NUM_LOADED_LAPS + if current_iteration <= agent.delay_a + acc = agent.previous_inputs[previous_lap, agent.iterations_needed[previous_lap] - agent.delay_a + current_iteration + num_buffer + 1, 1] + else + acc = agent.inputs[current_iteration - agent.delay_a, 1] + end + if current_iteration <= agent.delay_df + steering = agent.previous_inputs[previous_lap, agent.iterations_needed[previous_lap] - agent.delay_df + current_iteration + num_buffer + 1, 2] + else + steering = agent.inputs[current_iteration - agent.delay_df, 2] + end + + current_dynamics = [v_x, v_y, psi_dot, acc, steering] + # current_dynamics = squeeze(agent.dynamics[agent.num_sessions, max(agent.counter[agent.current_session] - 1, 1), :], (1, 2)) + # println("Current dynamics: ", current_dynamics) + + num_recorded_laps = agent.num_loaded_laps + agent.current_lap - 1 + # num_considered_states = size(agent.selected_states_s)[1] + + # Count the number of recorded states + total_recorded_states = current_iteration - 1 + + # Create the dynamics of all the recorded laps + for i = 1 : num_recorded_laps + total_recorded_states += agent.iterations_needed[i] + end + + summed_iterations = cumsum([1; agent.iterations_needed]) + + ####################################################################### + # Selection based on previous lap # + ####################################################################### + # Find closest states in terms of s + @assert NUM_LOADED_LAPS > 1 + + selected_dynamics = [] + + println("LAPS: ", laps) + + for lap in laps + println("LAP: ", lap) + # previous_lap = agent.current_lap - 1 + NUM_LOADED_LAPS + + if lap in agent.not_for_dynamics + lap = 2 # is always a complete path_following lap + end + + println("iterations needed: ", agent.iterations_needed[lap]) + + previous_indeces = (1 : agent.iterations_needed[lap]) + num_buffer + previous_s = agent.trajectories_s[lap, previous_indeces, 1] + + distance_to_s = colwise(Euclidean(), previous_s, [current_s]) + closest_index = findmin(distance_to_s)[2] + selected_indeces = collect((closest_index - num_previous : closest_index + num_after) + num_buffer) + + dynamic_states = squeeze(agent.trajectories_s[lap, :, mapping], 1) + previous_acc = squeeze(agent.previous_inputs[lap, :, 1], 1) + previous_acc = [zeros(agent.delay_a); previous_acc[1 : end - agent.delay_a]] + previous_steering = squeeze(agent.previous_inputs[lap, :, 2], 1) + previous_steering = [zeros(agent.delay_df); previous_steering[1 : end - agent.delay_df]] + + previous_inputs = [previous_acc previous_steering] + dynamic_states = [dynamic_states previous_inputs] + + if size(selected_dynamics, 1) == 0 + selected_dynamics = dynamic_states[selected_indeces, :] + next_dynamics = dynamic_states[selected_indeces + 1, 1 : 3] + else + selected_dynamics = [selected_dynamics; dynamic_states[selected_indeces, :]] + next_dynamics = [next_dynamics; dynamic_states[selected_indeces + 1, 1 : 3]] + end + end + + current_lap = agent.current_lap + NUM_LOADED_LAPS + previous_iteration = agent.current_iteration - 1 + + if current_lap in agent.not_for_dynamics + current_lap = 2 # is always a complete path_following lap + end + + if false # using current lap for system identification or not + indeces = collect((max(1, previous_iteration - SYS_ID_AFTER) : previous_iteration - 1) + num_buffer) + + previous_dynamic_states = squeeze(agent.trajectories_s[current_lap, :, mapping], 1) + previous_acc = squeeze(agent.previous_inputs[current_lap, :, 1], 1) + previous_acc = [zeros(agent.delay_a); previous_acc[1 : end - agent.delay_a]] + previous_steering = squeeze(agent.previous_inputs[current_lap, :, 2], 1) + previous_steering = [zeros(agent.delay_df); previous_steering[1 : end - agent.delay_df]] + + previous_inputs = [previous_acc previous_steering] + previous_dynamic_states = [previous_dynamic_states previous_inputs] + + selected_dynamics = [selected_dynamics; previous_dynamic_states[indeces, :]] + next_dynamics = [next_dynamics; previous_dynamic_states[indeces + 1, 1 : 3]] + end + + history_vx = selected_dynamics[:, 1] + history_vy = selected_dynamics[:, 2] + history_psi_dot = selected_dynamics[:, 3] + history_acc = selected_dynamics[:, 4] + history_steering = selected_dynamics[:, 5] + + # Create vectors for linear regression + delta_vx = next_dynamics[:, 1] - history_vx + delta_vy = next_dynamics[:, 2] - history_vy + delta_psi_dot = next_dynamics[:, 3] - history_psi_dot + + # println("Delta_Vx: ", delta_vx) + + vy_over_vx = history_vy ./ history_vx + psi_dot_over_vx = history_psi_dot ./ history_vx + vy_times_psi_dot = history_vy .* history_psi_dot + vx_times_psi_dot = history_vx .* history_psi_dot + + # println("vy_times_psi_dot: ", vy_times_psi_dot) + + # Create matrices for linear regression + X_vx = [vy_times_psi_dot history_vx history_acc] + # X_vx = [history_vx history_acc] + X_vy = [vy_over_vx vx_times_psi_dot psi_dot_over_vx history_steering] + X_psi_dot = [psi_dot_over_vx vy_over_vx history_steering] + + # linear regression + + dummy_vx = zeros(agent.theta_vx) + dummy_vy = zeros(agent.theta_vy) + dummy_psi_dot = zeros(agent.theta_psi_dot) + + try + # agent.theta_vx = inv(X_vx' * X_vx + 1.0 * eye(X_vx' * X_vx)) * X_vx' * delta_vx + dummy_vx = inv(X_vx' * X_vx) * X_vx' * delta_vx + # agent.theta_vx = [1.0 agent.theta_vx] + catch + println("history vx: ", history_vx) + println("Test: ", X_vx) + println("Singular Xvx: ", X_vx' * X_vx) + exit() + end + + try + # agent.theta_vy = inv(X_vy' * X_vy + 1.0 * eye(X_vy' * X_vy)) * X_vy' * delta_vy + dummy_vy = inv(X_vy' * X_vy) * X_vy' * delta_vy + catch + println("Singular Xvy: ", X_vy' * X_vy) + exit() + end + + try + # agent.theta_psi_dot = inv(X_psi_dot' * X_psi_dot + 1.0 * eye(X_psi_dot' * X_psi_dot)) * X_psi_dot' * delta_psi_dot + dummy_psi_dot = inv(X_psi_dot' * X_psi_dot) * X_psi_dot' * delta_psi_dot + catch + println("Singular X_psi_dot: ", X_psi_dot' * X_psi_dot) + exit() + end + + if any(isnan(dummy_vy)) || any(isnan(dummy_psi_dot)) + println("Not updating thetas.") + println("selected_dynamics: ", selected_dynamics) + exit() + else + agent.theta_vx = dummy_vx + agent.theta_vy = dummy_vy + agent.theta_psi_dot = dummy_psi_dot + end + + # println("THETA VX: ", agent.theta_vx) + # println("THETA VY: ", agent.theta_vy) + # println("THETA PSI DOT: ", agent.theta_psi_dot) +end + +function determine_needed_iterations!(agent::Agent, indeces) + num_buffer = NUM_STATES_BUFFER + + println(agent.iterations_needed) + + for i = indeces + agent.iterations_needed[i] = findmin(sumabs(agent.trajectories_s[i, :, :], (1, 3)))[2] - 1 - 2 * num_buffer + # No data has been appended to the last lap yet. + if i == indeces[end] + agent.iterations_needed[i] += num_buffer + end + # No data has been appended to the first lap yet. + if i == indeces[1] + agent.iterations_needed[i] += num_buffer + if agent.iterations_needed[i] < 1 + agent.iterations_needed[i] = findmin(sumabs(agent.trajectories_s[i, num_buffer + 1 : end, :], (1, 3)))[2] - 1 - num_buffer + end + end + end +end + +function determine_needed_iterations!(agent::Agent) + determine_needed_iterations!(agent, 1 : agent.num_loaded_laps) +end \ No newline at end of file diff --git a/workspace/src/barc/src/agent_node.jl b/workspace/src/barc/src/agent_node.jl new file mode 100755 index 00000000..41f6a571 --- /dev/null +++ b/workspace/src/barc/src/agent_node.jl @@ -0,0 +1,640 @@ +#!/usr/bin/env julia + +#= + File name: agent_node.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + + +using RobotOS +@rosimport barc.msg: ECU, pos_info, Vel_est, prediction, xy_prediction, theta, selected_states +@rosimport sensor_msgs.msg : Imu +@rosimport marvelmind_nav.msg : hedge_pos +@rosimport std_msgs.msg : Header +rostypegen() + +using barc.msg +using sensor_msgs.msg +using marvelmind_nav.msg +using std_msgs.msg + +include("config_2.jl") +include("track.jl") +include("transformations.jl") +include("agent.jl") +# include("estimator.jl") +include("optimizer.jl") +include("low_level_controller.jl") +include("mpc_test.jl") + + +function race(num_laps::Int64) + # Create strings for the current date + hour_minute_second = Libc.strftime("%H%M%S", time()) + month_day = Libc.strftime("%m%d", time()) + + # Initialize ROS node" + init_node("agent_node") + # init_node("agent_node_" * string(agent.index)) + node_name = get_name() + + # Initialization + init_states = INIT_STATES + v_max = V_MAX[1] + color = COLOR[1] + + track = Track() + init!(track) + + agent = Agent() + init!(agent, 1, track, MAXIMUM_NUM_ITERATIONS, HORIZON, NUM_LAPS, + NUM_CONSIDERED_STATES, NUM_LOADED_LAPS, v_max, color) + set_state_s!(agent, init_states[1, :], track) + + xy_pub = Publisher("xy_prediction", xy_prediction, + queue_size=1)::RobotOS.Publisher{barc.msg.xy_prediction} + + if get_param(node_name * "/sim") == "simulation" + xy_sub = Subscriber("real_val", pos_info, xy_callback, (agent, track), + queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + else + xy_sub = Subscriber("pos_info", pos_info, xy_callback, (agent, track), + queue_size=1)::RobotOS.Subscriber{barc.msg.pos_info} + end + + theta_pub = Publisher("theta", theta, queue_size=1)::RobotOS.Publisher{barc.msg.theta} + selection_pub = Publisher("selected_states", selected_states, queue_size=1)::RobotOS.Publisher{barc.msg.selected_states} + + #= + estimator = StateEstimator() + init!(estimator, agent.dt) + =# + + #= + identifier = SystemIdentifier() + init!(identifier) + =# + + optimizer = Optimizer() + init!(optimizer, agent, HORIZON) + + mpc_model_pf = MpcModel_pF(agent, track) + if NUM_AGENTS == 1 + mpc_model_convhull = MpcModel_convhull(agent, track) + println("Succesfully initialized convhull model.") + elseif NUM_AGENTS == 2 + mpc_model_obstacle = MpcModel_obstacle(agent, track) + println("Succesfully initialized obstacle model.") + else + error("Too many agents.") + end + + # Create an Optimizer object for the first path following lap of the LMPC + #= + optimizer_path_following = nothing + if LEARNING + optimizer_path_following = Optimizer() + init!(optimizer_path_following, agent, HORIZON) + end + =# + + input_pub = Publisher("ecu", ECU, + queue_size=1)::RobotOS.Publisher{barc.msg.ECU} + + low_level_controller = LowLevelController() + init!(low_level_controller) + + if LEARNING + select_states(agent, track) + if NUM_AGENTS == 1 + solveMpcProblem_convhull(mpc_model_convhull, optimizer, agent, track) + else + solveMpcProblem_obstacle(mpc_model_obstacle, optimizer, agent, track) + end + + # Warm start optimizer + max_acc = agent.input_upper_bound[1] + warm_start_input = repmat([max_acc 0.0], HORIZON) + else + # init_path_following!(optimizer, track) + end + + # Add states from the last loaded lap to the current lap + current_lap = agent.current_lap + + println("Sleeping for ", get_param(node_name * "/time_offset"), " seconds.") + rossleep(get_param(node_name * "/time_offset")) + + for i = 1 : num_laps + println("Lap: ", i) + + # Do path following for the first lap, if LMPC is used. + if LEARNING && i <= NUM_PF_LAPS + race_lap!(agent, optimizer, low_level_controller, + track, xy_pub, theta_pub, selection_pub, input_pub, mpc_model_pf) + # Make sure the same agent object is used after the first path + # following lap has been completed + # optimizer.agent = optimizer_path_following.agent + # setvalue(optimizer.inputs, getvalue(optimizer_path_following.inputs)) + elseif !LEARNING + race_lap!(agent, optimizer, low_level_controller, + track, xy_pub, theta_pub, selection_pub, input_pub, mpc_model_pf) + else + if MODE == "learning" + race_lap!(agent, optimizer, low_level_controller, track ,xy_pub, + theta_pub, selection_pub, input_pub, mpc_model_convhull) + elseif MODE == "racing" + race_lap!(agent, optimizer, low_level_controller, track ,xy_pub, + theta_pub, selection_pub, input_pub, mpc_model_obstacle) + end + end + end + + # Set motor to neutral on shutdown. Stopping the Barc. + neutralize!(low_level_controller) + + # Create a folder for every day, if it does not exist yet: + cd("/home/mpcubuntu/simulations/lukas") # move into simulations folder + try + cd(month_day) + catch + println("Creating a new folder for today: ", month_day) + mkdir(month_day) + end + + node_name = (get_name())[2 : end] + if MODE == "path_following" + save_trajectories(agent, "/home/mpcubuntu/simulations/lukas/" * month_day * "/" * + hour_minute_second * "_" * node_name * + "_path_following_" * INITIALIZATION_TYPE * ".jld") + # save_trajectories(agent, "/home/lukas/simulations/lmpc.jld") + elseif MODE == "learning" + save_trajectories(agent, "/home/mpcubuntu/simulations/lukas/" * month_day * "/" * + hour_minute_second * "_" * node_name * + "_lmpc_" * INITIALIZATION_TYPE * ".jld") + # save_trajectories(agent, "/home/lukas/simulations/lmpc_2.jld") + elseif MODE == "racing" + save_trajectories(agent, "/home/mpcubuntu/simulations/lukas/" * month_day * "/" * + hour_minute_second * "_" * node_name * + "_racing_" * INITIALIZATION_TYPE * ".jld") + else + error("MODE $(MODE) is not defined. Please choose one of the following: + path_following, learning or racing.") + end +end + + +function xy_callback(msg::pos_info, agent::Agent, track::Track) + if !agent.blocked + current_xy = zeros(6) + current_xy[1] = msg.x + current_xy[2] = msg.y + current_xy[3] = msg.v_x + current_xy[4] = msg.v_y + current_xy[5] = msg.psi + current_xy[5] = atan2(sin(msg.psi), cos(msg.psi)) + current_xy[6] = msg.psiDot + set_current_xy!(agent, current_xy, track) + agent.state_initialized = true + + current_time = to_sec(get_rostime()) + agent.time_states = current_time + end +end + +function publish_inputs(optimizer::Optimizer, input_pub, agent::Agent) + current_time = to_sec(get_rostime()) + optimizer.first_input.motor = optimizer.solution_inputs[1, 1] + optimizer.first_input.servo = optimizer.solution_inputs[1, 2] + # println(optimizer.solution_inputs) + publish(input_pub, optimizer.first_input) + agent.time_inputs = current_time +end + + +function race_lap!(agent::Agent, optimizer::Optimizer, + low_level_controller::LowLevelController, track::Track, + xy_pub, theta_pub, selection_pub, input_pub, mpc_model) + # set node rate + loop_rate = Rate(1 / agent.dt) + + leading_index = 0 + following_index = 0 + + iteration = 1 + finished = false + + prev_s = agent.states_s[1, 1] + current_s = agent.states_s[1, 1] + + num_loaded_laps = NUM_LOADED_LAPS + current_lap = agent.current_lap + lap_index = current_lap + num_loaded_laps + # num_considered_states = size(agent.selected_states_s)[1] + num_buffer = NUM_STATES_BUFFER + considered_indeces = 1 : num_buffer + shifted_s = ([track.total_length 0 0 0 0 0] .* ones(num_buffer, 6)) + single_shifted_s = ([track.total_length 0 0 0 0 0] .* ones(1, 6)) + + mapping = [5, 6, 4] + + while !is_shutdown() && iteration <= MAXIMUM_NUM_ITERATIONS && !finished + lap_index = current_lap + num_loaded_laps + agent.iterations_needed[lap_index] = agent.current_iteration - 1 + + # Get ROS time. Don't set to zero, because needs to be comparable to + # the other agent. + time_now = to_sec(get_rostime()) + agent.time_log[agent.counter[end]] = time_now + + current_iteration = agent.current_iteration + + println("Iteration: ", current_iteration) + + # Make sure state is initialized, if not give more time for callbacks + while sumabs(agent.states_s[current_iteration, :]) < 1e-5 + rossleep(Duration(0.005)) + end + + if agent.states_s[current_iteration, 1] > track.total_length / 2 && current_lap == 1 && current_iteration < 20 + agent.states_s[current_iteration, 1] = - (track.total_length - agent.states_s[current_iteration, 1]) + end + + println("Velocity: ", agent.states_s[current_iteration, 5]) + + publish_inputs(optimizer, input_pub, agent) + # Convert and send the calculated steering commands + pwm_converter!(low_level_controller, agent.current_input[1], + agent.current_input[2]) + + agent.blocked = true + + race_iteration!(agent, optimizer, low_level_controller, track, xy_pub, + theta_pub, selection_pub, mpc_model) + + # Constantly add the current state to the end of the trajectory of the + # previous lap + if current_lap > 1 && current_iteration <= num_buffer + previous_lap = agent.current_lap - 1 + NUM_LOADED_LAPS + iterations_needed = agent.iterations_needed[previous_lap] + if current_lap == 2 && NUM_LOADED_LAPS == 0 + last_index = iterations_needed + current_iteration + else + last_index = num_buffer + iterations_needed + current_iteration + end + # Because the next lap already starts at iteration 2 + if current_iteration == 2 + agent.trajectories_s[previous_lap, last_index - 1, :] = agent.states_s[1, :] + single_shifted_s + agent.trajectories_xy[previous_lap, last_index - 1, :] = agent.states_xy[1, :] + agent.previous_inputs[previous_lap, last_index - 1, :] = agent.inputs[1, :] + end + agent.trajectories_s[previous_lap, last_index, :] = agent.states_s[current_iteration, :] + single_shifted_s + agent.trajectories_xy[previous_lap, last_index, :] = agent.states_xy[current_iteration, :] + agent.previous_inputs[previous_lap, last_index, :] = agent.inputs[current_iteration, :] + end + + # Only take the number of iterations minus the current + # iteration, because it is already part of the next lap. + lap_index = current_lap + num_loaded_laps + + if lap_index > num_loaded_laps + 1 && current_iteration <= num_buffer + # Add the last couple of states to the beginning of the next lap + needed_iterations = agent.iterations_needed[lap_index - 1] + iteration_index = needed_iterations + current_iteration + if current_iteration == 2 + agent.trajectories_s[lap_index, 1, :] = squeeze(agent.trajectories_s[lap_index - 1, iteration_index - 1, :], 1) - single_shifted_s + agent.trajectories_xy[lap_index, 1, :] = agent.trajectories_xy[lap_index - 1, iteration_index - 1, :] + agent.previous_inputs[lap_index, 1, :] = agent.previous_inputs[lap_index - 1, iteration_index - 1, :] + end + agent.trajectories_s[lap_index, current_iteration, :] = squeeze(agent.trajectories_s[lap_index - 1, iteration_index, :], 1) - single_shifted_s + agent.trajectories_xy[lap_index, current_iteration, :] = agent.trajectories_xy[lap_index - 1, iteration_index, :] + agent.previous_inputs[lap_index, current_iteration, :] = agent.previous_inputs[lap_index - 1, iteration_index, :] + end + + if LEARNING && agent.current_lap > NUM_PF_LAPS + agent.all_predictions[agent.counter[end], :, :] = agent.predicted_s + else + agent.all_predictions[agent.counter[end], :, [1, 2, 3, 5]] = agent.predicted_s + end + + if agent.counter[end] == 1 + agent.theta_vx = zeros(agent.theta_vx) + agent.theta_vy = zeros(agent.theta_vy) + agent.theta_psi_dot = zeros(agent.theta_psi_dot) + end + + agent.t_vx_log[agent.counter[end], :] = agent.theta_vx + agent.t_vy_log[agent.counter[end], :] = agent.theta_vy + agent.t_psidot_log[agent.counter[end], :] = agent.theta_psi_dot + + agent_counter = agent.counter[agent.current_session] + current_dynamics = [agent.states_s[current_iteration, mapping] agent.inputs[current_iteration, :]] + agent.dynamics[agent.num_sessions, agent_counter, :] = current_dynamics + agent.counter[agent.current_session] += 1 + + current_s = agent.states_s[current_iteration, 1] + + # Check if there is a big change in the s-coordinate, which + # indicates that the current lap has been terminated. + if abs(prev_s - current_s) >= 0.5 * track.total_length && + agent.current_iteration > 1 + + s_coords = agent.states_s[current_iteration, :] + agent.states_s[current_iteration, 1] = get_s_coord(track, s_coords[1]) + + final_s = agent.states_s[current_iteration, :] + final_xy = agent.states_xy[current_iteration, :] + final_input = agent.inputs[current_iteration, :] + + current_lap = agent.current_lap + println("Current Lap: ", current_lap) + + lap_index = current_lap + num_loaded_laps + agent.iterations_needed[lap_index] = agent.current_iteration - 1 + println("UPDATED INDEX FOR LAP: ", lap_index) + + # Reset states and inputs + agent.states_s = zeros(agent.states_s) + agent.states_xy = zeros(agent.states_xy) + agent.inputs = zeros(agent.inputs) + + # Initalize states and inputs for next lap + agent.states_s[1, :] = final_s + agent.states_xy[1, :] = final_xy + agent.inputs[1, :] = final_input + + if lap_index + 1 <= NUM_LAPS + num_loaded_laps + lap_index += 1 + shifted_iteration = 1 + num_buffer + agent.trajectories_s[lap_index, shifted_iteration, :] = agent.states_s[1, :] + agent.trajectories_xy[lap_index, shifted_iteration, :] = agent.states_xy[1, :] + agent.previous_inputs[lap_index, shifted_iteration, :] = agent.inputs[1, :] + end + + # Make sure there are more iterations in the lap than there saved + # states in the buffer + if agent.current_iteration < NUM_STATES_BUFFER + error("Too many states in the buffer. Reduce the buffer size.") + end + + agent.current_lap += 1 + + # Set iteration to 2, because the next lap has already been + # reached + agent.current_iteration = 2 + finished = true + else + current_iteration = agent.current_iteration + if current_lap == 1 && num_loaded_laps == 0 + # Do not shift if it is the overall first lap, because there + # is no data, which can be appended + shifted_iteration = current_iteration + else + shifted_iteration = current_iteration + num_buffer + end + + # Save actual trajectories and inputs. + agent.trajectories_s[lap_index, shifted_iteration, :] = agent.states_s[current_iteration, :] + agent.trajectories_xy[lap_index, shifted_iteration, :] = agent.states_xy[current_iteration, :] + agent.previous_inputs[lap_index, shifted_iteration, :] = agent.inputs[current_iteration, :] + end + + prev_s = agent.states_s[current_iteration, 1] + + if !finished + agent.current_iteration += 1 + iteration += 1 + end + + agent.state_initialized = false + agent.blocked = false + + rossleep(loop_rate) + end + + println("Finished lap") +end + + +function race_iteration!(agent::Agent, optimizer::Optimizer, + low_level_controller::LowLevelController, track::Track, + xy_pub, theta_pub, selection_pub, mpc_model) + distance = nothing + leading = false + + # println("I am solving agent $(agent.index)") + + current_iteration = agent.current_iteration + num_buffer = NUM_STATES_BUFFER + + # Reset the weights and the predicted states + # num_considered_states = size(agent.selected_states_s)[1] + agent.weights_states = ones(NUM_CONSIDERED_STATES) + agent.predicted_xy = zeros(agent.predicted_xy) + + # println("adv predictions: ", optimizer.adv_predictions_s) + + if LEARNING + # TODO: Figure out a different way to know how many agents there are + ####################################################################### + if NUM_AGENTS > 1 + current_s = agent.states_s[agent.current_iteration, 1] + adv_s = optimizer.adv_predictions_s[1, 1] + # distance = abs((agent.current_lap * track.total_length + current_s) - + # (optimizer.adv_current_lap * track.total_length + adv_s)) + distance = abs(current_s - adv_s) + if distance > track.total_length / 2.0 + distance = track.total_length - distance + end + println("DISTANCE: ", distance) + # if get_total_distance(simulator.optimizers[i], simulator.track.total_length) >= 0 + # println("direct distance on track: ", get_leading_agent_on_track(simulator, i)) + leading = get_leading_agent_on_track(track, agent, optimizer) + agent.leading = leading + println("Leading " * string(agent.index) * ": ", leading) + end + + publish_selection(selection_pub, agent) + + tic() + if agent.current_lap <= NUM_PF_LAPS + solveMpcProblem_pathFollow(mpc_model, optimizer, agent, track, + CURRENT_REFERENCE) + # solve_path_following!(optimizer, track, CURRENT_REFERENCE) + else + if NUM_AGENTS == 1 + select_states(agent, track) + # elseif distance > NUM_HORIZONS * HORIZON * track.ds || leading + elseif distance > 8.0 * track.ds || leading + chosen_lap = NUM_LOADED_LAPS + select_states(agent,track) + else + adv_agent_e_y = optimizer.adv_predictions_s[end, 2] + # adv_agent_e_y = adv_agent.predicted_s[end, 2] + + #= + adv_agent_v = optimizer.adv_predictions_s[end, 5] + if adv_agent_v > agent.states_s[current_iteration, 5] + select_states(agent) + else + select_states_smartly(agent, adv_agent_e_y, track) + adjust_convex_hull(agent, optimizer) + end + =# + + adv_s = optimizer.adv_predictions_s[[1, HORIZON + 1], 1] + select_states_smartly(agent, adv_agent_e_y, adv_s, track) + adjust_convex_hull(agent, optimizer) + end + + if NUM_AGENTS == 2 + solveMpcProblem_obstacle(mpc_model, optimizer, agent, track, leading) + elseif NUM_AGENTS == 1 + # println("Current s: ", agent.states_s[agent.current_iteration, :]) + solveMpcProblem_convhull(mpc_model, optimizer, agent, track) + end + end + toc() + else + # println("s now: ", agent.states_s[agent.current_iteration, :]) + # println("s previous: ", agent.states_s[max(agent.current_iteration - 1, 1), :]) + if agent.states_s[agent.current_iteration, :] == agent.states_s[max(agent.current_iteration - 1, 1), :] + println("=========================Same state used!!!!=======================") + end + solveMpcProblem_pathFollow(mpc_model, optimizer, agent, track, + CURRENT_REFERENCE) + # solve_path_following!(optimizer, track, CURRENT_REFERENCE) + end + + # Determines the optimal inputs and also states in s + agent.optimal_inputs = optimizer.solution_inputs + agent.predicted_s = optimizer.solution_states_s + # println("predicted s: ", agent.predicted_s) + + # Convert the prediction from s-coordinates to xy-coordinates for plotting + for j = 1 : HORIZON + 1 + agent.predicted_xy[j, :] = s_to_xy(track, agent.predicted_s[j, :]) + end + + publish_xy(xy_pub, agent) + publish_theta(theta_pub, agent) + + # Apply the first input + agent.inputs[current_iteration, :] = agent.optimal_inputs[1, :] + agent.current_input = agent.optimal_inputs[1, :] + # println("optimal inputs:", agent.optimal_inputs[:, :]) +end + +function publish_xy(pub_xy, agent) + agent.xy_predictions.x = agent.predicted_xy[:, 1] + agent.xy_predictions.y = agent.predicted_xy[:, 2] + agent.xy_predictions.psi = agent.predicted_xy[:, 5] + agent.xy_predictions.acc = agent.optimal_inputs[:, 1] + agent.xy_predictions.steering = agent.optimal_inputs[:, 2] + agent.xy_predictions.current_lap = agent.current_lap + publish(pub_xy, agent.xy_predictions) +end + +function publish_theta(pub_theta, agent) + agent.thetas.theta_vx = agent.theta_vx + agent.thetas.theta_vy = agent.theta_vy + agent.thetas.theta_psi_dot = agent.theta_psi_dot + publish(pub_theta, agent.thetas) +end + +function publish_selection(selection_pub, agent) + agent.selection.x = agent.selected_states_xy[:, 1] + agent.selection.y = agent.selected_states_xy[:, 2] + publish(selection_pub, agent.selection) +end + + +function get_num_loaded_laps(filename::ASCIIString) + Data = load(filename) + return size(Data["trajectories_s"])[1] +end + + +function get_num_loaded_laps(filenames::Array{ASCIIString}) + num_loaded_laps = 0 + for filename in filenames + Data = load(filename) + num_loaded_laps += size(Data["trajectories_s"])[1] + end + + return num_loaded_laps +end + + +function get_distance_on_track(agent::Agent, optimizer::Optimizer, track::Track) + # calculate the distance on the track + @assert NUM_AGENTS > 1 + track_length = track.total_length + s_coords = zeros(NUM_AGENTS) + + iteration = agent.current_iteration + # TODO: Assuming only two agents + s_coords[1] = agent.states[iteration, 1] + s_coords[2] = optimizer.adv_predictions[1, 1] + + distance = abs(sum(s_coords .* [1; - 1])) + + if distance > track_length / 2 + distance = track_length - distance + end + + return distance +end + + +function get_leading_agent_on_track(track::Track, agent::Agent, optimizer::Optimizer) + current_iteration = agent.current_iteration + current_s = agent.states_s[current_iteration, 1] + adv_s = optimizer.adv_predictions_s[1, 1] + track_length = track.total_length + + #= + if current_s <= track_length / 2 + if current_s > adv_s + return true + elseif current_s <= adv_s && adv_s <= current_s + track_length / 2 + return false + else + return true + end + else + if current_s < adv_s + return false + elseif adv_s <= current_s && adv_s > current_s - track_length / 2 + return true + else + return false + end + end + =# + + distance = current_s - adv_s + + if distance > 0 + if distance < track.total_length / 2.0 + return true + else + return false + end + else + if - distance < track.total_length / 2.0 + return false + else + return true + end + end + +end + + +# Start race() function +if !isinteractive() + race(NUM_LAPS) +end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl new file mode 100644 index 00000000..1c787d71 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/MPC_models.jl @@ -0,0 +1,1054 @@ +# THERE ARE TWO DIFFERENT CORRESPONDING mpc_solving FUNCTIONS FOR THIS MODEL +type MpcModel_pF + # Fields here provides a channel for JuMP model to communicate with data outside of it + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + c::Array{JuMP.NonlinearParameter,1} + z_Ref::Array{JuMP.NonlinearParameter,2} + + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + derivCost::JuMP.NonlinearExpression # cost for state and input change + costZ::JuMP.NonlinearExpression # cost to the reference state + controlCost::JuMP.NonlinearExpression # soft constraints cost + + uPrev::Array{JuMP.NonlinearParameter,2} + a_his::Array{JuMP.NonlinearParameter,1} + df_his::Array{JuMP.NonlinearParameter,1} + + function MpcModel_pF(mpcParams_pF::MpcParams,modelParams::ModelParams) + m = new() + # Model parameters + # dt = 0.04 # time step + dt = modelParams.dt + L_a = modelParams.l_A + L_b = modelParams.l_B + # u_lb = mpcParams_pF.u_lb + # u_ub = mpcParams_pF.u_ub + # z_lb = mpcParams_pF.z_lb + # z_ub = mpcParams_pF.z_ub + u_lb = [-1 -18/180*pi] + u_ub = [ 2 18/180*pi] + z_lb = [-Inf -Inf -Inf -0.5] # 1.s 2.ey 3.epsi 4.v + z_ub = [ Inf Inf Inf 3.0] # 1.s 2.ey 3.epsi 4.v + + c_f = modelParams.c_f # motor drag coefficient + # MPC prameters + N = mpcParams_pF.N + Q = mpcParams_pF.Q + R = mpcParams_pF.R + QderivZ = mpcParams_pF.QderivZ + QderivU = mpcParams_pF.QderivU + # Problem specific parameters + v_ref = mpcParams_pF.vPathFollowing + + # Create Model + mdl = Model(solver = IpoptSolver(print_level=0,linear_solver="ma27")) #,max_cpu_time=0.09))#,linear_solver="ma57",print_user_options="yes")) + + # Create variables (these are going to be optimized) + @variable( mdl, z_Ol[1:(N+1),1:4], start = 0) # z = s, ey, epsi, v + @variable( mdl, u_Ol[1:N,1:2], start = 0) + + # Set bounds + z_lb_4s = ones(N+1,1)*z_lb # lower bounds on states: hard constraints + z_ub_4s = ones(N+1,1)*z_ub # upper bounds on states: hard constraints + u_lb_4s = ones(N,1) *u_lb # lower bounds on steering: hard constraints + u_ub_4s = ones(N,1) *u_ub # upper bounds on steering: hard constraints + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_4s[j,i]) + setupperbound(u_Ol[j,i], u_ub_4s[j,i]) + end + end + # path follower is really conservative controller regarding speed constraint, + # so there is actully no need to set more constaints for states + + # Nonlinear parameters initialization + @NLparameter(mdl, z_Ref[1:N+1,1:4]==0); setvalue(z_Ref,hcat(zeros(N+1,3),v_ref*ones(N+1,1))) + @NLparameter(mdl, z0[i=1:4]==0); setvalue(z0[4],v_ref) # initial speed for first initial solution + @NLparameter(mdl, uPrev[1:N,1:2]==0) + @NLparameter(mdl, df_his[1:mpcParams_pF.delay_df] == 0) + @NLparameter(mdl, a_his[1:mpcParams_pF.delay_a] == 0) + @NLparameter(mdl, zPrev[1:N+1,1:4]==0) + @NLparameter(mdl, c[1:N+1]==0) + + # System dynamics + @NLconstraint(mdl, [i=1:4], z_Ol[1,i] == z0[i]) # initial condition + # @NLconstraint(mdl, u_Ol[1,1] == uPrev[2,1]) + @NLconstraint(mdl, [i=1:mpcParams_pF.delay_a], u_Ol[i,1] == a_his[i]) # steering delay is 1 for simulation and 3 for experiment + + @NLconstraint(mdl, [i=1:mpcParams_pF.delay_df], u_Ol[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + for i=1:N + @NLexpression(mdl, bta[i],atan( L_a / (L_a + L_b) * tan(u_Ol[i,2]))) + @NLexpression(mdl, dsdt[i], z_Ol[i,4]*cos(z_Ol[i,3]+bta[i])/(1-z_Ol[i,2]*c[i])) + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*dsdt[i] ) # s + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt*z_Ol[i,4]*sin(z_Ol[i,3]+bta[i]) ) # ey + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt*(z_Ol[i,4]/L_a*sin(bta[i])-dsdt[i]*c[i]) ) # epsi + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(u_Ol[i,1] - c_f*z_Ol[i,4])) # v + end + + ###### ------ Cost definitions ------ ###### + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2,i=1:N}),j=1:4} + + sum{QderivU[j]*(sum{(u_Ol[i,j]-u_Ol[i+1,j])^2,i=1:N-1}+(uPrev[1,j]-u_Ol[1,j])^2),j=1:2}) + # Control Input cost + @NLexpression(mdl, controlCost, 0.5*sum{R[1]*sum{(u_Ol[i,1])^2,i=1:N},j=1:2}) + # State cost + @NLexpression(mdl, costZ, 0.5*sum{Q[j]*sum{(z_Ol[i,j]-z_Ref[i,j])^2,i=2:N+1},j=1:4}) + + # Objective function + @NLobjective(mdl, Min, costZ + derivCost + controlCost) + + m.mdl=mdl + m.z0=z0; m.z_Ref=z_Ref; m.c=c; + m.z_Ol=z_Ol; m.u_Ol = u_Ol + m.uPrev=uPrev + m.df_his = df_his + m.a_his = a_his + + m.derivCost=derivCost; m.costZ=costZ; m.controlCost=controlCost + return m + end +end + +type MpcModel_convhull_kin + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + c::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + + eps_lane::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + derivCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + slackS::JuMP.NonlinearExpression + slackEy::JuMP.NonlinearExpression + slackEpsi::JuMP.NonlinearExpression + slackV::JuMP.NonlinearExpression + + a_his::Array{JuMP.NonlinearParameter,1} + df_his::Array{JuMP.NonlinearParameter,1} + + GP_e_vy::Array{JuMP.NonlinearParameter,1} + GP_e_psidot::Array{JuMP.NonlinearParameter,1} + + function MpcModel_convhull_kin(mpcParams::MpcParams,modelParams::ModelParams,selectedStates::SelectedStates) + m = new(); n_state=4 + #### Initialize parameters + # dt = modelParams.dt # time step + dt = 0.04 # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + # u_lb = mpcParams.u_lb + # u_ub = mpcParams.u_ub + # z_lb = mpcParams.z_lb + # z_ub = mpcParams.z_ub + u_lb = [-2 -18/180*pi] + u_ub = [ 2 18/180*pi] + z_lb = [-Inf -Inf -Inf -0.5] # 1.s 2.ey 3.epsi 4.v + z_ub = [ Inf Inf Inf 6.0] # 1.s 2.ey 3.epsi 4.v + c_f = modelParams.c_f + + ey_max = get_param("ey")*get_param("ey_tighten")/2 + + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q_term_cost= mpcParams.Q_term_cost::Float64 # weights on terminal cost + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + Q_slack = mpcParams.Q_slack + + Np = selectedStates.Np::Int64 # how many states to select + Nl = selectedStates.Nl::Int64 # how many previous laps to select + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.02,linear_solver="ma27")) #,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @variable( mdl, z_Ol[1:(N+1),1:n_state]) + @variable( mdl, u_Ol[1:N,1:2]) + @variable( mdl, eps_lane[1:N+1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + + # Set bounds + z_lb_4s = ones(N+1,1)*z_lb # lower bounds on states: hard constraints + z_ub_4s = ones(N+1,1)*z_ub # upper bounds on states: hard constraints + u_lb_4s = ones(N,1) *u_lb # lower bounds on steering: hard constraints + u_ub_4s = ones(N,1) *u_ub # upper bounds on steering: hard constraints + + for i=1:2 + for j=1:N + setlowerbound(u_Ol[j,i], u_lb_4s[j,i]) + setupperbound(u_Ol[j,i], u_ub_4s[j,i]) + end + end + for i=1:n_state + for j=1:N+1 + setlowerbound(z_Ol[j,i], z_lb_4s[j,i]) + setupperbound(z_Ol[j,i], z_ub_4s[j,i]) + end + end + + @NLparameter(mdl, z0[i=1:n_state] == 0) + @NLparameter(mdl, c[1:N]==0) + @NLparameter(mdl, uPrev[1:N,1:2] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:n_state] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + @NLparameter(mdl, df_his[1:mpcParams.delay_df] == 0) + @NLparameter(mdl, a_his[1:mpcParams.delay_a] == 0) + + @NLparameter(mdl, GP_e_vy[i=1:N] == 0) + @NLparameter(mdl, GP_e_psidot[i=1:N] == 0) + + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] >= -ey_max - eps_lane[i]) + @NLconstraint(mdl, sum{alpha[i], i=1:Nl*Np} == 1) # constraint on the coefficients of the convex hull + + # @NLconstraint(mdl, [i=1:mpcParams.delay_a], u_Ol[i,1] == a_his[i]) # steering delay is 1 for simulation and 3 for experiment + # @NLconstraint(mdl, [i=1:mpcParams.delay_df], u_Ol[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + @NLexpression(mdl, bta[i=1:N], atan( L_b / (L_a + L_b) * tan(u_Ol[i,2]))) + @NLexpression(mdl, dsdt[i=1:N], z_Ol[i,4]*cos(z_Ol[i,3]+bta[i])/(1-z_Ol[i,2]*c[i])) + + # System dynamics + @NLconstraint(mdl, [i=1:4], z_Ol[1,i] == z0[i]) # initial condition + for i=1:N + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt*dsdt[i]) # s + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt*z_Ol[i,4]*sin(z_Ol[i,3]+bta[i]) ) # ey + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt*(z_Ol[i,4]/L_b*sin(bta[i])-dsdt[i]*c[i]) ) # epsi + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt*(u_Ol[i,1] - c_f*z_Ol[i,4])) # v + end + + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.12) + @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.12) + for i=1:N-1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.12) + @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.12) + end + + # for i=1:4 + # @NLconstraint(mdl, z_Ol[N+1,i] == sum(alpha[j]*selStates[j,i] for j=1:Nl*Np)) + # end + + # Cost functions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2 , i=1:N}) , j=1:4} + + sum{QderivU[j]*(sum{(u_Ol[i,j]-u_Ol[i+1,j])^2 , i=1:N-1}+(uPrev[1,j]-u_Ol[1,j])^2) , j=1:2}) + # Lane cost (soft) + @NLexpression(mdl, laneCost, Q_lane*sum{1.0*eps_lane[i]+20.0*eps_lane[i]^2 , i=2:N+1}) + # Terminal Cost + @NLexpression(mdl, terminalCost , Q_term_cost*sum{alpha[i]*statesCost[i] , i=1:Nl*Np}) + #---------------------------------- + # Slack cost on s + @NLexpression(mdl, slackS, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1] , j=1:Nl*Np})^2) + # Slack cost on ey + @NLexpression(mdl, slackEy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2] , j=1:Nl*Np})^2) + # Slack cost on ePsi + @NLexpression(mdl, slackEpsi, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3] , j=1:Nl*Np})^2) + # Slack cost on v + @NLexpression(mdl, slackV, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4] , j=1:Nl*Np})^2) + + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackS + Q_slack[2]*slackEy + Q_slack[3]*slackEpsi + Q_slack[4]*slackV) #+ Q_slack[5]*slackEy + Q_slack[6]*slackS) #+ controlCost + + m.mdl = mdl + m.z0 = z0 + m.c = c + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.uPrev = uPrev + + m.derivCost = derivCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + m.eps_lane = eps_lane + + m.slackS = slackS + m.slackEy = slackEy + m.slackEpsi = slackEpsi + m.slackV = slackV + + m.a_his = a_his + m.df_his = df_his + + m.GP_e_vy = GP_e_vy + m.GP_e_psidot = GP_e_psidot + + return m + end +end + +type MpcModel_convhull_dyn_iden + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + # coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,2} + c_Vy::Array{JuMP.NonlinearParameter,2} + c_Psi::Array{JuMP.NonlinearParameter,2} + uPrev::Array{JuMP.NonlinearParameter,2} + df_his::Array{JuMP.NonlinearParameter,1} + GP_e_vy::Array{JuMP.NonlinearParameter,1} + GP_e_psidot::Array{JuMP.NonlinearParameter,1} + + eps_lane::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearParameter,1} + + derivCost::JuMP.NonlinearExpression + # controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + # slackVx::JuMP.NonlinearExpression + # slackVy::JuMP.NonlinearExpression + # slackPsidot::JuMP.NonlinearExpression + # slackEpsi::JuMP.NonlinearExpression + # slackEy::JuMP.NonlinearExpression + # slackS::JuMP.NonlinearExpression + + function MpcModel_convhull_dyn_iden(mpcParams::MpcParams,modelParams::ModelParams,selectedStates::SelectedStates) + m = new() + n_state=6; n_input=2 + #### Initialize parameters + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + # u_lb = mpcParams.u_lb # lower bounds for the control inputs + # u_ub = mpcParams.u_ub # upper bounds for the control inputs + # z_lb = mpcParams.z_lb # lower bounds for the states + # z_ub = mpcParams.z_ub # upper bounds for the states + ey_max = get_param("ey")*get_param("ey_tighten")/2 + u_lb = [ -0.5 -18/180*pi] + u_ub = [ 3.0 18/180*pi] + z_lb = [-Inf -Inf -Inf 0 -Inf -Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + z_ub = [ Inf Inf Inf 6.0 Inf Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + Q_term_cost = mpcParams.Q_term_cost + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + Q_slack = mpcParams.Q_slack + + Np = selectedStates.Np + Nl = selectedStates.Nl + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09)) #,linear_solver="ma27"))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + # + @variable( mdl, z_Ol[1:(N+1),1:n_state], start=0.1) + @variable( mdl, u_Ol[1:N,1:n_input]) + @variable( mdl, eps_lane[1:N] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + + for j=1:N + for i=1:n_input + setlowerbound(u_Ol[j,i], u_lb[i]) + setupperbound(u_Ol[j,i], u_ub[i]) + end + for i=1:n_state + setlowerbound(z_Ol[j+1,i], z_lb[i]) + setupperbound(z_Ol[j+1,i], z_ub[i]) + end + end + for i=1:n_state + setlowerbound(z_Ol[N+1,i], z_lb[i]) + setupperbound(z_Ol[N+1,i], z_ub[i]) + end + + @NLparameter(mdl, z0[i=1:n_state] == 0) + @NLparameter(mdl, GP_e_vy[i=1:N] == 0) + @NLparameter(mdl, GP_e_psidot[i=1:N] == 0) + @NLparameter(mdl, c[1:N] == 0) + @NLparameter(mdl, c_Vx[1:N,1:6] == 0) # system identification parameters + @NLparameter(mdl, c_Vy[1:N,1:4] == 0) # system identification parameters + @NLparameter(mdl, c_Psi[1:N,1:3] == 0) # system identification parameters + @NLparameter(mdl, uPrev[1:N,1:2] == 0) + @NLparameter(mdl, df_his[1:mpcParams.delay_df] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:n_state] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + + @NLconstraint(mdl, [i=1:n_state], z_Ol[1,i] == z0[i]) + # THIS INPUT CONSTRAINT IS FOR SYSTEM DELAY + @NLconstraint(mdl, u_Ol[1,1] == uPrev[2,1]) # acceleration delay is 1 + @NLconstraint(mdl, [i=1:mpcParams.delay_df], u_Ol[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] <= ey_max + eps_lane[i-1]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] >= -ey_max - eps_lane[i-1]) + @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) + + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,4]*cos(z_Ol[i,3]) - z_Ol[i,5]*sin(z_Ol[i,3]))/(1-z_Ol[i,2]*c[i])) + + # for i=1:n_state + # @NLconstraint(mdl, z_Ol[N+1,i] == sum(alpha[j]*selStates[j,i] for j=1:Nl*Np)) + # end + + # System dynamics + for i=1:N + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt * dsdt[i]) # s + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt * (z_Ol[i,4]*sin(z_Ol[i,3]) + z_Ol[i,5]*cos(z_Ol[i,3]))) # eY + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt * (z_Ol[i,6]-dsdt[i]*c[i])) # ePsi + # @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + c_Vx[i,1]*z_Ol[i,5]*z_Ol[i,6] + c_Vx[i,2]*z_Ol[i,4] + c_Vx[i,3]*u_Ol[i,1]) # vx + # @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + c_Vx[i,1]*z_Ol[i,5]*z_Ol[i,6] + c_Vx[i,2]*z_Ol[i,4] + c_Vx[i,3]*u_Ol[i,1] + c_Vx[i,4]*z_Ol[i,6]/z_Ol[i,4] + c_Vx[i,5]*z_Ol[i,5]/z_Ol[i,4] + c_Vx[i,6]*u_Ol[i,2]) # vx + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + 0.1*z_Ol[i,5]*z_Ol[i,6] + c_Vx[i,2]*z_Ol[i,4] + c_Vx[i,3]*u_Ol[i,1] + c_Vx[i,4]*z_Ol[i,6]/z_Ol[i,4] + c_Vx[i,5]*z_Ol[i,5]/z_Ol[i,4] + c_Vx[i,6]*u_Ol[i,2]) # vx + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + c_Vy[i,1]*z_Ol[i,5]/z_Ol[i,4] + c_Vy[i,2]*z_Ol[i,4]*z_Ol[i,6] + c_Vy[i,3]*z_Ol[i,6]/z_Ol[i,4] + c_Vy[i,4]*u_Ol[i,2] + GP_e_vy[i]) # vy + @NLconstraint(mdl, z_Ol[i+1,6] == z_Ol[i,6] + c_Psi[i,1]*z_Ol[i,6]/z_Ol[i,4] + c_Psi[i,2]*z_Ol[i,5]/z_Ol[i,4] + c_Psi[i,3]*u_Ol[i,2] + GP_e_psidot[i]) # psiDot + end + + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.12) + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.12) + # for i=1:N-1 # hard constraints on u, which means this confition is really serious + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.12) + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.12) + # end + # Cost functions + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2 ,i=1:N}), j=1:n_state} + + sum{QderivU[j]*(sum{(u_Ol[i,j]-u_Ol[i+1,j])^2 ,i=1:N-1} + (uPrev[1,j]-u_Ol[1,j])^2) ,j=1:n_input} ) + # @NLexpression(mdl, controlCost, sum(R[j]*sum((u_Ol[i,1])^2 for i=1:N) for j=1:2)) + # @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+50.0*eps_lane[i]^2 , i=1:N}) # original data for lane cost 10.0*eps_lane[i]+50.0*eps_lane[i]^2 + @NLexpression(mdl, laneCost, Q_lane*sum{1.0*eps_lane[i]+20.0*eps_lane[i]^2 , i=1:N}) # original data for lane cost 10.0*eps_lane[i]+50.0*eps_lane[i]^2 + @NLexpression(mdl, terminalCost , Q_term_cost*sum{alpha[i]*statesCost[i] , i=1:Nl*Np}) + # Slack cost on vx + #---------------------------------- + @NLexpression(mdl, slackVx, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4] , j=1:Nl*Np})^2) + + # Slack cost on vy + #---------------------------------- + @NLexpression(mdl, slackVy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5] , j=1:Nl*Np})^2) + + # Slack cost on Psi dot + #---------------------------------- + @NLexpression(mdl, slackPsidot, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6] ,j=1:Nl*Np})^2) + + # Slack cost on ePsi + #---------------------------------- + @NLexpression(mdl, slackEpsi, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3] , j=1:Nl*Np})^2) + + # Slack cost on ey + #---------------------------------- + @NLexpression(mdl, slackEy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2] , j=1:Nl*Np})^2) + + # Slack cost on s + #---------------------------------- + @NLexpression(mdl, slackS, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1] , j=1:Nl*Np})^2) + + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackS + Q_slack[2]*slackEy + Q_slack[3]*slackEpsi + Q_slack[4]*slackVx + Q_slack[5]*slackVy + Q_slack[6]*slackPsidot) #+ controlCost + + m.mdl = mdl + m.z0 = z0 + m.c = c + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + m.df_his = df_his + + m.derivCost = derivCost + # m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.GP_e_vy = GP_e_vy + m.GP_e_psidot = GP_e_psidot + m.alpha = alpha # parameters of the convex hull + + # m.slackVx = slackVx + # m.slackVy = slackVy + # m.slackPsidot = slackPsidot + # m.slackEpsi = slackEpsi + # m.slackEy = slackEy + # m.slackS = slackS + return m + end +end + +type MpcModel_convhull_dyn_iden_simple + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + # coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + c_Vx::Array{JuMP.NonlinearParameter,2} + c_Vy::Array{JuMP.NonlinearParameter,1} + psiDot::Array{JuMP.NonlinearParameter,1} + df_his::Array{JuMP.NonlinearParameter,1} + GP_e_vy::Array{JuMP.NonlinearParameter,1} + GP_e_psidot::Array{JuMP.NonlinearParameter,1} + + eps_lane::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearParameter,1} + + derivCost::JuMP.NonlinearExpression + # controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + # slackVx::JuMP.NonlinearExpression + # slackVy::JuMP.NonlinearExpression + # slackPsidot::JuMP.NonlinearExpression + # slackEpsi::JuMP.NonlinearExpression + # slackEy::JuMP.NonlinearExpression + # slackS::JuMP.NonlinearExpression + + function MpcModel_convhull_dyn_iden_simple(mpcParams::MpcParams,modelParams::ModelParams,selectedStates::SelectedStates) + m = new() + n_state=6; n_input=2 + #### Initialize parameters + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + # u_lb = mpcParams.u_lb # lower bounds for the control inputs + # u_ub = mpcParams.u_ub # upper bounds for the control inputs + # z_lb = mpcParams.z_lb # lower bounds for the states + # z_ub = mpcParams.z_ub # upper bounds for the states + ey_max = get_param("ey")*get_param("ey_tighten")/2 + u_lb = [ -0.5 -18/180*pi] + u_ub = [ 3.0 18/180*pi] + z_lb = [-Inf -Inf -Inf 0 -Inf -Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + z_ub = [ Inf Inf Inf 6.0 Inf Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + Q_term_cost = mpcParams.Q_term_cost + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + Q_slack = mpcParams.Q_slack + + Np = selectedStates.Np + Nl = selectedStates.Nl + + mdl = Model(solver = IpoptSolver(print_level=0,max_cpu_time=0.09)) #,linear_solver="ma27"))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + # + @variable( mdl, z_Ol[1:(N+1),1:n_state], start=0.1) + @variable( mdl, u_Ol[1:N,1:n_input]) + @variable( mdl, eps_lane[1:N] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + + for j=1:N + for i=1:n_input + setlowerbound(u_Ol[j,i], u_lb[i]) + setupperbound(u_Ol[j,i], u_ub[i]) + end + for i=1:n_state + setlowerbound(z_Ol[j+1,i], z_lb[i]) + setupperbound(z_Ol[j+1,i], z_ub[i]) + end + end + for i=1:n_state + setlowerbound(z_Ol[N+1,i], z_lb[i]) + setupperbound(z_Ol[N+1,i], z_ub[i]) + end + + @NLparameter(mdl, z0[i=1:6] == 0) + @NLparameter(mdl, GP_e_vy[i=1:N] == 0) + @NLparameter(mdl, GP_e_psidot[i=1:N] == 0) + @NLparameter(mdl, c[1:N] == 0) + @NLparameter(mdl, c_Vx[1:N,1:3] == 0) # system identification parameters + @NLparameter(mdl, c_Vy[1:N] == 0) # system identification parameters + # @NLparameter(mdl, c_Psi[1:N,1:3] == 0) # system identification parameters + @NLparameter(mdl, uPrev[1:N,1:2] == 0) + @NLparameter(mdl, df_his[1:mpcParams.delay_df] == 0) + @NLparameter(mdl, selStates[1:Nl*Np,1:6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1:Nl*Np] == 0) # costs of the states selected in "convhullStates" + + @NLconstraint(mdl, [i=1:n_state], z_Ol[1,i] == z0[i]) + # THIS INPUT CONSTRAINT IS FOR SYSTEM DELAY + @NLconstraint(mdl, u_Ol[1,1] == uPrev[2,1]) # acceleration delay is 1 + @NLconstraint(mdl, [i=1:mpcParams.delay_df], u_Ol[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] <= ey_max + eps_lane[i-1]) + @NLconstraint(mdl, [i=2:N+1], z_Ol[i,2] >= -ey_max - eps_lane[i-1]) + @NLconstraint(mdl, sum{alpha[i],i=1:Nl*Np} == 1) + + @NLexpression(mdl, dsdt[i = 1:N], (z_Ol[i,4]*cos(z_Ol[i,3]) - z_Ol[i,5]*sin(z_Ol[i,3]))/(1-z_Ol[i,2]*c[i])) + + @NLparameter(mdl, psiDot[i = 1:N]==0) + + # for i=1:n_state + # @NLconstraint(mdl, z_Ol[N+1,i] == sum(alpha[j]*selStates[j,i] for j=1:Nl*Np)) + # end + + # System dynamics + for i=1:N + @NLconstraint(mdl, z_Ol[i+1,1] == z_Ol[i,1] + dt * dsdt[i]) # s + @NLconstraint(mdl, z_Ol[i+1,2] == z_Ol[i,2] + dt * (z_Ol[i,4]*sin(z_Ol[i,3]) + z_Ol[i,5]*cos(z_Ol[i,3]))) # eY + @NLconstraint(mdl, z_Ol[i+1,3] == z_Ol[i,3] + dt * (psiDot[i]-dsdt[i]*c[i])) # ePsi + # @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt * (c_Vx[i,1]*u_Ol[i,1] + c_Vx[i,2]*z_Ol[i,4] + c_Vx[i,3]*u_Ol[i,2] + z_Ol[i,5]*psiDot[i])) + @NLconstraint(mdl, z_Ol[i+1,4] == z_Ol[i,4] + dt * (u_Ol[i,1] + 0.05*z_Ol[i,4] + 0.0*u_Ol[i,2] + z_Ol[i,5]*psiDot[i])) + @NLconstraint(mdl, z_Ol[i+1,5] == z_Ol[i,5] + dt * (c_Vy[i]*u_Ol[i,2] - z_Ol[i,4]*psiDot[i])) # vy + @NLconstraint(mdl, z_Ol[i,6] == psiDot[i]) # psiDot + end + + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] <= 0.12) + # @NLconstraint(mdl, u_Ol[1,2]-uPrev[1,2] >= -0.12) + # for i=1:N-1 # hard constraints on u, which means this confition is really serious + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] <= 0.12) + # @NLconstraint(mdl, u_Ol[i+1,2]-u_Ol[i,2] >= -0.12) + # end + # Cost functions + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]-z_Ol[i+1,j])^2 ,i=1:N}), j=1:n_state} + + sum{QderivU[j]*(sum{(u_Ol[i,j]-u_Ol[i+1,j])^2 ,i=1:N-1} + (uPrev[1,j]-u_Ol[1,j])^2) ,j=1:n_input} ) + # @NLexpression(mdl, controlCost, sum(R[j]*sum((u_Ol[i,1])^2 for i=1:N) for j=1:2)) + # @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+50.0*eps_lane[i]^2 , i=1:N}) # original data for lane cost 10.0*eps_lane[i]+50.0*eps_lane[i]^2 + @NLexpression(mdl, laneCost, Q_lane*sum{1.0*eps_lane[i]+20.0*eps_lane[i]^2 , i=1:N}) # original data for lane cost 10.0*eps_lane[i]+50.0*eps_lane[i]^2 + @NLexpression(mdl, terminalCost , Q_term_cost*sum{alpha[i]*statesCost[i] , i=1:Nl*Np}) + # Slack cost on vx + #---------------------------------- + @NLexpression(mdl, slackVx, (z_Ol[N+1,4] - sum{alpha[j]*selStates[j,4] , j=1:Nl*Np})^2) + + # Slack cost on vy + #---------------------------------- + @NLexpression(mdl, slackVy, (z_Ol[N+1,5] - sum{alpha[j]*selStates[j,5] , j=1:Nl*Np})^2) + + # Slack cost on Psi dot + #---------------------------------- + @NLexpression(mdl, slackPsidot, (z_Ol[N+1,6] - sum{alpha[j]*selStates[j,6] ,j=1:Nl*Np})^2) + + # Slack cost on ePsi + #---------------------------------- + @NLexpression(mdl, slackEpsi, (z_Ol[N+1,3] - sum{alpha[j]*selStates[j,3] , j=1:Nl*Np})^2) + + # Slack cost on ey + #---------------------------------- + @NLexpression(mdl, slackEy, (z_Ol[N+1,2] - sum{alpha[j]*selStates[j,2] , j=1:Nl*Np})^2) + + # Slack cost on s + #---------------------------------- + @NLexpression(mdl, slackS, (z_Ol[N+1,1] - sum{alpha[j]*selStates[j,1] , j=1:Nl*Np})^2) + + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackS + Q_slack[2]*slackEy + Q_slack[3]*slackEpsi + Q_slack[4]*slackVx + Q_slack[5]*slackVy + Q_slack[6]*slackPsidot) #+ controlCost + + m.mdl = mdl + m.z0 = z0 + m.c = c + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.psiDot = psiDot + m.uPrev = uPrev + m.df_his = df_his + + m.derivCost = derivCost + # m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.GP_e_vy = GP_e_vy + m.GP_e_psidot = GP_e_psidot + m.alpha = alpha # parameters of the convex hull + + # m.slackVx = slackVx + # m.slackVy = slackVy + # m.slackPsidot = slackPsidot + # m.slackEpsi = slackEpsi + # m.slackEy = slackEy + # m.slackS = slackS + return m + end +end + +type MpcModel_convhull_kin_linear + + mdl::JuMP.Model + + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + zPrev::Array{JuMP.NonlinearParameter,2} + df_his::Array{JuMP.NonlinearParameter,1} + + A::Array{JuMP.NonlinearParameter,3} + B::Array{JuMP.NonlinearParameter,3} + + eps_lane::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + z_linear::Array{JuMP.NonlinearParameter,2} + u_linear::Array{JuMP.NonlinearParameter,2} + + derivCost + laneCost + terminalCost + # slackS + # slackEy + # slackEpsi + # slackV + + function MpcModel_convhull_kin_linear(mpcParams::MpcParams,modelParams::ModelParams,selectedStates::SelectedStates) + m = new(); n_state=4; n_input=2 + # A big difference in this model is that the first state vector is fixed variable, which is intialized individually + # So the dimensions of value assignment need to match and be paied attention + # IMPORTANT: decision variables here are delta state, which needs to be added back to the linearization point + #### Initialize parameters + dt = modelParams.dt # time step + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + # u_lb = mpcParams.u_lb # lower bounds for the control inputs + # u_ub = mpcParams.u_ub # upper bounds for the control inputs + # z_lb = mpcParams.z_lb # lower bounds for the states + # z_ub = mpcParams.z_ub # upper bounds for the states + u_lb = [-1 -18/180*pi] + u_ub = [ 2 18/180*pi] + z_lb = [-Inf -Inf -pi -0.5] # 1.s 2.ey 3.epsi 4.v + z_ub = [ Inf Inf pi 2.5] # 1.s 2.ey 3.epsi 4.v + + c_f = modelParams.c_f + + ey_max = get_param("ey")*get_param("ey_tighten")/2 + + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_term_cost= mpcParams.Q_term_cost::Float64 # weights on the terminal states + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + Q_slack = mpcParams.Q_slack + + Np = selectedStates.Np # how many states to select + Nl = selectedStates.Nl # how many previous laps to select + + mdl = Model(solver = IpoptSolver(print_level=0,linear_solver="ma27",max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @NLparameter( mdl, zPrev[1:N+1,1:n_state] == 0) + @NLparameter( mdl, uPrev[1:N,1:n_input] == 0) + @NLparameter( mdl, z_linear[1:N+1,1:n_state] == 0) # reference variables, which need to be added back to delta decision variables + @NLparameter( mdl, u_linear[1:N,1:n_input] == 0) + @NLparameter( mdl, A[1:n_state,1:n_state,1:N]==0) + @NLparameter( mdl, B[1:n_state,1:n_input,1:N]==0) + @NLparameter( mdl, selStates[1:Nl*Np,1:n_state]==0) + @NLparameter( mdl, statesCost[1:Nl*Np]==0) + @NLparameter( mdl, df_his[1:mpcParams.delay_df] == 0) + + @variable( mdl, z_Ol[1:N,1:n_state]) + @variable( mdl, u_Ol[1:N,1:n_input]) + @variable( mdl, eps_lane[1:N] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + + @NLconstraint(mdl, [i=1:N], z_Ol[i,2] + z_linear[i+1,2] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=1:N], z_Ol[i,2] + z_linear[i+1,2] >= -ey_max - eps_lane[i]) + @NLconstraint(mdl, sum{alpha[i] , i=1:Nl*Np} == 1) + + # Variable boundary hard constraint on variables + @NLconstraint(mdl, [i=1:N], z_Ol[i,4] + z_linear[i+1,4] <= z_ub[4]) + @NLconstraint(mdl, [i=1:N], z_Ol[i,4] + z_linear[i+1,4] >= z_lb[4]) + for j=1:n_input + @NLconstraint(mdl, [i=1:N], u_Ol[i,j] + u_linear[i,j] >= u_lb[j]) + @NLconstraint(mdl, [i=1:N], u_Ol[i,j] + u_linear[i,j] <= u_ub[j]) + end + + @NLconstraint(mdl, u_Ol[1,1]+u_linear[1,1] == uPrev[2,1]) # acceleration delay is 1 + @NLconstraint(mdl, [i=1:mpcParams.delay_df], u_Ol[i,2]+u_linear[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + # System dynamics + for i=1:N-1 + for j=1:n_state + @NLconstraint(mdl, z_Ol[i+1,j] == A[j,1,i+1]*z_Ol[i,1]+A[j,2,i+1]*z_Ol[i,2]+A[j,3,i+1]*z_Ol[i,3]+A[j,4,i+1]*z_Ol[i,4]+B[j,1,i+1]*u_Ol[i+1,1]+B[j,2,i+1]*u_Ol[i+1,2] ) + end + end + for j=1:n_state + # @constraint(mdl, z_Ol[1,j] == dot(B[j,:,1],u_Ol[1,:]) ) + @NLconstraint(mdl, z_Ol[1,j] == B[j,1,1]*u_Ol[1,1]+B[j,2,1]*u_Ol[1,2] ) + end + + # for i=1:N-1 + # @constraint(mdl, z_Ol[i+1,:] .== A[:,:,i+1]*z_Ol[i,:] + B[:,:,i+1]*u_Ol[i+1,:] ) + # end + + @NLconstraint(mdl, u_Ol[1,2]+u_linear[1,2]-uPrev[1,2] <= 0.12) + @NLconstraint(mdl, u_Ol[1,2]+u_linear[1,2]-uPrev[1,2] >= -0.12) + for i=1:N-1 + @NLconstraint(mdl, u_Ol[i+1,2]+u_linear[i+1,2]-(u_Ol[i,2]+u_linear[i,2]) <= 0.12) + @NLconstraint(mdl, u_Ol[i+1,2]+u_linear[i+1,2]-(u_Ol[i,2]+u_linear[i,2]) >= -0.12) + end + + # for i=1:n_state + # @constraint(mdl, z_Ol[N,i] + z_linear[N+1,i] == sum(alpha[j]*selStates[j,i] for j=1:Nl*Np)) + # end + + # Cost functions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]+z_linear[i+1,j]-z_Ol[i+1,j]-z_linear[i+2,j])^2 , i=1:N-1}+(zPrev[1,j]-z_Ol[1,j]-z_linear[2,j])^2) , j=1:n_state} + + sum{QderivU[j]*(sum{(u_Ol[i,j]+u_linear[i ,j]-u_Ol[i+1,j]-u_linear[i+1,j])^2 , i=1:N-1}+(uPrev[1,j]-u_Ol[1,j]-u_linear[1,j])^2) , j=1:n_input}) + # Lane cost (soft) + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+50.0*eps_lane[i]^2 , i=1:N}) + # Terminal Cost + @NLexpression(mdl, terminalCost , Q_term_cost*sum{alpha[i]*statesCost[i] , i=1:Nl*Np}) + ###### SLACK VARIABLES: ###### + # When due to the linear modeling in JuMP, there is no parameter as NLparameters, + # which will cause the noolinear expression here + ###### Slack cost on s ###### + @NLexpression(mdl, slackS, (z_Ol[N,1] + z_linear[N+1,1] - sum{alpha[j]*selStates[j,1] , j=1:Nl*Np})^2) + # Slack cost on ey + @NLexpression(mdl, slackEy, (z_Ol[N,2] + z_linear[N+1,2] - sum{alpha[j]*selStates[j,2] , j=1:Nl*Np})^2) + # Slack cost on ePsi + @NLexpression(mdl, slackEpsi, (z_Ol[N,3] + z_linear[N+1,3] - sum{alpha[j]*selStates[j,3] , j=1:Nl*Np})^2) + # Slack cost on v + @NLexpression(mdl, slackV, (z_Ol[N,4] + z_linear[N+1,4] - sum{alpha[j]*selStates[j,4] , j=1:Nl*Np})^2) + + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackS + Q_slack[2]*slackEy + Q_slack[3]*slackEpsi + Q_slack[4]*slackV) #+ Q_slack[5]*slackEy + Q_slack[6]*slackS) #+ controlCost + + m.mdl = mdl + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.z_linear = z_linear + m.u_linear = u_linear + m.zPrev = zPrev + m.uPrev = uPrev + m.df_his = df_his + + m.derivCost = derivCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + m.eps_lane = eps_lane + + m.A=A + m.B=B + + # m.slackS = slackS + # m.slackEy = slackEy + # m.slackEpsi = slackEpsi + # m.slackV = slackV + + return m + end +end + +type MpcModel_convhull_dyn_linear + + mdl::JuMP.Model + + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + zPrev::Array{JuMP.NonlinearParameter,2} + + A::Array{JuMP.NonlinearParameter,3} + B::Array{JuMP.NonlinearParameter,3} + + eps_lane::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + z_linear::Array{JuMP.NonlinearParameter,2} + u_linear::Array{JuMP.NonlinearParameter,2} + + GP_e_vy::Array{JuMP.NonlinearParameter,1} + GP_e_psidot::Array{JuMP.NonlinearParameter,1} + + df_his::Array{JuMP.NonlinearParameter,1} + + derivCost + laneCost + terminalCost + # slackS + # slackEy + # slackEpsi + # slackV + + function MpcModel_convhull_dyn_linear(mpcParams::MpcParams,modelParams::ModelParams,selectedStates::SelectedStates) + m = new(); n_state=6; n_input=2 + # A big difference in this model is that the first state vector is fixed variable, which is intialized individually + # So the dimensions of value assignment need to match and be paied attention + # IMPORTANT: decision variables here are delta state, which needs to be added back to the linearization point + #### Initialize parameters + dt = modelParams.dt # time step + m_car = modelParams.m + I_z = modelParams.I_z + L_a = modelParams.l_A # distance from CoM of the car to the front wheels + L_b = modelParams.l_B # distance from CoM of the car to the rear wheels + # u_lb = mpcParams.u_lb # lower bounds for the control inputs + # u_ub = mpcParams.u_ub # upper bounds for the control inputs + # z_lb = mpcParams.z_lb # lower bounds for the states + # z_ub = mpcParams.z_ub # upper bounds for the states + u_lb = [ -0.5 -18/180*pi] + u_ub = [ 2 18/180*pi] + z_lb = [-Inf -Inf -Inf 0 -Inf -Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + z_ub = [ Inf Inf Inf 2.5 Inf Inf] # 1.s 2.ey 3.epsi 4.vx 5.vy 6.psi_dot + c_f = modelParams.c_f + + ey_max = get_param("ey")*get_param("ey_tighten")/2 + + N = mpcParams.N # Prediction horizon + QderivZ = mpcParams.QderivZ::Array{Float64,1} # weights for the derivative cost on the states + QderivU = mpcParams.QderivU::Array{Float64,1} # weights for the derivative cost on the control inputs + R = mpcParams.R::Array{Float64,1} # weights on the control inputs + Q = mpcParams.Q::Array{Float64,1} # weights on the states for path following + Q_term_cost= mpcParams.Q_term_cost::Float64 # weights on the terminal states + Q_lane = mpcParams.Q_lane::Float64 # weight on the soft constraint on the lane + Q_vel = mpcParams.Q_vel::Float64 # weight on the soft constraint for the max velocity + Q_slack = mpcParams.Q_slack + + Np = selectedStates.Np::Int64 # how many states to select + Nl = selectedStates.Nl::Int64 # how many previous laps to select + + mdl = Model(solver = IpoptSolver(print_level=0,linear_solver="ma27")) #,max_cpu_time=0.09))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + @NLparameter( mdl, zPrev[1:N+1,1:n_state] == 0) + @NLparameter( mdl, uPrev[1:N,1:n_input] == 0) + @NLparameter( mdl, z_linear[1:N+1,1:n_state] == 0) # reference variables, which need to be added back to delta decision variables + @NLparameter( mdl, u_linear[1:N,1:n_input] == 0) + @NLparameter( mdl, A[1:n_state,1:n_state,1:N]==0) + @NLparameter( mdl, B[1:n_state,1:n_input,1:N]==0) + @NLparameter( mdl, selStates[1:Nl*Np,1:n_state]==0) + @NLparameter( mdl, statesCost[1:Nl*Np]==0) + @NLparameter( mdl, df_his[1:mpcParams.delay_df] == 0) + @NLparameter( mdl, GP_e_vy[1:N]==0) + @NLparameter( mdl, GP_e_psidot[1:N]==0) + + @variable( mdl, z_Ol[1:N,1:n_state]) + @variable( mdl, u_Ol[1:N,1:n_input]) + @variable( mdl, eps_lane[1:N] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1:Nl*Np] >= 0) # coefficients of the convex hull + + @NLconstraint(mdl, u_Ol[1,1]+u_linear[1,1] == uPrev[2,1]) # acceleration delay is 1 + @NLconstraint(mdl, [i=1:mpcParams.delay_df], u_Ol[i,2]+u_linear[i,2] == df_his[i]) # steering delay is 1 for simulation and 3 for experiment + + @NLconstraint(mdl, [i=1:N], z_Ol[i,2] + z_linear[i+1,2] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i=1:N], z_Ol[i,2] + z_linear[i+1,2] >= -ey_max - eps_lane[i]) + @constraint(mdl, sum{alpha[i] , i=1:Nl*Np} == 1) + + # Variable boundary hard constraint on variables + @NLconstraint(mdl, [i=1:N], z_Ol[i,4] + z_linear[i+1,4] <= z_ub[4]) + @NLconstraint(mdl, [i=1:N], z_Ol[i,4] + z_linear[i+1,4] >= z_lb[4]) + for j=1:n_input + @NLconstraint(mdl, [i=1:N], u_Ol[i,j] + u_linear[i,j] <= u_ub[j]) + @NLconstraint(mdl, [i=1:N], u_Ol[i,j] + u_linear[i,j] >= u_lb[j]) + end + + # System dynamics + for i=1:N-1 + for j=1:n_state + if j==5 + @NLconstraint(mdl, z_Ol[i+1,j] == sum{A[j,k,i+1]*z_Ol[i,k], k=1:n_state}+sum{B[j,k,i+1]*u_Ol[i+1,k], k=1:n_input} ) + elseif j == 6 + @NLconstraint(mdl, z_Ol[i+1,j] == sum{A[j,k,i+1]*z_Ol[i,k], k=1:n_state}+sum{B[j,k,i+1]*u_Ol[i+1,k], k=1:n_input} ) + else + @NLconstraint(mdl, z_Ol[i+1,j] == sum{A[j,k,i+1]*z_Ol[i,k], k=1:n_state}+sum{B[j,k,i+1]*u_Ol[i+1,k], k=1:n_input} ) + end + end + end + for j=1:n_state + if j==5 + @NLconstraint(mdl, z_Ol[1,j] == sum{B[j,k,1]*u_Ol[1,k], k=1:n_input} ) + elseif j==6 + @NLconstraint(mdl, z_Ol[1,j] == sum{B[j,k,1]*u_Ol[1,k], k=1:n_input} ) + else + @NLconstraint(mdl, z_Ol[1,j] == sum{B[j,k,1]*u_Ol[1,k], k=1:n_input} ) + end + end + # IDENTIFIED DISTURBANCE FOR THE LINEARIZED \delta MODEL IS TOO MUCH. SO THIS SHOULD BE PUT INTO THE LINEARIZATION POINTS + + # STEERING DERIVATIVE CONSTRAINTS + # @NLconstraint(mdl, u_Ol[1,2]+u_linear[1,2]-uPrev[2] <= 0.12) + # @NLconstraint(mdl, u_Ol[1,2]+u_linear[1,2]-uPrev[2] >= -0.12) + for i=1:N-1 + @NLconstraint(mdl, u_Ol[i+1,2]+u_linear[i+1,2]-(u_Ol[i,2]+u_linear[i,2]) <= 0.12) + @NLconstraint(mdl, u_Ol[i+1,2]+u_linear[i+1,2]-(u_Ol[i,2]+u_linear[i,2]) >= -0.12) + end + + # for i=1:n_state + # @NLconstraint(mdl, z_Ol[N,i] + z_linear[N+1,i] == sum{alpha[j]*selStates[j,i] , j=1:Nl*Np}) + # end + + # Cost functions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j]*(sum{(z_Ol[i,j]+z_linear[i+1,j]-z_Ol[i+1,j]-z_linear[i+2,j])^2 , i=1:N-1} + (z_Ol[1,j]+z_linear[2,j]-z_linear[1,j])^2 ) , j=1:n_state} + + QderivU[1]*sum{(u_Ol[i,1]+u_linear[i ,1]-u_Ol[i+1,1]-u_linear[i+1,1])^2 , i=1:N-1} + + QderivU[2]*sum{(u_Ol[i,2]+u_linear[i ,2]-u_Ol[i+1,2]-u_linear[i+1,2])^2 , i=1+mpcParams.delay_df:N-1}) + + # Lane cost (soft) + @NLexpression(mdl, laneCost, Q_lane*sum{10.0*eps_lane[i]+50.0*eps_lane[i]^2 , i=1:N}) + # Terminal Cost + @NLexpression(mdl, terminalCost , Q_term_cost*sum{alpha[i]*statesCost[i] , i=1:Nl*Np}) + ###### SLACK VARIABLES: ###### + # When due to the linear modeling in JuMP, there is no parameter as NLparameters, + # which will cause the noolinear expression here + ###### Slack cost on s ###### + @NLexpression(mdl, slackS, (z_Ol[N,1] + z_linear[N+1,1] - sum{alpha[j]*selStates[j,1] , j=1:Nl*Np})^2) + # Slack cost on ey + @NLexpression(mdl, slackEy, (z_Ol[N,2] + z_linear[N+1,2] - sum{alpha[j]*selStates[j,2] , j=1:Nl*Np})^2) + # Slack cost on ePsi + @NLexpression(mdl, slackEpsi, (z_Ol[N,3] + z_linear[N+1,3] - sum{alpha[j]*selStates[j,3] , j=1:Nl*Np})^2) + # Slack cost on Vx + @NLexpression(mdl, slackVx, (z_Ol[N,4] + z_linear[N+1,4] - sum{alpha[j]*selStates[j,4] , j=1:Nl*Np})^2) + # Slack cost on Vy + @NLexpression(mdl, slackVy, (z_Ol[N,5] + z_linear[N+1,5] - sum{alpha[j]*selStates[j,5] , j=1:Nl*Np})^2) + # Slack cost on Psidot + @NLexpression(mdl, slackPsidot, (z_Ol[N,6] + z_linear[N+1,6] - sum{alpha[j]*selStates[j,6] , j=1:Nl*Np})^2) + + @NLobjective(mdl, Min, derivCost + laneCost + terminalCost + Q_slack[1]*slackS + Q_slack[2]*slackEy + Q_slack[3]*slackEpsi + Q_slack[4]*slackVx + Q_slack[5]*slackVy + Q_slack[6]*slackPsidot) #+ controlCost + + m.mdl = mdl + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.z_linear = z_linear + m.u_linear = u_linear + m.zPrev = zPrev + m.uPrev = uPrev + m.GP_e_vy = GP_e_vy + m.GP_e_psidot = GP_e_psidot + m.df_his = df_his + + # m.derivCost = derivCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + m.eps_lane = eps_lane + + m.A=A + m.B=B + + # m.slackS = slackS + # m.slackEy = slackEy + # m.slackEpsi = slackEpsi + # m.slackV = slackV + + return m + end +end + + diff --git a/workspace/src/barc/src/barc_lib/LMPC/functions.jl b/workspace/src/barc/src/barc_lib/LMPC/functions.jl new file mode 100644 index 00000000..eeee17d9 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/functions.jl @@ -0,0 +1,1155 @@ +# Important information about oldTraj: +# ====================================== +# oldTraj.oldTraj contains all state information of one lap. It is structured like follows: +# The first values are the end of one lap before the next lap starts (necessary for inter-lap-system-ID) +# The last value of ends with the last value *before* the finish line (s < s_target) +# The s-values within need to be below zero (-> before the finish line). Otherwise they can be mistaken as the end of the current (not previous) lap. +# After follow the recorded states of the trajectory (as many as there were during one lap) +# The number of recorded states during one trajectory (0 <= s < s_target) is equal to . +# After the recorded trajectory, the rest of the vector (until ) is filled up with constant values + +function saveOldTraj(oldTraj::OldTrajectory,zCurr::Array{Float64},uCurr::Array{Float64},lapStatus::LapStatus,posInfo::PosInfo,buffersize::Int64) + println("Starting function") + i = lapStatus.currentIt-1 # i = number of points for 0 <= s < s_target (= cost of this lap) + prebuf = oldTraj.prebuf # so many points of the end of the previous old traj will be attached to the beginning + zCurr_export = zeros(buffersize,6) + uCurr_export = zeros(buffersize,2) + + println("Creating exports") + zCurr_export = cat(1,oldTraj.oldTraj[oldTraj.oldCost[1]+1:oldTraj.oldCost[1]+prebuf,:,1], + zCurr[1:i,:], NaN*ones(buffersize-i-prebuf,6))#[ones(buffersize-i-prebuf,1)*zCurr[i,1:5] zCurr[i,6]+collect(1:buffersize-i-prebuf)*dt*zCurr[i,1]]) + uCurr_export = cat(1,oldTraj.oldInput[oldTraj.oldCost[1]+1:oldTraj.oldCost[1]+prebuf,:,1], + uCurr[1:i,:], NaN*ones(buffersize-i-prebuf,2))#zeros(buffersize-i-prebuf,2)) + + zCurr_export[1:prebuf,6] -= posInfo.s_target # make the prebuf-values below zero + costLap = i # the cost of the current lap is the time it took to reach the finish line + println("Saving") + # Save all data in oldTrajectory: + if lapStatus.currentLap <= 2 # if it's the first or second lap + oldTraj.oldTraj[:,:,1] = zCurr_export # ... just save everything + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldTraj[:,:,2] = zCurr_export + oldTraj.oldInput[:,:,2] = uCurr_export + oldTraj.oldCost = [costLap,costLap] + else # idea: always copy the new trajectory in the first array! + if oldTraj.oldCost[1] < oldTraj.oldCost[2] # if the first old traj is better than the second + oldTraj.oldTraj[:,:,2] = oldTraj.oldTraj[:,:,1] # ... copy the first in the second + oldTraj.oldInput[:,:,2] = oldTraj.oldInput[:,:,1] # ... same for the input + oldTraj.oldCost[2] = oldTraj.oldCost[1] + end + oldTraj.oldTraj[:,:,1] = zCurr_export # ... and write the new traj in the first + oldTraj.oldInput[:,:,1] = uCurr_export + oldTraj.oldCost[1] = costLap + end +end + +# FUNCTIONS FOR LOCALIZATION +function find_idx(z::Array{Float64,2},track::Track) + idx = Int(ceil(z[1]/track.ds)+1) + idx>track.n_node ? idx=idx%track.n_node : nothing + idx<=0 ? idx+=track.n_node : nothing + return idx +end + +function curvature_prediction(z::Array{Float64,2},track::Track) + if size(z,1)==1 + curvature=track.curvature[find_idx(z,track)] + else + curvature=zeros(size(z,1)) + for i=1:size(z,1) + curvature[i]=track.curvature[find_idx(z[i,:],track)] + end + end + return curvature +end + +function trackFrame_to_xyFrame(z_sol::Array{Float64,2},track::Track) + # Position sanity check + n = size(z_sol,1) + z_x = zeros(n) + z_y = zeros(n) + for i in 1:n + z = z_sol[i,:] + if z[1]>track.s + z[1]-=track.s + elseif z[1]<0 + z[1]+=track.s + end # At the end of one lap, the prediction can be out of the lap idx range + + ds=track.ds; s=z[1]; ey=z[2]; epsi=z[3] + idx=Int64(ceil(s/ds))+1 # correction for the starting original point + idx>track.n_node ? idx=idx%track.n_node : nothing + # println(idx) + # println(track.n_node) + + x_track=track.xy[idx,1]; y_track=track.xy[idx,2]; theta=track.theta[idx] + x=x_track+ey*cos(theta+pi/2); y=y_track+ey*sin(theta+pi/2) + # psi=theta+epsi + z_x[i]=x + z_y[i]=y + end + return z_x, z_y +end + +function xyFrame_to_trackFrame(z::Array{Float64,1},track::Track) # Localization in Julia which is corresponding to Localization helper in python + # x,y,vx,vy,psi,psidot -> s,ey,epsi,vx,vy,psidot + dist = (z[1]-track.xy[:,1]).^2 + (z[2]-track.xy[:,2]).^2 + (val_min,idx_min)=findmin(dist) + s = (idx_min-1)*track.ds + theta = track.theta[idx_min] + epsi = z[5] - theta + # println(epsi) + while epsi<-pi + epsi += 2*pi + end + while epsi>pi + epsi -= 2*pi + end + # epsi = epsi % pi + # println(epsi) + ey = sqrt(val_min) + bound1_dist = (z[1]-track.bound1xy[idx_min,1]).^2 + (z[2]-track.bound1xy[idx_min,2]).^2 + bound2_dist = (z[1]-track.bound2xy[idx_min,1]).^2 + (z[2]-track.bound2xy[idx_min,2]).^2 + if bound1_dist > bound2_dist + ey = -ey + else + ey = ey + end + return [s,ey,epsi,z[3],z[4],z[6]] +end + +# FUNCTIONS FOR FEATURE DATA SELECTING AND SYS_ID +function find_feature_dist(z_feature::Array{Float64,3},u_feature::Array{Float64,2},z_curr::Array{Float64,2},u_curr::Array{Float64,2},selectedStates::SelectedStates) + Np=selectedStates.feature_Np # Just to make life easier, we directly put a specific number here + # Clear the safe set data of previous iteration + iden_z=zeros(Np,3,2) + iden_z_plot=zeros(Np,6) + iden_u=zeros(Np,2) + + # curr_state=hcat(z_curr[1],z_curr[4:6]',u_curr') + # norm_state=[0.5 1 0.1 1 1 0.3] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + curr_state=hcat(z_curr[4:6]',u_curr) + + norm_state=[1 0.1 1 1 0.2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + # norm_state=[1 1 1 1/2 1/2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + dummy_state=z_feature[:,:,1] + dummy_input=u_feature + # cal_state=Float64[] # stored for normalization calculation + cal_state=hcat(dummy_state,dummy_input) + dummy_norm=zeros(size(dummy_state,1),2) + + norm_dist=(curr_state.-cal_state[:,4:8])./norm_state + dummy_norm[:,1]=norm_dist[:,1].^2+norm_dist[:,2].^2+norm_dist[:,3].^2+norm_dist[:,4].^2+norm_dist[:,5].^2 + dummy_norm[:,2]=1:size(dummy_state,1) + dummy_norm=sortrows(dummy_norm) # pick up the first minimum Np points + # println(dummy_norm[1:Np,:]) + for i=1:Np + iden_z[i,:,1]=z_feature[Int(dummy_norm[i,2]),4:6,1] + iden_z[i,:,2]=z_feature[Int(dummy_norm[i,2]),4:6,2] + iden_z_plot[i,:]=dummy_state[Int(dummy_norm[i,2]),:,1] + iden_u[i,:]=dummy_input[Int(dummy_norm[i,2]),:] + end + + # iden_z: Npx3x2 states selected for system identification + # iden_u: Npx2 inputs selected for system identification + return iden_z, iden_u, iden_z_plot +end + +function find_feature_dist_simple(z_feature::Array{Float64,3},u_feature::Array{Float64,2},z_curr::Array{Float64,2},u_curr::Array{Float64,2},selectedStates::SelectedStates) + Np=selectedStates.feature_Np # Just to make life easier, we directly put a specific number here + # Clear the safe set data of previous iteration + iden_z=zeros(Np,5,2) + iden_z_plot=zeros(Np,6) + iden_u=zeros(Np,2) + + # curr_state=hcat(z_curr[1],z_curr[4:6]',u_curr') + # norm_state=[0.5 1 0.1 1 1 0.3] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + curr_state=hcat(z_curr[4:6]',u_curr) + + norm_state=[1 0.1 1 1 0.2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + # norm_state=[1 1 1 1/2 1/2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + dummy_state=z_feature[:,:,1] + dummy_input=u_feature + # cal_state=Float64[] # stored for normalization calculation + cal_state=hcat(dummy_state,dummy_input) + dummy_norm=zeros(size(dummy_state,1),2) + + norm_dist=(curr_state.-cal_state[:,4:8])./norm_state + dummy_norm[:,1]=norm_dist[:,1].^2+norm_dist[:,2].^2+norm_dist[:,3].^2+norm_dist[:,4].^2+norm_dist[:,5].^2 + dummy_norm[:,2]=1:size(dummy_state,1) + dummy_norm=sortrows(dummy_norm) # pick up the first minimum Np points + # println(dummy_norm[1:Np,:]) + for i=1:Np + iden_z[i,:,1]=z_feature[Int(dummy_norm[i,2]),4:8,1] + iden_z[i,:,2]=z_feature[Int(dummy_norm[i,2]),4:8,2] + iden_z_plot[i,:]=dummy_state[Int(dummy_norm[i,2]),1:6,1] + iden_u[i,:]=dummy_input[Int(dummy_norm[i,2]),:] + end + + # iden_z: Npx3x2 states selected for system identification + # iden_u: Npx2 inputs selected for system identification + return iden_z, iden_u, iden_z_plot +end + +# THE DIFFERENCE TO THE FUNCTON ABOVE IS THAT THIS ONE IS SELECTING THE FEATURE POINTS FROM THE HISTORY +function find_SS_dist(solHistory::SolHistory,z_curr::Array{Float64,2},u_curr::Array{Float64,2},lapStatus::LapStatus,selectedStates::SelectedStates) + + Nl=selectedStates.feature_Nl + Np=selectedStates.feature_Np + # Clear the safe set data of previous iteration + iden_z=zeros(Np,3,2) + iden_z_plot=zeros(Np,6) + iden_u=zeros(Np,2) + + # curr_state=hcat(z_curr[1],z_curr[4:6]',u_curr') + # norm_state=[0.5 1 0.1 1 1 0.3] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + curr_state=hcat(z_curr[4:6]',u_curr) + norm_state=[1 0.1 1 1 0.2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + dummy_state=Array{Float64}(0,6) + dummy_input=Array{Float64}(0,2) + cal_state=Array{Float64}(0,8) # stored for normalization calculation + + # collect out all the state and input history data + # at maximum, Nl laps data will be collected, it was actually reshaped into the whole history + for i=max(1,lapStatus.currentLap-Nl):max(1,(lapStatus.currentLap-1)) + dummy_state=vcat(dummy_state,reshape(solHistory.z[1:Int(solHistory.cost[i]),i,1,:],Int(solHistory.cost[i]),6)) + dummy_input=vcat(dummy_input,reshape(solHistory.u[1:Int(solHistory.cost[i]),i,1,:],Int(solHistory.cost[i]),2)) + end + cal_state=vcat(cal_state,hcat(dummy_state,dummy_input)) + dummy_norm=zeros(size(dummy_state,1)-1,2) + for i=1:size(dummy_state,1)-1 # The last state point will be removed + # dummy_norm[i,1]=norm((curr_state-cal_state[i,vcat(1,4:8)]')./norm_state) # this is the state after normalization + norm_dist=(curr_state.-cal_state[i,4:8])./norm_state + dummy_norm[i,1]=norm_dist[1]^2+norm_dist[2]^2+norm_dist[3]^2+norm_dist[4]^2+norm_dist[5]^2 + # dummy_norm[i,1]=norm((curr_state-cal_state[i,4:8])./norm_state) # this is the state after normalization + dummy_norm[i,2]=i + end + + dummy_norm=sortrows(dummy_norm) # pick up the first minimum Np points + + for i=1:min(Np,size(dummy_norm,1)) + iden_z[i,:,1]=dummy_state[Int(dummy_norm[i,2]),4:6] + iden_z[i,:,2]=dummy_state[Int(dummy_norm[i,2])+1,4:6] + iden_z_plot[i,:]=dummy_state[Int(dummy_norm[i,2]),:] + iden_u[i,:]=dummy_input[Int(dummy_norm[i,2]),:] + end + # iden_z: Npx3x2 states selected for system identification + # iden_u: Npx2 inputs selected for system identification + return iden_z, iden_u, iden_z_plot +end + +function coeff_iden_dist(idenStates::Array{Float64,3},idenInputs::Array{Float64,2}) + z = idenStates + # z[:,2,:]*=10 + u = idenInputs + # println(z) + # println(u) + size(z,1)==size(u,1) ? nothing : error("state and input in coeff_iden() need to have the same dimensions") + A_vx=zeros(size(z,1),6) + A_vy=zeros(size(z,1),4) + A_psi=zeros(size(z,1),3) + # y_vx=diff(z[:,1,:],2) + # y_vy=diff(z[:,2,:],2) + # y_psi=diff(z[:,3,:],2) + + # n = 0.02*randn(size(z,1),1) + # n = max(n,-0.1) + # n = min(n, 0.1) + # z[:,1,1]+=n + + # n = 0.005*randn(size(z,1),1) + # n = max(n,-0.005) + # n = min(n, 0.005) + # z[:,2,1]+=n + + # n = 0.05*randn(size(z,1),1) + # n = max(n,-0.05) + # n = min(n, 0.05) + # z[:,3,1]+=n + + # n = 0.05*randn(size(z,1),1) + # n = max(n,-0.05) + # n = min(n, 0.05) + # u[:,1]+=n + + # n = 0.01*randn(size(z,1),1) + # n = max(n,-0.01) + # n = min(n, 0.01) + # u[:,2]+=n + + # y_vx=diff(reshape(z[:,1,:],size(z,1),size(z,3)),2) + # y_vy=diff(reshape(z[:,2,:],size(z,1),size(z,3)),2)#/10 + # y_psi=diff(reshape(z[:,3,:],size(z,1),size(z,3)),2) + + y_vx = z[:,1,2] - z[:,1,1] + y_vy = z[:,2,2] - z[:,2,1] + y_psi = z[:,3,2] - z[:,3,1] + + for i=1:size(z,1) + A_vx[i,1]=z[i,2,1]*z[i,3,1] + A_vx[i,2]=z[i,1,1] + A_vx[i,3]=u[i,1] + A_vx[i,4]=z[i,3,1]/z[i,1,1] + A_vx[i,5]=z[i,2,1]/z[i,1,1] + A_vx[i,6]=u[i,2] + A_vy[i,1]=z[i,2,1]/z[i,1,1] + A_vy[i,2]=z[i,1,1]*z[i,3,1] + A_vy[i,3]=z[i,3,1]/z[i,1,1] + A_vy[i,4]=u[i,2] + A_psi[i,1]=z[i,3,1]/z[i,1,1] + A_psi[i,2]=z[i,2,1]/z[i,1,1] + A_psi[i,3]=u[i,2] + end + # BACKSLASH OPERATOR WITHOUT REGULIZATION + # c_Vx = A_vx\y_vx + # c_Vy = A_vy\y_vy + # c_Psi = A_psi\y_psi + + # BY IVS() + # c_Vx = inv(A_vx'*A_vx)*A_vx'*y_vx + # c_Vy = inv(A_vy'*A_vy)*A_vy'*y_vy + # c_Psi = inv(A_psi'*A_psi)*A_psi'*y_psi + + # BACKSLASH WITH REGULARIZATION + mu_Vx = zeros(6,6); mu_Vx[1,1] = 1e-5 + mu_Vy = zeros(4,4); mu_Vy[1,1] = 1e-5 + mu_Psi = zeros(3,3); mu_Psi[2,2] = 1e-5 + c_Vx = (A_vx'*A_vx+mu_Vx)\(A_vx'*y_vx) + c_Vy = (A_vy'*A_vy+mu_Vy)\(A_vy'*y_vy) + c_Psi = (A_psi'*A_psi+mu_Psi)\(A_psi'*y_psi) + + + # println("c_Vx is $c_Vx") + # println("c_Vx is $c_Vy") + # println("c_Vx is $c_Psi") + # c_Vx[1] = max(-0.3,min(0.3,c_Vx[1])) + # c_Vy[2] = max(-1,min(1,c_Vy[2])) + # c_Vy[3] = max(-1,min(1,c_Vy[3])) + + return c_Vx, c_Vy, c_Psi +end + +function find_SS_dist_simple(solHistory::SolHistory,z_curr::Array{Float64,2},u_curr::Array{Float64,2},lapStatus::LapStatus,selectedStates::SelectedStates) + + Nl=selectedStates.feature_Nl + Np=selectedStates.feature_Np + # Clear the safe set data of previous iteration + iden_z=zeros(Np,5,2) + iden_z_plot=zeros(Np,6) + iden_u=zeros(Np,2) + + # curr_state=hcat(z_curr[1],z_curr[4:6]',u_curr') + # norm_state=[0.5 1 0.1 1 1 0.3] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + curr_state=hcat(z_curr[4:6]',u_curr) + norm_state=[1 0.1 1 1 0.2] # [1 0.1 1] are the normed state, the first state is for "s", which is for putting some weight on the track place + dummy_state=Array{Float64}(0,8) + dummy_input=Array{Float64}(0,2) + cal_state=Array{Float64}(0,8) # stored for normalization calculation + + # collect out all the state and input history data + # at maximum, Nl laps data will be collected, it was actually reshaped into the whole history + for i=max(1,lapStatus.currentLap-Nl):max(1,(lapStatus.currentLap-1)) + dummy_state=vcat(dummy_state,reshape(solHistory.z[1:Int(solHistory.cost[i]),i,1,:],Int(solHistory.cost[i]),8)) + dummy_input=vcat(dummy_input,reshape(solHistory.u[1:Int(solHistory.cost[i]),i,1,:],Int(solHistory.cost[i]),2)) + end + # println(size(dummy_state)) + # println(size(dummy_input)) + cal_state=vcat(cal_state,hcat(dummy_state[:,1:6],dummy_input)) + dummy_norm=zeros(size(dummy_state,1)-1,2) + for i=1:size(dummy_state,1)-1 # The last state point will be removed + # dummy_norm[i,1]=norm((curr_state-cal_state[i,vcat(1,4:8)]')./norm_state) # this is the state after normalization + norm_dist=(curr_state.-cal_state[i,4:8])./norm_state + dummy_norm[i,1]=norm_dist[1]^2+norm_dist[2]^2+norm_dist[3]^2+norm_dist[4]^2+norm_dist[5]^2 + # dummy_norm[i,1]=norm((curr_state-cal_state[i,4:8])./norm_state) # this is the state after normalization + dummy_norm[i,2]=i + end + + dummy_norm=sortrows(dummy_norm) # pick up the first minimum Np points + + for i=1:min(Np,size(dummy_norm,1)) + iden_z[i,:,1]=dummy_state[Int(dummy_norm[i,2]),4:8] + # iden_z[i,:,2]=dummy_state[Int(dummy_norm[i,2])+1,4:8] + iden_z_plot[i,:]=dummy_state[Int(dummy_norm[i,2]),1:6] + iden_u[i,:]=dummy_input[Int(dummy_norm[i,2]),:] + end + # iden_z: Npx3x2 states selected for system identification + # iden_u: Npx2 inputs selected for system identification + return iden_z, iden_u, iden_z_plot +end + +function coeff_iden_dist_simple(idenStates::Array{Float64,3},idenInputs::Array{Float64,2}) + z = idenStates + # z[:,2,:]*=10 + u = idenInputs + # println(z) + # println(u) + size(z,1)==size(u,1) ? nothing : error("state and input in coeff_iden() need to have the same dimensions") + A_vx=zeros(size(z,1),3) + A_vy=zeros(size(z,1),1) + # y_vx=diff(z[:,1,:],2) + # y_vy=diff(z[:,2,:],2) + # y_psi=diff(z[:,3,:],2) + + # n = 0.02*randn(size(z,1),1) + # n = max(n,-0.1) + # n = min(n, 0.1) + # z[:,1,1]+=n + + # n = 0.005*randn(size(z,1),1) + # n = max(n,-0.005) + # n = min(n, 0.005) + # z[:,2,1]+=n + + # n = 0.05*randn(size(z,1),1) + # n = max(n,-0.05) + # n = min(n, 0.05) + # z[:,3,1]+=n + + # n = 0.05*randn(size(z,1),1) + # n = max(n,-0.05) + # n = min(n, 0.05) + # u[:,1]+=n + + # n = 0.01*randn(size(z,1),1) + # n = max(n,-0.01) + # n = min(n, 0.01) + # u[:,2]+=n + + # y_vx=diff(reshape(z[:,1,:],size(z,1),size(z,3)),2) + # y_vy=diff(reshape(z[:,2,:],size(z,1),size(z,3)),2)#/10 + # y_psi=diff(reshape(z[:,3,:],size(z,1),size(z,3)),2) + + y_vx = z[:,4,1] + y_vy = z[:,5,1] + + for i=1:size(z,1) + A_vx[i,1]=u[i,1] + A_vx[i,2]=z[i,1,1] + A_vx[i,3]=u[i,2] + + A_vy[i,1]=u[i,2] + end + # BACKSLASH OPERATOR WITHOUT REGULIZATION + c_Vx = A_vx\y_vx + c_Vy = A_vy\y_vy + # c_Psi = A_psi\y_psi + + # BY IVS() + # c_Vx = inv(A_vx'*A_vx)*A_vx'*y_vx + # c_Vy = inv(A_vy'*A_vy)*A_vy'*y_vy + # c_Psi = inv(A_psi'*A_psi)*A_psi'*y_psi + + # BACKSLASH WITH REGULARIZATION + # mu_Vx = zeros(6,6); mu_Vx[1,1] = 1e-5 + # mu_Vy = zeros(4,4); mu_Vy[1,1] = 1e-5 + # mu_Psi = zeros(3,3); mu_Psi[2,2] = 1e-5 + # c_Vx = (A_vx'*A_vx+mu_Vx)\(A_vx'*y_vx) + # c_Vy = (A_vy'*A_vy+mu_Vy)\(A_vy'*y_vy) + # c_Psi = (A_psi'*A_psi+mu_Psi)\(A_psi'*y_psi) + + + # println("c_Vx is $c_Vx") + # println("c_Vx is $c_Vy") + # println("c_Vx is $c_Psi") + # c_Vx[1] = max(-0.3,min(0.3,c_Vx[1])) + # c_Vy[2] = max(-1,min(1,c_Vy[2])) + # c_Vy[3] = max(-1,min(1,c_Vy[3])) + + # println(typeof(c_Vx)) + # println(typeof(c_Vy)) + return c_Vx, c_Vy[1] +end + +function find_SS(safeSetData::SafeSetData,selectedStates::SelectedStates, + s_curr::Float64,z_prev::Array{Float64,2},lapStatus::LapStatus, + modelParams::ModelParams,mpcParams::MpcParams,track::Track) + + s=s_curr; v=z_prev[2,4]; + N=mpcParams.N; dt=modelParams.dt + Nl=selectedStates.Nl; Np_here=copy(selectedStates.Np/2) + # Clear the safe set data of previous iteration + selectedStates.selStates=Array{Float64}(0,6); + selectedStates.statesCost=Array{Float64}(0,1); + # target_s=s+v*dt*(N) # 3 is a temporary shift number + target_s=z_prev[end,1]+z_prev[end,4]*dt + # for i=1:lapStatus.currentLap-1 + cost_correction = findmin(safeSetData.oldCost[lapStatus.currentLap-Nl:lapStatus.currentLap-1])[1] + for i=1:Nl + SS_curr=safeSetData.oldSS[:,:,lapStatus.currentLap-i] + SS_next=safeSetData.oldSS[:,:,lapStatus.currentLap-i+1] + SScost_curr=copy(safeSetData.cost2target[:,lapStatus.currentLap-i]) + all_s=SS_curr[1:Int(safeSetData.oldCost[lapStatus.currentLap-i]),1] + if target_s>track.s + target_s-=track.s # correction when switching the lap + end + (value_s,idx_s)=findmin(abs(all_s-target_s)) + idx_s_start=Int(idx_s-Np_here); + # idx_s_start=max(idx_s_start,find_idx(z_prev[2,:],track)); + idx_s_end=Int(idx_s+Np_here-1) # to be consistant with the number of points selected in the safe set + # idx sanity check + cost=Int(safeSetData.oldCost[lapStatus.currentLap-i]) + # println(safeSetData.oldCost) + # println("Target s is: $target_s") + # println("Safe set idx start and end are") + # println(idx_s_start) + # println(idx_s_end + # println("Size of SS_curr is") + # println(size(SS_curr)) + # println("Size of SS_cost is") + # println(size(SScost_curr)) + if idx_s_start<1 + # println("lapStatus:",lapStatus) + # println("oldCost:",safeSetData.oldCost[1:5]) + # println("<1:","idx_start:",idx_s_start," idx_end:",idx_s_end) + # println(SS_next[1:idx_s_end,1]) + flag_array = isnan(SS_next[1:idx_s_end,:]) + flag = false + for i = 1:length(flag_array) + flag = flag | flag_array[i] + end + if flag + # println("hahaha") + SS_curr[cost+idx_s_start:cost,1]-=track.s + selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[cost+idx_s_start:cost,:],SS_curr[1:idx_s_end,:]) + else + SS_curr[1:idx_s_end,1]+=track.s # correction when switching lap + add_s = hcat(track.s*ones(idx_s_end),zeros(idx_s_end,size(SS_curr,2)-1)) + selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[cost+idx_s_start:cost,:],reshape(SS_next[1:idx_s_end,:],idx_s_end,size(SS_curr,2))+add_s) + end + SScost_curr[1:idx_s_end]-=cost # correction when switching lap + # SScost_curr[1:idx_s_end]-=cost_correction # correction when switching lap + # selectedStates.statesCost=vcat(selectedStates.statesCost,SScost_curr[cost+idx_s_start:cost],SScost_curr[1:idx_s_end]) + selectedStates.statesCost=vcat(selectedStates.statesCost,(selectedStates.Np:-1:1)+cost) + elseif idx_s_end>cost + # println(">cost:","idx_start:",idx_s_start," idx_end:",idx_s_end,"lap cost",cost) + SS_curr[1:idx_s_end-cost,1]+=track.s + add_s = hcat(track.s*ones(idx_s_end-cost),zeros(idx_s_end-cost,size(SS_curr,2)-1)) + + # selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[idx_s_start:cost,:],SS_curr[1:idx_s_end-cost,:]) + selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[idx_s_start:cost,:],reshape(SS_next[1:idx_s_end-cost,:],idx_s_end-cost,size(SS_curr,2))+add_s) + SScost_curr[1:idx_s_end-cost]-=cost + # SScost_curr[1:idx_s_end-cost]-=cost_correction + # selectedStates.statesCost=vcat(selectedStates.statesCost,SScost_curr[idx_s_start:cost],SScost_curr[1:idx_s_end-cost]) + selectedStates.statesCost=vcat(selectedStates.statesCost,(selectedStates.Np:-1:1)+cost) + else + # println("else:","idx_start:",idx_s_start," idx_end:",idx_s_end,"lap cost",cost) + if s>track.s-v*dt*(N) + SS_curr[idx_s_start:idx_s_end,1]+=track.s + add_s = hcat(track.s*ones(idx_s_end-idx_s_start+1),zeros(idx_s_end-idx_s_start+1,size(SS_curr,2)-1)) + # selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[idx_s_start:idx_s_end,:]) + selectedStates.selStates=vcat(selectedStates.selStates,reshape(SS_next[idx_s_start:idx_s_end,:],idx_s_end-idx_s_start+1,size(SS_curr,2))+add_s) + # selectedStates.statesCost=vcat(selectedStates.statesCost,SScost_curr[idx_s_start:idx_s_end]) + selectedStates.statesCost=vcat(selectedStates.statesCost,(selectedStates.Np:-1:1)+cost) + else + selectedStates.selStates=vcat(selectedStates.selStates,SS_curr[idx_s_start:idx_s_end,:]) + # selectedStates.statesCost=vcat(selectedStates.statesCost,SScost_curr[idx_s_start:idx_s_end]) + selectedStates.statesCost=vcat(selectedStates.statesCost,(selectedStates.Np:-1:1)+cost) + end + end + end + selectedStates.statesCost[:] -= cost_correction + return selectedStates +end + +function find_feature_space(solHistory::SolHistory,z_curr::Array{Float64,2},u_curr::Array{Float64,2},lapStatus::LapStatus,selectedStates::SelectedStates,posInfo::PosInfo) + n_front = Int(round(0.66*selectedStates.feature_Np)) + n_rear = Int(round(0.44*selectedStates.feature_Np)) + Nl = selectedStates.feature_Nl + dummy_state=Array{Float64}(0,6) + dummy_input=Array{Float64}(0,2) + + iden_z =zeros(Nl*(n_front+n_rear+1)-1,3,2) + iden_z_plot =zeros(Nl*(n_front+n_rear+1)-1,6) + iden_u =zeros(Nl*(n_front+n_rear+1)-1,2) + + for i = lapStatus.currentLap-Nl:lapStatus.currentLap-1 + state=reshape(solHistory.z[2:Int(solHistory.cost[i]),i,1,:],Int(solHistory.cost[i])-1,6) + input=reshape(solHistory.u[1:Int(solHistory.cost[i])-1,i,2,:],Int(solHistory.cost[i])-1,2) + state_rear = state[end-n_rear:end,:] + state_rear[:,1] -= posInfo.s_target + state_front = state[1:n_front,:] + state_front[:,1] += posInfo.s_target + state=vcat(state_rear,state,state_front) + (~,idx)=findmin(abs(state[:,1]-z_curr[1])) + # println(state[1:20,1]) + # println(z_curr) + dummy_state=vcat(dummy_state,state[idx-n_rear:idx+n_front,:]) + dummy_input=vcat(dummy_input,input[idx-n_rear:idx+n_front,:]) + end + iden_z[:,:,1] = dummy_state[1:end-1,4:6] + iden_z[:,:,1] = dummy_state[2:end,4:6] + iden_u[:,:] = dummy_input[1:end-1,:] + iden_z_plot[:,:] = dummy_state[1:end-1,:] + return iden_z, iden_u, iden_z_plot +end + +function InitializeParameters(mpcParams::MpcParams,mpcParams_4s::MpcParams,mpcParams_pF::MpcParams,modelParams::ModelParams,mpcSol::MpcSol, + selectedStates::SelectedStates,oldSS::SafeSetData,oldTraj::OldTrajectory,mpcCoeff::MpcCoeff,mpcCoeff_dummy::MpcCoeff, + LMPC_LAP::Int64,delay_df::Int64,delay_a::Int64,N::Int64,BUFFERSIZE::Int64) + simulator_flag = get_param("sim_flag") + + if simulator_flag # if the simulator is in use + + mpcParams.N = N + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 1.0*[0,0.1,0.1,2,0.1,0.0] # cost matrix for derivative cost of states + mpcParams.QderivU = 1.0*[0.01,0.5] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 2.0 # scaling of Q-function + mpcParams.delay_df = delay_df # steering delay + mpcParams.delay_a = delay_a # acceleration delay + mpcParams.Q_lane = 10 # weight on the soft constraint for the lane + mpcParams.Q_slack = 50.0*[1.0,5.0,5.0,1.0,1.0,1.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #s,ey,epsi,vx,vy,psiDot + + mpcParams_4s.N = N + mpcParams_4s.R = 0.0*[1,1] + mpcParams_4s.QderivZ = 1.0*[0,0.1,0.1,2] + mpcParams_4s.QderivU = 1.0*[0.1,1] + mpcParams_4s.Q_term_cost = 1.0 # scaling of Q-function + mpcParams_4s.Q_lane = 10.0 # weight on the soft constraint for the lane bounds + mpcParams_4s.Q_slack = 50.0*[1,1,1,1] + mpcParams_4s.delay_df = delay_df # steering delay + mpcParams_4s.delay_a = delay_a # acceleration delay + + mpcParams_pF.N = N + mpcParams_pF.Q = [0.0,50.0,5.0,20.0] + mpcParams_pF.R = 0*[1.0,1.0] # put weights on a and d_f + mpcParams_pF.QderivZ = 1.0*[0.0,0,1.0,0] # cost matrix for derivative cost of states + mpcParams_pF.QderivU = 1*[2,1] # cost matrix for derivative cost of inputs + mpcParams_pF.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams_pF.delay_df = delay_df # steering delay (number of steps) + mpcParams_pF.delay_a = delay_a # acceleration delay + + modelParams.c_f = 0.05 + + else # if the BARC is in use + + mpcParams.N = N + mpcParams.Q = [5.0,0.0,0.0,0.1,50.0,0.0] # Q (only for path following mode) + mpcParams.vPathFollowing = 1.0 # reference speed for first lap of path following + mpcParams.R = 0*[10.0,10.0] # put weights on a and d_f + mpcParams.QderivZ = 10.0*[0,0,1,1,1,1] # cost matrix for derivative cost of states + mpcParams.QderivU = 1*[1.0,5.0] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + mpcParams.Q_term_cost = 0.1 # scaling of Q-function + mpcParams.delay_df = delay_df # steering delay + mpcParams.delay_a = delay_a # acceleration delay + mpcParams.Q_lane = 16 # weight on the soft constraint for the lane + mpcParams.Q_slack = 3.0*[20.0,20.0,20.0,5.0,5.0,5.0]#[20.0,10.0,10.0,30.0,80.0,50.0] #s,ey,epsi,vx,vy,psiDot + + mpcParams_4s.N = N + mpcParams_4s.R = 0.0*[1,1] + mpcParams_4s.QderivZ = 1.0*[0,0.1,0.1,2] + mpcParams_4s.QderivU = 1.0*[1.0,0.5] + mpcParams_4s.Q_term_cost = 0.5 # scaling of Q-function + mpcParams_4s.Q_lane = 10.0 # weight on the soft constraint for the lane bounds + mpcParams_4s.Q_slack = 30.0*[1,1,1,1] + mpcParams_4s.delay_df = delay_df # steering delay + mpcParams_4s.delay_a = delay_a # acceleration delay + + mpcParams_pF.N = N + mpcParams_pF.Q = [0.0,20.0,2.0,10.0] + mpcParams_pF.R = 2*[1.0,0.1] # put weights on a and d_f + mpcParams_pF.QderivZ = 1.0*[0.0,1.0,1.0,1.0] # cost matrix for derivative cost of states + mpcParams_pF.QderivU = 0.1*[1,1] # cost matrix for derivative cost of inputs + mpcParams_pF.vPathFollowing = 1.3 # reference speed for first lap of path following + mpcParams_pF.delay_df = delay_df # steering delay (number of steps) + mpcParams_pF.delay_a = delay_a # acceleration delay + + modelParams.c_f = 0.5 + + end + + selectedStates.Np = 10 # please select an even number + selectedStates.Nl = 10 # Number of previous laps to include in the convex hull + selectedStates.feature_Np = 60 # Number of points from previous laps to do SYS_ID + selectedStates.feature_Nl = 2 # Number of previous laps to do SYS_ID + selectedStates.selStates = zeros(selectedStates.Nl*selectedStates.Np,6) + selectedStates.statesCost = zeros(selectedStates.Nl*selectedStates.Np) + + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.dt = 0.1 + modelParams.m = 1.98 + modelParams.I_z = 0.03 + + num_lap = 1 + selectedStates.Nl + LMPC_LAP + + oldSS.oldSS = NaN*ones(BUFFERSIZE,6,num_lap) # contains data from previous laps usefull to build the safe set + oldSS.oldSS_xy = NaN*ones(BUFFERSIZE,4,num_lap) + oldSS.cost2target = zeros(BUFFERSIZE,num_lap) # cost to arrive at the target, i.e. how many iterations from the start to the end of the lap + oldSS.oldCost = ones(Int64,num_lap) # contains costs of laps + oldSS.count = ones(num_lap)*2 # contains the counter for each lap + + oldTraj.oldTraj = NaN*ones(5*BUFFERSIZE,7,num_lap) + oldTraj.count = ones(num_lap) + + mpcCoeff.c_Vx = zeros(mpcParams.N,6) + mpcCoeff.c_Vy = zeros(mpcParams.N,4) + mpcCoeff.c_Psi = zeros(mpcParams.N,3) + + # mpcCoeff.c_Vx = zeros(mpcParams.N,3) + # mpcCoeff.c_Vy = zeros(mpcParams.N) + # mpcCoeff.c_Psi = zeros(mpcParams.N) + + mpcCoeff_dummy.c_Vx = zeros(1,6) + mpcCoeff_dummy.c_Vy = zeros(1,4) + mpcCoeff_dummy.c_Psi = zeros(1,3) + + # mpcCoeff_dummy.c_Vx = zeros(1,6) + # mpcCoeff_dummy.c_Vy = zeros(1) + # mpcCoeff_dummy.c_Psi = zeros(1) + + mpcSol.z = zeros(N+1,4) + mpcSol.u = hcat(0.5*ones(N),zeros(N)) + mpcSol.z[1,4] = 1.0 # give the vehcile some initial speed to simulate forward + for i in 2:N+1 + mpcSol.z[i,4]=1.0 + mpcSol.z[i,1]=mpcSol.z[i-1,1]+0.1 + end + mpcSol.a_x = 0 + mpcSol.d_f = 0 + mpcSol.df_his = zeros(delay_df) # DELAT COMES FROM TWO PARTS, ONLY THE SYSTEM DELAY NEEDS TO BE CONSIDERED + mpcSol.a_his = zeros(delay_a) # DELAT COMES FROM TWO PARTS, ONLY THE SYSTEM DELAY NEEDS TO BE CONSIDERED +end + +function s6_to_s4(z::Array{Float64}) + if size(z,1)==1 + z=vcat(z[1:3],sqrt(z[4]^2+z[5]^2)) + else # size(z,1)==2 + z=hcat(z[:,1:3],sqrt(z[:,4].^2+z[:,5].^2)) + # else + # error("Please check the input dimension of function \"6s_to_4s\" ") + end + return z +end + +# FUNCTIONS FOR MODEL SIM +function car_sim_kin(z::Array{Float64},u::Array{Float64},track::Track,modelParams::ModelParams) + # Car parameters + dt = modelParams.dt + L_a = modelParams.l_A + L_b = modelParams.l_B + + idx=Int(ceil(z[1]/track.ds))+1 # correct the starting original point idx problem + idx>track.n_node ? idx=idx%track.n_node : nothing + idx<0 ? idx+=track.n_node : nothing + + # println(idx) + # println(track.n_node) + c=track.curvature[idx] + + bta = atan(L_a/(L_a+L_b)*tan(u[2])) + dsdt = z[4]*cos(z[3]+bta)/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt*dsdt # s + zNext[2] = z[2] + dt*z[4] * sin(z[3] + bta) # eY + zNext[3] = z[3] + dt*(z[4]/L_b*sin(bta)-dsdt*c) # ePsi + zNext[4] = z[4] + dt*(u[1] - modelParams.c_f*z[4]) # v + + return zNext +end + +function car_pre_dyn(z_curr::Array{Float64},u_sol::Array{Float64},track::Track,modelParams::ModelParams,outputDims::Int64) + if size(u_sol,1)==1 + (z_dummy, Fy_dummy, a_slip_dummy)=car_sim_dyn_exact(z_curr,u_sol,track,modelParams) + if outputDims==4 + z_dummy=s6_to_s4(z_dummy) + end + elseif size(u_sol,2)==2 + z_dummy=zeros(size(u_sol,1)+1,6) + Fy_dummy=zeros(size(u_sol,1)+1,2) + a_slip_dummy=zeros(size(u_sol,1)+1,2) + + z_dummy[1,:]=z_curr + for i=2:size(u_sol,1)+1 + (z_dummy[i,:], Fy_dummy[i,:], a_slip_dummy[i,:])=car_sim_dyn_exact(z_dummy[i-1,:],u_sol[i-1,:],track,modelParams) + end + if outputDims==4 + z_dummy=s6_to_s4(z_dummy) + end + else + error("Please check the u_sol dimension of function \"car_pre_dyn\" ") + end + + return z_dummy, Fy_dummy, a_slip_dummy +end + +function car_pre_dyn_true(z_curr::Array{Float64},u_sol::Array{Float64},track::Track,modelParams::ModelParams,outputDims::Int64) + if size(u_sol,1)==1 + (z_dummy, Fy_dummy, a_slip_dummy)=car_sim_dyn_exact(z_curr,u_sol,track,modelParams) + if outputDims==4 + z_dummy=s6_to_s4(z_dummy) + end + elseif size(u_sol,2)==2 + z_dummy=zeros(size(u_sol,1)+1,6) + Fy_dummy=zeros(size(u_sol,1)+1,2) + a_slip_dummy=zeros(size(u_sol,1)+1,2) + + z_dummy[1,:]=z_curr + for i=2:size(u_sol,1)+1 + (z_dummy[i,:], Fy_dummy[i,:], a_slip_dummy[i,:])=car_sim_dyn_exact_true(z_dummy[i-1,:],u_sol[i-1,:],track,modelParams) + end + if outputDims==4 + z_dummy=s6_to_s4(z_dummy) + end + else + error("Please check the u_sol dimension of function \"car_pre_dyn\" ") + end + + return z_dummy, Fy_dummy, a_slip_dummy +end + +function car_sim_dyn_exact_true(z::Array{Float64,2},u::Array{Float64,2},track::Track,modelParams::ModelParams) + # This function uses smaller steps to achieve higher fidelity than we would achieve using longer timesteps + z_final = copy(z) + Fy=zeros(2); a_slip=zeros(2); + u[1] = min(u[1],2) + u[1] = max(u[1],-1) + u[2] = min(u[2],0.1*pi) + u[2] = max(u[2],-0.1*pi) + dt=modelParams.dt; dtn=dt/10 + for i=1:10 + (z_final, Fy, a_slip)= car_sim_dyn(z_final,u,dtn,track,modelParams) + end + return z_final, Fy, a_slip +end + +function car_sim_dyn_exact(z::Array{Float64,2},u::Array{Float64,2},track::Track,modelParams::ModelParams) + # This function uses smaller steps to achieve higher fidelity than we would achieve using longer timesteps + z_final = copy(z) + Fy=zeros(2); a_slip=zeros(2); + u[1] = min(u[1],2) + u[1] = max(u[1],-1) + u[2] = min(u[2],0.1*pi) + u[2] = max(u[2],-0.1*pi) + dt=modelParams.dt; dtn=dt/10 + for i=1:10 + (z_final, Fy, a_slip)= car_sim_dyn(z_final,u,dtn,track,modelParams) + end + return z_final, Fy, a_slip +end + +function car_sim_dyn_true(z::Array{Float64},u::Array{Float64},dt::Float64,track::Track,modelParams::ModelParams) + # s, ey, epsi, vx, vy, Psidot + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f # motor drag coefficient + + a_F = 0 # front tire slip angle + a_R = 0 # rear tire slip angle + if abs(z[4]) >= 0.1 + a_F = atan((z[5] + L_f*z[6])/abs(z[4])) - u[2] + a_R = atan((z[5] - L_r*z[6])/abs(z[4])) + else + warn("too low speed, not able to simulate the dynamic model") + end + if max(abs(a_F),abs(a_R))>30/180*pi + # warn("Large tire angles: a_F = $a_F, a_R = $a_R, xDot = $(z[4]), d_F = $(u[2])") + end + + FyF = -pacejka_true(a_F) + FyR = -pacejka_true(a_R) + + idx=Int(ceil(z[1]/track.ds))+1 # correct the starting original point idx problem + # println(z[1]) + # println(idx) + idx>track.n_node ? idx=idx%track.n_node : nothing + idx<=0 ? idx += track.n_node : nothing + # println(idx) + + # println(track.n_node) + c=track.curvature[idx] + + + dsdt = (z[4]*cos(z[3]) - z[5]*sin(z[3]))/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt * dsdt # s + zNext[2] = z[2] + dt * (z[4]*sin(z[3]) + z[5]*cos(z[3])) # eY + zNext[3] = z[3] + dt * (z[6]-dsdt*c) # ePsi + zNext[4] = z[4] + dt * (u[1] + z[5]*z[6] - c_f*z[4]) # vx + zNext[5] = z[5] + dt * ((FyF*cos(u[2])+FyR)/m - z[4]*z[6]) # vy + zNext[6] = z[6] + dt * ((L_f*FyF*cos(u[2]) - L_r*FyR)/I_z) # psiDot + + return zNext, [FyF,FyR], [a_F,a_R] +end + +function car_sim_dyn(z::Array{Float64},u::Array{Float64},dt::Float64,track::Track,modelParams::ModelParams) + # s, ey, epsi, vx, vy, Psidot + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f # motor drag coefficient + + a_F = 0 # front tire slip angle + a_R = 0 # rear tire slip angle + if abs(z[4]) >= 0.1 + a_F = atan((z[5] + L_f*z[6])/abs(z[4])) - u[2] + a_R = atan((z[5] - L_r*z[6])/abs(z[4])) + else + warn("too low speed, not able to simulate the dynamic model") + end + if max(abs(a_F),abs(a_R))>30/180*pi + # warn("Large tire angles: a_F = $a_F, a_R = $a_R, xDot = $(z[4]), d_F = $(u[2])") + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + idx=Int(ceil(z[1]/track.ds))+1 # correct the starting original point idx problem + # println(z[1]) + # println(idx) + idx>track.n_node ? idx=idx%track.n_node : nothing + idx<=0 ? idx += track.n_node : nothing + # println(idx) + + # println(track.n_node) + c=track.curvature[idx] + + + dsdt = (z[4]*cos(z[3]) - z[5]*sin(z[3]))/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt * dsdt # s + zNext[2] = z[2] + dt * (z[4]*sin(z[3]) + z[5]*cos(z[3])) # eY + zNext[3] = z[3] + dt * (z[6]-dsdt*c) # ePsi + zNext[4] = z[4] + dt * (u[1] + z[5]*z[6] - c_f*z[4]) # vx + zNext[5] = z[5] + dt * ((FyF*cos(u[2])+FyR)/m - z[4]*z[6]) # vy + zNext[6] = z[6] + dt * ((L_f*FyF*cos(u[2]) - L_r*FyR)/I_z) # psiDot + + return zNext, [FyF,FyR], [a_F,a_R] +end + +function pacejka_true(a) + # B = 1.0 # This value determines the steepness of the curve + # C = 1.25 + B = 6.0 # This value determines the steepness of the curve + C = 1.6 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + C_alpha_f = D*sin.(C*atan.(B*a)) + return C_alpha_f +end + +function pacejka(a) + # B = 1.0 # This value determines the steepness of the curve + # C = 1.25 + B = 6.0 # This value determines the steepness of the curve + C = 1.6 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + # C_alpha_f = D*sin.(C*atan.(B*a)) + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function car_sim_iden_tv(z::Array{Float64},u::Array{Float64},dt::Float64,mpcCoeff::MpcCoeff,modelParams::ModelParams,track::Track) + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f + + c_Vx=mpcCoeff.c_Vx + c_Vy=mpcCoeff.c_Vy + c_Psi=mpcCoeff.c_Psi + + idx=Int(ceil(z[1]/track.ds))+1 # correct the starting original point idx problem + idx>track.n_node ? idx=idx%track.n_node : nothing + idx<=0 ? idx+=track.n_node : nothing + + c=track.curvature[idx] + dsdt = (z[4]*cos(z[3]) - z[5]*sin(z[3]))/(1-z[2]*c) + + z_next=copy(z) + + z_next[1] = z_next[1] + dt * dsdt # s + z_next[2] = z_next[2] + dt * (z[4]*sin(z[3]) + z[5]*cos(z[3])) # eY + z_next[3] = z_next[3] + dt * (z[6]-dsdt*c) # ePsi + z_next[4] = z_next[4] + c_Vx[1]*z[5]*z[6] + c_Vx[2]*z[4] + c_Vx[3]*u[1] # vx + z_next[5] = z_next[5] + c_Vy[1]*z[5]/z[4] + c_Vy[2]*z[4]*z[6] + c_Vy[3]*z[6]/z[4] + c_Vy[4]*u[2] # vy + z_next[6] = z_next[6] + c_Psi[1]*z[6]/z[4] + c_Psi[2]*z[5]/z[4] + c_Psi[3]*u[2] # psiDot + return z_next +end + +# FUNCTIONS FOR GP REGRESSION +function covar_fun(z1::Array{Float64,2},z2::Array{Float64,2}) + # input: [vx,vy,psi_dot,a,df] + z = z1-z2 + # L[1,1]=0.1; + # L[2,2]=5; + # L[3,3]=5; + # L[4,4]=0.1; + # L[5,5]=5 + k = exp(-0.5*(0.1*z[1]^2+5*z[2]^2+5*z[3]^2+0.1*z[4]^2+5*z[5]^2)) + return k +end + +function regre(z::Array{Float64,2},u::Array{Float64,2},e::Array{Float64,1},s::Array{Float64,2},i::Array{Float64,2}) + # find the closest local feature points + dummy_norm = zeros(size(s,1),2) + state = hcat(z,u) + feature_state = hcat(s,i) + dist = state[4:8]'.-feature_state[:,4:8] + # We have 5 dimensions for distance calculation + dummy_norm[:,1] = dist[:,1].^2+(5*dist[:,5]).^2#+dist[:,3].^2+dist[:,4].^2+dist[:,5].^2 + dummy_norm[:,2] = 1:size(dummy_norm,1) + dummy_norm=sortrows(dummy_norm) # pick up the first minimum Np points + + # do local GP + # pick the first 20 points for local GP + num = 20 + K=zeros(num,num) + k=zeros(num) + y=zeros(num) + for i = 1:num + for j=1:num + z = feature_state[Int(dummy_norm[i,2]),4:8] - feature_state[Int(dummy_norm[j,2]),4:8] + K[i,j] = exp(-0.5*(0.1*z[1]^2+5*z[2]^2+5*z[3]^2+0.1*z[4]^2+5*z[5]^2)) + # K[i,j]=covar_fun(feature_state[Int(dummy_norm[i,2]),4:8],feature_state[Int(dummy_norm[j,2]),4:8]) + end + z = feature_state[Int(dummy_norm[i,2]),4:8]-state[4:8]' + k[i] = exp(-0.5*(0.1*z[1]^2+5*z[2]^2+5*z[3]^2+0.1*z[4]^2+5*z[5]^2)) + # k[i]=covar_fun(feature_state[Int(dummy_norm[i,2]),4:8],state[4:8]') + y[i]=e[Int(dummy_norm[i,2])] + # println(feature_state[Int(dummy_norm[i,2]),4:8]) + end + + local_e = k'*(K\y) + return local_e[1] +end + +function GP_prepare(e::Array{Float64,1},s::Array{Float64,2},i::Array{Float64,2}) + # find the closest local feature points + feature_state = hcat(s,i) + num = size(feature_state,1) + # pick the first 20 points for local GP + # y=zeros(num) + # L = eye(5); + # L[1,1]=0.1; + # L[2,2]=5; + # L[3,3]=5; + # L[4,4]=0.1; + # L[5,5]=5 + Z=zeros(num,num) + for i = 1:num + for j=1:num + z = feature_state[i,4:8]-feature_state[j,4:8] + Z[i,j] = (0.5*z[1]^2+5*z[2]^2+5*z[3]^2+0.5*z[4]^2+5*z[5]^2) + end + # y[i]=e[i] + end + K=exp(-0.5*Z) + return K\e +end + +function GP_full_vy(z::Array{Float64,2},u::Array{Float64,2},feature_state::Array{Float64,2},GP_prepare::Array{Float64,1}) + state = hcat(z,u) + z = feature_state[:,4:8].-state[1,4:8] + Z = 0.5*z[:,1].^2+5*z[:,2].^2+5*z[:,3].^2+0.5*z[:,4].^2+5*z[:,5].^2 + # Z = z[:,1].^2+z[:,2].^2+z[:,3].^2+z[:,4].^2+z[:,5].^2 + k = 0.1^2*exp(-0.5*Z) + GP_e = k'*GP_prepare + return GP_e[1] +end + +function GP_full_psidot(z::Array{Float64,2},u::Array{Float64,2},feature_state::Array{Float64,2},GP_prepare::Array{Float64,1}) + state = hcat(z,u) + z = feature_state[:,4:8].-state[1,4:8] + Z = 0.5*z[:,1].^2+5*z[:,2].^2+5*z[:,3].^2+0.5*z[:,4].^2+5*z[:,5].^2 + # Z = z[:,1].^2+z[:,2].^2+z[:,3].^2+z[:,4].^2+z[:,5].^2 + k = 0.1^2*exp(-0.5*Z) + GP_e = k'*GP_prepare + return GP_e[1] +end + +function createTrack(name::ASCIIString) + # RACING TRACK DATA + if name == "race" + # TRACK USED IN MY SIMULATION + track_data=[80 0; + 120 -pi/2; + 80 0; + 220 -pi*0.85; + 105 pi/15; + 300 pi*1.15; + 240 -pi*0.865; + 100 0; + 120 -pi/2; + 153 0; + 120 -pi/2; + 211 0] + elseif name == "3110" + # EXPERIEMENT TRACK DATA + num = 100 + track_data=[Int(ceil(2*80)) 0; + Int(ceil(2*num)) pi/2; + Int(ceil(2*(80+47))) 0; + Int(ceil(2*num)) pi/2; + Int(ceil(2*50)) 0; + Int(ceil(2*num)) pi/2; + Int(ceil(2*4)) 0; + Int(ceil(2*num)) -pi/2; + Int(ceil(2*30)) 0; + Int(ceil(2*num)) pi/2; + Int(ceil(2*4)) 0; + Int(ceil(2*num)) pi/2; + Int(ceil(2*(71+48))) 0] + elseif name == "basic" + # Basic experiment track + track_data = [Int(ceil(3*60)) 0; + Int(ceil(3*80)) pi/2; + Int(ceil(3*20)) 0; + Int(ceil(3*80)) pi/2; + Int(ceil(3*40)) -pi/10; + Int(ceil(3*60)) pi/5; + Int(ceil(3*40)) -pi/10; + Int(ceil(3*80)) pi/2; + Int(ceil(3*20)) 0; + Int(ceil(3*80)) pi/2; + Int(ceil(3*75)) 0] + + # track_data = [Int(ceil(2.8*40)) 0; + # Int(ceil(2.8*120)) -pi/2; + # Int(ceil(2.8*5)) 0; + # Int(ceil(2.8*120)) -pi/2; + # Int(ceil(2.8*80)) 0; + # Int(ceil(2.8*120)) -pi/2; + # Int(ceil(2.8*5)) 0; + # Int(ceil(2.8*120)) -pi/2; + # Int(ceil(2.8*40)) 0] + elseif name == "MSC_lab" + # TRACK TO USE IN THE SMALL EXPERIMENT ROOM + track_data = [Int(ceil(1.5*3*10)) 0; + Int(ceil(1.5*3*120)) pi; + Int(ceil(1.5*3*20)) 0; + Int(ceil(1.5*3*120)) pi; + Int(ceil(1.5*3*10)) 0] + elseif name == "feature" + # FEATURE TRACK DATA + ds = 0.01 + v = 2.5 + max_a=7.6; + R=v^2/max_a + max_c=1/R + angle=(pi+pi/2)-0.105 + R_kin = 0.8 + num_kin = Int(round(angle/ ( ds/R_kin ) * 2)) + num = max(Int(round(angle/ ( ds/R ) * 2)),num_kin) + num=Int(ceil(num*1.0)) + track_data=[num -angle; + num angle] + else + error("Please input the correct track name") + end + return track_data +end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl new file mode 100644 index 00000000..5e62c693 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/LMPC/solveMpcProblem.jl @@ -0,0 +1,509 @@ +# Variable definitions +# mdl.z_Ol[i,j] = z_OpenLoop, open loop prediction of the state, i = state, j = step + +# States in path following mode: +# i = 1 -> s +# i = 2 -> ey +# i = 3 -> epsi +# i = 4 -> v + +# States in LMPC and system ID mode: +# i = 1 -> xDot +# i = 2 -> yDot +# i = 3 -> psiDot +# i = 4 -> ePsi +# i = 5 -> eY +# i = 6 -> s + +function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcParams_pF::MpcParams,modelParams::ModelParams,mpcSol::MpcSol, + z_curr::Array{Float64,1}, + z_prev::Array{Float64,2},u_prev::Array{Float64,2},track::Track) + + # println("from decision variable",getvalue(mdl.u_Ol[1:3,2])) + # println("from nonlinear paramet",getvalue(mdl.df_his)) + + # Load Parameters + v_ref=mpcParams_pF.vPathFollowing + width=0.8*0.9 # 0.9 is to give some margin for safety due tp hard constraint for simple path following case + + z_final=car_sim_kin(z_prev[end,1:4],u_prev[end,:],track,modelParams) + z_curvature=vcat(z_curr',z_prev[3:end,1:4],z_final) + # println("u_prev is $(u_prev[:,1])") + # println("s is $(z_curvature[:,1])") + curvature=curvature_prediction(z_curvature,track) + # println("Predicted curvature",round(curvature,2)) + # println("Predicted ey",round(z_curvature[:,2],2)) + # println("Predicted u[2]",round(u_prev[:,2],2)) + # println("predicted curvature is $(curvature)") + ey_ref=zeros(mpcParams_pF.N+1) + for i=1:mpcParams_pF.N+1 + ey_ref[i]=-curvature[i]/track.max_curvature*width/2 + end + # z_ref = hcat(zeros(mpcParams_pF.N+1,1),ey_ref,zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) + z_ref = hcat(zeros(mpcParams_pF.N+1,1),zeros(mpcParams_pF.N+1),zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) + + + # println("z_ref is: $z_ref") + + # Update current initial condition, curvature and previous input + setvalue(mdl.z0,z_curr) + setvalue(mdl.c,curvature) + + setvalue(mdl.uPrev,u_prev) + setvalue(mdl.df_his,mpcSol.df_his) + setvalue(mdl.a_his,mpcSol.a_his) + # println("a_his",mpcSol.a_his) + setvalue(mdl.z_Ref,z_ref) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_featureData(mdl::MpcModel_pF,mpcParams_pF::MpcParams,modelParams::ModelParams,mpcSol::MpcSol, + z_curr::Array{Float64,1}, + z_prev::Array{Float64,2},u_prev::Array{Float64,2},track::Track,v_ref::Float64) + + z_final=car_sim_kin(z_prev[end,:],u_prev[end,:],track,modelParams) + # println("z_curr is $z_curr") + # println("z_prev is $z_prev") + # println("z_final is $z_final") + z_curvature=vcat(z_curr',z_prev[3:end,:],z_final) + + curvature=curvature_prediction(z_curvature,track) + z_ref = hcat(zeros(mpcParams_pF.N+1,3),v_ref*ones(mpcParams_pF.N+1,1)) + + # Update current initial condition, curvature and previous input + setvalue(mdl.z0,z_curr) + setvalue(mdl.c,curvature) + setvalue(mdl.df_his,mpcSol.df_his) + setvalue(mdl.uPrev,u_prev) + setvalue(mdl.z_Ref,z_ref) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_convhull_dyn_iden(mdl::MpcModel_convhull_dyn_iden,mpcSol::MpcSol, + mpcCoeff::MpcCoeff,zCurr::Array{Float64,1}, + zPrev::Array{Float64,2},uPrev::Array{Float64,2},selectedStates::SelectedStates,track::Track, + GP_e_vy::Array{Float64,1},GP_e_psidot::Array{Float64,1}) + + # IMPORTANT: this warm start must be done manually when swiching the lap, but here, this warm start is done in the swiching lap section outside + # setvalue(mdl.z_Ol,vcat(zPrev[2:end,:],zPrev[end,:])) + # setvalue(mdl.u_Ol,vcat(uPrev[2:end,:],uPrev[end,:])) + + # zeros in the model initialization is dangerous for optimization: invalid number might occur when divided by zero happends + + selStates = selectedStates.selStates + statesCost = selectedStates.statesCost + + z_curvature=vcat(zCurr',zPrev[3:end,:]) + curvature=curvature_prediction(z_curvature,track) + + # Update current initial condition, curvature and System ID coefficients + setvalue(mdl.z0,zCurr) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.df_his,mpcSol.df_his) + setvalue(mdl.c,curvature) # Track curvature + setvalue(mdl.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(mdl.c_Vy,mpcCoeff.c_Vy) + setvalue(mdl.c_Psi,mpcCoeff.c_Psi) + setvalue(mdl.selStates,selStates) + setvalue(mdl.statesCost,statesCost) + setvalue(mdl.GP_e_vy,GP_e_vy) + setvalue(mdl.GP_e_psidot,GP_e_psidot) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_convhull_dyn_iden_simple(mdl::MpcModel_convhull_dyn_iden_simple,mpcSol::MpcSol, + mpcCoeff::MpcCoeff,zCurr::Array{Float64,1}, + zPrev::Array{Float64,2},uPrev::Array{Float64,2},selectedStates::SelectedStates,track::Track, + GP_e_vy::Array{Float64,1},GP_e_psidot::Array{Float64,1}) + + # IMPORTANT: this warm start must be done manually when swiching the lap, but here, this warm start is done in the swiching lap section outside + # setvalue(mdl.z_Ol,vcat(zPrev[2:end,:],zPrev[end,:])) + # setvalue(mdl.u_Ol,vcat(uPrev[2:end,:],uPrev[end,:])) + + # zeros in the model initialization is dangerous for optimization: invalid number might occur when divided by zero happends + + selStates = selectedStates.selStates + statesCost = selectedStates.statesCost + + z_curvature=vcat(zCurr',zPrev[3:end,:]) + curvature=curvature_prediction(z_curvature,track) + + # Update current initial condition, curvature and System ID coefficients + setvalue(mdl.z0,zCurr) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.df_his,mpcSol.df_his) + setvalue(mdl.c,curvature) # Track curvature + setvalue(mdl.c_Vx,mpcCoeff.c_Vx) # System ID coefficients + setvalue(mdl.c_Vy,mpcCoeff.c_Vy) + setvalue(mdl.psiDot,mpcCoeff.c_Psi) + setvalue(mdl.selStates,selStates) + setvalue(mdl.statesCost,statesCost) + setvalue(mdl.GP_e_vy,GP_e_vy) + setvalue(mdl.GP_e_psidot,GP_e_psidot) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_convhull_kin_linear(mdl::MpcModel_convhull_kin_linear,mpcSol::MpcSol,mpcParams::MpcParams,modelParams::ModelParams, + z_linear::Array{Float64,2},u_linear::Array{Float64,2}, + zPrev::Array{Float64,2},uPrev::Array{Float64,2}, # this is the delta state hot start + selectedStates::SelectedStates,track::Track) + + # Interface: from absolute value to relative state value + n_state=4; n_input=2; + # warm start: Julia will do the warm start automatically + # if lapStatus.switchingLap + # println("warm start during switching") + # # setvalue(mdl.z_Ol,zPrev[2:end,:]) + # # setvalue(mdl.u_Ol,vcat(uPrev[2:end,:],uPrev[end,:]')) + # lapStatus.switchingLap=false + # end + + selStates = selectedStates.selStates + statesCost = selectedStates.statesCost + + # for i=1:mpcParams.N + # for j=1:n_state + # JuMP.fix(mdl.z_linear[i,j],z_linear[i,j]) + # end + # for j=1:n_input + # JuMP.fix(mdl.u_linear[i,j],u_linear[i,j]) + # end + # end + # for j=1:n_state + # JuMP.fix(mdl.z_linear[mpcParams.N+1,j],z_linear[mpcParams.N+1,j]) + # end + setvalue(mdl.z_linear,z_linear) + setvalue(mdl.u_linear,u_linear) + # println(size(mdl.df_his)) + # println(size(mpcSol.df_his)) + setvalue(mdl.df_his,mpcSol.df_his) + + + # uPrev=u_prev; zPrev=z_prev + # i=1; j=1; k=1; + # mdl=mdl_convhull + + A=zeros(n_state,n_state,mpcParams.N) + B=zeros(n_state,n_input,mpcParams.N) + L_a=modelParams.l_A; L_b=modelParams.l_B + dt=modelParams.dt; c_f=modelParams.c_f + curvature=curvature_prediction(z_linear,track) + for k=1:mpcParams.N + bta_val=atan((L_a*tan(u_linear[k,2]))/(L_a + L_b)) + + # A[1,1,k]= dt*z_linear[k,2]*z_linear[k,4]*cos(z_linear[k,3] + bta_val)*curvature_deri/(z_linear[k,2]*(curvature) - 1)^2 + 1 + A[1,1,k]= 1 + A[1,2,k]= dt*z_linear[k,4]*cos(z_linear[k,3] + bta_val)*curvature[k]/(z_linear[k,2]*(curvature[k]) - 1)^2 + A[1,3,k]= dt*z_linear[k,4]*sin(z_linear[k,3] + bta_val)/(z_linear[k,2]*(curvature[k]) - 1) + A[1,4,k]=-dt*cos(z_linear[k,3] + bta_val)/(z_linear[k,2]*(curvature[k]) - 1) + A[2,1,k]= 0 + A[2,2,k]= 1 + A[2,3,k]= dt*z_linear[k,4]*cos(z_linear[k,3] + bta_val) + A[2,4,k]= dt*sin(z_linear[k,3] + bta_val) + # A[3,1,k]= dt*( (z_linear[k,4]*cos(z_linear[k,3] + bta_val)*curvature_deri)/(z_linear[k,2]*curvature - 1)- + # (z_linear[k,2]*z_linear[k,4]*cos(z_linear[k,3] + bta_val)*curvature_deri*curvature)/(z_linear[k,2]*curvature - 1)^2 ) + A[3,1,k]= 0 + A[3,2,k]=-(dt*z_linear[k,4]*cos(z_linear[k,3] + bta_val)*(curvature[k])^2)/(z_linear[k,2]*(curvature[k]) - 1)^2 + A[3,3,k]=-(dt*z_linear[k,4]*sin(z_linear[k,3] + bta_val)*(curvature[k]))/(z_linear[k,2]*(curvature[k]) - 1) + 1 + A[3,4,k]=dt*((cos(z_linear[k,3] + bta_val)*(curvature[k]))/(z_linear[k,2]*(curvature[k]) - 1) + sin(bta_val)/L_b) + A[4,1,k]= 0 + A[4,2,k]= 0 + A[4,3,k]= 0 + A[4,4,k]= 1-c_f*dt + B[1,1,k]=0 + B[2,1,k]=0 + B[3,1,k]=0 + B[4,1,k]=dt + B[1,2,k]=(L_a*dt*z_linear[k,4]*sin(z_linear[k,3] + atan((L_a*tan(u_linear[k,2]))/(L_a + L_b)))*(tan(u_linear[k,2])^2 + 1))/(((L_a^2*tan(u_linear[k,2])^2)/(L_a + L_b)^2 + 1)*(z_linear[k,2]*curvature[k] - 1)*(L_a + L_b)) + B[2,2,k]=(L_a*dt*z_linear[k,4]*cos(z_linear[k,3] + atan((L_a*tan(u_linear[k,2]))/(L_a + L_b)))*(tan(u_linear[k,2])^2 + 1))/(((L_a^2*tan(u_linear[k,2])^2)/(L_a + L_b)^2 + 1)*(L_a + L_b)) + B[3,2,k]=-dt*((L_a^3*z_linear[k,4]*tan(u_linear[k,2])^2*(tan(u_linear[k,2])^2 + 1))/(L_b*((L_a^2*tan(u_linear[k,2])^2)/(L_a + L_b)^2 + 1)^(3/2)*(L_a + L_b)^3) - (L_a*z_linear[k,4]*(tan(u_linear[k,2])^2 + 1))/(L_b*((L_a^2*tan(u_linear[k,2])^2)/(L_a + L_b)^2 + 1)^(1/2)*(L_a + L_b)) + (L_a*z_linear[k,4]*sin(z_linear[k,3] + atan((L_a*tan(u_linear[k,2]))/(L_a + L_b)))*(tan(u_linear[k,2])^2 + 1)*curvature[k])/(((L_a^2*tan(u_linear[k,2])^2)/(L_a + L_b)^2 + 1)*(z_linear[k,2]*curvature[k] - 1)*(L_a + L_b))) + B[4,2,k]=0 + + setvalue(mdl.A,A) + setvalue(mdl.B,B) + # for i=1:n_state + # for j=1:n_state + # # A[i,j,k] = subs(mdl.A_s[i,j],mdl.z_s[1]=>z_linear[1],mdl.z_s[2]=>z_linear[k,2], + # # mdl.z_s[3]=>z_linear[k,3],mdl.z_s[4]=>z_linear[k,4], + # # mdl.u_s[1]=>u_linear[k,1],mdl.u_s[2]=>u_linear[k,2], + # # mdl.coeff_s[1]=>curvatureCoeff[1],mdl.coeff_s[2]=>curvatureCoeff[2], + # # mdl.coeff_s[3]=>curvatureCoeff[3],mdl.coeff_s[4]=>curvatureCoeff[4], + # # mdl.coeff_s[5]=>curvatureCoeff[5],mdl.coeff_s[6]=>curvatureCoeff[6], + # # mdl.coeff_s[7]=>curvatureCoeff[7]) + # JuMP.fix(mdl.A[i,j,k],A[i,j,k]) + # end + # for j=1:n_input + # # B[i,j,k] = subs(mdl.B_s[i,j],mdl.z_s[1]=>z_linear[k,1],mdl.z_s[2]=>z_linear[k,2], + # # mdl.z_s[3]=>z_linear[k,3],mdl.z_s[4]=>z_linear[k,4], + # # mdl.u_s[1]=>u_linear[k,1],mdl.u_s[2]=>u_linear[k,2], + # # mdl.coeff_s[1]=>curvatureCoeff[1],mdl.coeff_s[2]=>curvatureCoeff[2], + # # mdl.coeff_s[3]=>curvatureCoeff[3],mdl.coeff_s[4]=>curvatureCoeff[4], + # # mdl.coeff_s[5]=>curvatureCoeff[5],mdl.coeff_s[6]=>curvatureCoeff[6], + # # mdl.coeff_s[7]=>curvatureCoeff[7]) + # JuMP.fix(mdl.B[i,j,k],B[i,j,k]) + # end + # end + end + + # for i=1:selectedStates.Nl*selectedStates.Np + # for i=1:size(selectedStates.selStates,1) + # for j=1:n_state + # JuMP.fix(mdl.selStates[i,j],selStates[i,j]) + # end + # JuMP.fix(mdl.statesCost[i],statesCost[i]) + # end + # for i=1:n_state + # JuMP.fix(mdl.zPrev[i],zPrev[1,i]) + # end + # JuMP.fix(mdl.uPrev[1],uPrev[1,1]) + # JuMP.fix(mdl.uPrev[2],uPrev[1,2]) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.selStates,selStates) + setvalue(mdl.statesCost,statesCost) + + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + u_linear + sol_z = vcat(z_linear[1,:], getvalue(mdl.z_Ol) + z_linear[2:end,:]) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_convhull_dyn_linear(mdl::MpcModel_convhull_dyn_linear,mpcSol::MpcSol,mpcParams::MpcParams,modelParams::ModelParams, + lapStatus::LapStatus, + z_linear::Array{Float64,2},u_linear::Array{Float64,2}, + zPrev::Array{Float64,2},uPrev::Array{Float64,2}, # this is the delta state hot start + selectedStates::SelectedStates,track::Track,GP_e_vy::Array{Float64,1},GP_e_psidot::Array{Float64,1}) + + # Interface: from absolute value to relative state value + n_state=6; n_input=2; + # warm start: Julia will do the warm start automatically + # if lapStatus.switchingLap + # println("warm start during switching") + # z_linear[:,1]=-=track.s + # setvalue(mdl.z_Ol,vcat(zPrev[3:end,:],zPrev[end,:])) + # setvalue(mdl.u_Ol,vcat(uPrev[2:end,:],uPrev[end,:])) + # lapStatus.switchingLap=false + # end + + selStates = selectedStates.selStates + statesCost = selectedStates.statesCost + + # for i=1:mpcParams.N + # for j=1:n_state + # JuMP.fix(mdl.z_linear[i,j],z_linear[i,j]) + # end + # for j=1:n_input + # JuMP.fix(mdl.u_linear[i,j],u_linear[i,j]) + # end + # end + # for j=1:n_state + # JuMP.fix(mdl.z_linear[mpcParams.N+1,j],z_linear[mpcParams.N+1,j]) + # end + + z_linear[1:end-1,5] += GP_e_vy + z_linear[1:end-1,6] += GP_e_psidot + + setvalue(mdl.z_linear,z_linear) + setvalue(mdl.u_linear,u_linear) + + # uPrev=u_prev; zPrev=z_prev + # i=1; j=1; k=1; + # mdl=mdl_convhull + L_a=modelParams.l_A; L_b=modelParams.l_B + dt=modelParams.dt; c_f=modelParams.c_f + # THOSE TIRE PARAMETERS MIGHT NEED TO BE CHANGED + m=1.98; I_z=0.03; + B = 6.0; C = 1.6; mu = 0.8; g = 9.81 + A_m=zeros(n_state,n_state,mpcParams.N) + B_m=zeros(n_state,n_input,mpcParams.N) + curvature=curvature_prediction(z_linear,track) + for k=1:mpcParams.N + # curvature=0; curvature_deri=0 + # for i=1:7 + # curvature = curvature + curvatureCoeff[i]*z_linear[k,1]^(i-1); + # curvature_deri = curvature_deri + (i-1)*curvatureCoeff[i]*z_linear[k,1]^(i-2); + # end + # A_m[1,1,k]= 1 + dt*(z_linear[k,4]*cos(z_linear[k,3]) - z_linear[k,5]*sin(z_linear[k,3]))*z_linear[k,2]*curvature_deri/(1 - z_linear[k,2]*curvature)^2 + A_m[1,1,k]= 1 + A_m[1,2,k]= (dt*(z_linear[k,4]*cos(z_linear[k,3]) - z_linear[k,5]*sin(z_linear[k,3]))*curvature[k])/(z_linear[k,2]*curvature[k] - 1)^2 + A_m[1,3,k]= (dt*(z_linear[k,5]*cos(z_linear[k,3]) + z_linear[k,4]*sin(z_linear[k,3])))/(z_linear[k,2]*curvature[k] - 1) + A_m[1,4,k]= -(dt*cos(z_linear[k,3]))/(z_linear[k,2]*curvature[k] - 1) + A_m[1,5,k]= (dt*sin(z_linear[k,3]))/(z_linear[k,2]*curvature[k] - 1) + A_m[1,6,k]= 0 + A_m[2,1,k]= 0 + A_m[2,2,k]= 1 + A_m[2,3,k]= dt*(z_linear[k,4]*cos(z_linear[k,3])-z_linear[k,5]*sin(z_linear[k,3])) + A_m[2,4,k]= dt*sin(z_linear[k,3]) + A_m[2,5,k]= dt*cos(z_linear[k,3]) + A_m[2,6,k]= 0 + # A_m[3,1,k]= dt*(((z_linear[k,4]*cos(z_linear[k,3]) - z_linear[k,5]*sin(z_linear[k,3]))*curvature_deri)/(z_linear[k,2]*curvature - 1) - (z_linear[k,2]*(z_linear[k,4]*cos(z_linear[k,3]) - z_linear[k,5]*sin(z_linear[k,3]))*curvature_deri*curvature)/(z_linear[k,2]*curvature - 1)^2) + A_m[3,1,k]=0 + A_m[3,2,k]= -(dt*(z_linear[k,4]*cos(z_linear[k,3]) - z_linear[k,5]*sin(z_linear[k,3]))*curvature[k]^2)/(z_linear[k,2]*curvature[k] - 1)^2 + A_m[3,3,k]= 1 - (dt*(z_linear[k,5]*cos(z_linear[k,3]) + z_linear[k,4]*sin(z_linear[k,3]))*curvature[k])/(z_linear[k,2]*curvature[k] - 1) + A_m[3,4,k]= (dt*cos(z_linear[k,3])*curvature[k])/(z_linear[k,2]*curvature[k] - 1) + A_m[3,5,k]= -(dt*sin(z_linear[k,3])*curvature[k])/(z_linear[k,2]*curvature[k] - 1) + A_m[3,6,k]= dt + A_m[4,1,k]= 0 + A_m[4,2,k]= 0 + A_m[4,3,k]= 0 + A_m[4,4,k]= 1 - c_f*dt + A_m[4,5,k]= dt*z_linear[k,6] + A_m[4,6,k]= dt*z_linear[k,5] + A_m[5,1,k]= 0 + A_m[5,2,k]= 0 + A_m[5,3,k]= 0 + A_m[5,4,k]= -dt*(z_linear[k,6] + ((B*C*(-g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])))*(z_linear[k,5] - L_a*z_linear[k,6]))/(2*z_linear[k,4]^2*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] -L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) + (B*C*(-g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2])*(z_linear[k,5] + L_a*z_linear[k,6]))/(2*z_linear[k,4]^2*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)))/m) + + A_m[5,5,k]= (dt*((B*C*(-g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4]))))/(2*z_linear[k,4]*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] - L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) + (B*C*(-g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*z_linear[k,4]*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1))))/m + 1 + + A_m[5,6,k]= -dt*(z_linear[k,4] + ((B*C*L_a*(-g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4]))))/(2*z_linear[k,4]*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] - L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) - (B*C*L_a*(-g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*z_linear[k,4]*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)))/m) + + A_m[6,1,k]= 0 + A_m[6,2,k]= 0 + A_m[6,3,k]= 0 + A_m[6,4,k]= -(dt*((B*C*L_a*(g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])))*(z_linear[k,5] - L_a*z_linear[k,6]))/(2*z_linear[k,4]^2*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] - L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) - (B*C*L_a*(g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2])*(z_linear[k,5] + L_a*z_linear[k,6]))/(2*z_linear[k,4]^2*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1))))/I_z + + A_m[6,5,k]= (dt*((B*C*L_a*(g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4]))))/(2*z_linear[k,4]*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] - L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) - (B*C*L_a*(g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*z_linear[k,4]*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1))))/I_z + + + A_m[6,6,k]= -(dt*((B*C*L_a^2*(g*m*mu)*cos(C*atan(B*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4]))))/(2*z_linear[k,4]*(B^2*atan((z_linear[k,5] - L_a*z_linear[k,6])/z_linear[k,4])^2 + 1)*((z_linear[k,5] - L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1)) + (B*C*L_a^2*(g*m*mu)*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*z_linear[k,4]*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1)*((z_linear[k,5] + L_a*z_linear[k,6])^2/z_linear[k,4]^2 + 1))))/I_z +1 + + B_m[1,1,k]= 0 + B_m[1,2,k]= 0 + B_m[2,1,k]= 0 + B_m[2,2,k]= 0 + B_m[3,1,k]= 0 + B_m[3,2,k]= 0 + B_m[4,1,k]= dt + B_m[4,2,k]= 0 + B_m[5,1,k]= 0 + B_m[5,2,k]= -(dt*((g*m*mu*sin(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*sin(u_linear[k,2]))/2 - (B*C*g*m*mu*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1))))/m + B_m[6,1,k]= 0 + B_m[6,2,k]= -(dt*((L_a*g*m*mu*sin(u_linear[k,2])*sin(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4])))))/2 - (B*C*L_a*g*m*mu*cos(C*atan(B*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))))*cos(u_linear[k,2]))/(2*(B^2*(u_linear[k,2] - atan((z_linear[k,5] + L_a*z_linear[k,6])/z_linear[k,4]))^2 + 1))))/I_z + # for i=1:n_state + # for j=1:n_state + # JuMP.fix(mdl.A[i,j,k],A_m[i,j,k]) + # end + # for j=1:n_input + # JuMP.fix(mdl.B[i,j,k],B_m[i,j,k]) + # end + # end + setvalue(mdl.A,A_m) + setvalue(mdl.B,B_m) + end + # for k=1:mpcParams.N + # for i=1:n_state + # for j=1:n_state + # A[i,j,k] = subs(mdl.A_s[i,j],mdl.z_s[1]=>z_linear[k,1],mdl.z_s[2]=>z_linear[k,2], + # mdl.z_s[3]=>z_linear[k,3],mdl.z_s[4]=>z_linear[k,4], + # mdl.z_s[5]=>z_linear[k,5],mdl.z_s[6]=>z_linear[k,6], + # mdl.u_s[1]=>u_linear[k,1],mdl.u_s[2]=>u_linear[k,2], + # mdl.coeff_s[1]=>curvatureCoeff[1],mdl.coeff_s[2]=>curvatureCoeff[2], + # mdl.coeff_s[3]=>curvatureCoeff[3],mdl.coeff_s[4]=>curvatureCoeff[4], + # mdl.coeff_s[5]=>curvatureCoeff[5],mdl.coeff_s[6]=>curvatureCoeff[6], + # mdl.coeff_s[7]=>curvatureCoeff[7]) + # JuMP.fix(mdl.A[i,j,k],A[i,j,k]) + # end + # for j=1:n_input + # B[i,j,k] = subs(mdl.B_s[i,j],mdl.z_s[1]=>z_linear[k,1],mdl.z_s[2]=>z_linear[k,2], + # mdl.z_s[3]=>z_linear[k,3],mdl.z_s[4]=>z_linear[k,4], + # mdl.z_s[5]=>z_linear[k,5],mdl.z_s[6]=>z_linear[k,6], + # mdl.u_s[1]=>u_linear[k,1],mdl.u_s[2]=>u_linear[k,2], + # mdl.coeff_s[1]=>curvatureCoeff[1],mdl.coeff_s[2]=>curvatureCoeff[2], + # mdl.coeff_s[3]=>curvatureCoeff[3],mdl.coeff_s[4]=>curvatureCoeff[4], + # mdl.coeff_s[5]=>curvatureCoeff[5],mdl.coeff_s[6]=>curvatureCoeff[6], + # mdl.coeff_s[7]=>curvatureCoeff[7]) + # JuMP.fix(mdl.B[i,j,k],B[i,j,k]) + # end + # end + # end + + # for i=1:selectedStates.Nl*selectedStates.Np + # for j=1:n_state + # JuMP.fix(mdl.selStates[i,j],selStates[i,j]) + # end + # JuMP.fix(mdl.statesCost[i],statesCost[i]) + # end + # println(size(mdl.selStates)) + # println(size(selStates)) + setvalue(mdl.selStates,selStates) + setvalue(mdl.statesCost,statesCost) + + setvalue(mdl.df_his,mpcSol.df_his) + # println(getvalue(mdl.df_his)) + # for i=1:n_state + # JuMP.fix(mdl.zPrev[i],zPrev[1,i]) + # end + # JuMP.fix(mdl.uPrev[1],uPrev[1,1]) + # JuMP.fix(mdl.uPrev[2],uPrev[1,2]) + # println(size(mdl.uPrev)) + # println(size(uPrev)) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.zPrev,zPrev) + + setvalue(mdl.GP_e_vy,GP_e_vy) + setvalue(mdl.GP_e_psidot,GP_e_psidot) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + u_linear + sol_z = vcat(z_linear[1,:], getvalue(mdl.z_Ol) + z_linear[2:end,:]) + # println("Solved, status = $sol_status") + return sol_z,sol_u,sol_status +end + +function solveMpcProblem_convhull_kin(mdl::MpcModel_convhull_kin,mpcSol::MpcSol, + zCurr::Array{Float64,1}, + zPrev::Array{Float64,2},uPrev::Array{Float64,2},selectedStates::SelectedStates,track::Track, + GP_e_vy::Array{Float64,1},GP_e_psidot::Array{Float64,1}) + + selStates = selectedStates.selStates + statesCost = selectedStates.statesCost + + z_curvature=vcat(zCurr',zPrev[3:end,:]) + curvature=curvature_prediction(z_curvature,track) + + # Update current initial condition, curvature and System ID coefficients + setvalue(mdl.z0,zCurr) + setvalue(mdl.uPrev,uPrev) + setvalue(mdl.df_his,mpcSol.df_his) + setvalue(mdl.c,curvature) # Track curvature + setvalue(mdl.selStates,selStates) + setvalue(mdl.statesCost,statesCost) + setvalue(mdl.GP_e_vy,GP_e_vy) + setvalue(mdl.GP_e_psidot,GP_e_psidot) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + println("Solved, status = $sol_status") + println(getvalue(mdl.terminalCost)) + return sol_z,sol_u,sol_status +end \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/README.md b/workspace/src/barc/src/barc_lib/README.md new file mode 100644 index 00000000..34a413cd --- /dev/null +++ b/workspace/src/barc/src/barc_lib/README.md @@ -0,0 +1,5 @@ +# barc_lib + +This repo contains functions and types that are used by both the Barc simulation framework and the "real" ROS framework. +It contains basic BARC-specific types (related to the model and to MPC functions) and functions for their initialization. There are also functions to simulate the car's dynamics and to control the car along a racetrack (using Learning Model Predictive Control). +The library should be structured in folders according to its purposes. \ No newline at end of file diff --git a/workspace/src/barc/src/barc_lib/classes.jl b/workspace/src/barc/src/barc_lib/classes.jl new file mode 100644 index 00000000..895d67ff --- /dev/null +++ b/workspace/src/barc/src/barc_lib/classes.jl @@ -0,0 +1,200 @@ +# VARIOUS TYPES FOR CALCULATIONS + +type LapStatus + currentLap::Int64 # current lap number + currentIt::Int64 # current iteration in current lap + switchLap::Bool + nextLap::Bool + s_lapTrigger::Float64 +end + +# Structure of coeffConst: +# 1st dimension is the polynomial coefficient +# 2nd dimension specifies the state (1 = eY, 2 = ePsi, 3 = v or 1 = xDot, 2 = yDot, 3 = psiDot, 4 = ePsi, 5 = eY) +# 3th dimension specifies one of the two lap numbers between which are iterated + +type MpcCoeff # coefficients for trajectory approximation + c_Vx::Array{Float64,2} + c_Vy::Array{Float64,2} + c_Psi::Array{Float64,2} + MpcCoeff(c_Vx=zeros(10,6),c_Vy=zeros(10,4),c_Psi=zeros(10,3)) = new(c_Vx, c_Vy, c_Psi) + # c_Vx::Array{Float64,2} + # c_Vy::Array{Float64,1} + # c_Psi::Array{Float64,1} + # MpcCoeff(c_Vx=zeros(10,3),c_Vy=zeros(10),c_Psi=zeros(10)) = new(c_Vx, c_Vy, c_Psi) +end + +type OldTrajectory # information about previous trajectories + oldTraj::Array{Float64} # contains all states over all laps + count::Array{Int64} # contains the counter for each lap + OldTrajectory(oldTraj=Float64[],count=Int64[]) = new(oldTraj,count) +end + +type SolHistory + u::Array{Float64,4} + z::Array{Float64,4} + cost::Array{Float64,1} + function SolHistory(bufferSize::Int64,N::Int64,n_state::Int64,n_lap::Int64) + solHistory=new() + solHistory.u=zeros(bufferSize,n_lap,N,2) + solHistory.z=zeros(bufferSize,n_lap,N+1,n_state) + solHistory.cost=2*ones(n_lap) + return solHistory + end +end + +# To make the data structure to be compatible with my own simulation code, idx_start and idx_end in SafeSetData are acturally not used, since I am using other way to cantacate the laps. +type SafeSetData + oldSS::Array{Float64} # contains data from previous laps usefull to build the safe set + cost2target::Array{Float64} # cost to arrive at the target, i.e. how many iterations from the start to the end of the lap + oldCost::Array{Int64} # contains costs of laps + count::Array{Int64} # contains the counter for each lap + oldSS_xy::Array{Float64} + SafeSetData(oldSS=Float64[],cost2target=Float64[],oldCost=Int64[],count=Int64[],oldSS_xy=Float64[]) = + new(oldSS,cost2target,oldCost,count,oldSS_xy) +end + +type SelectedStates # Values needed for the convex hull formulation + selStates::Array{Float64} # selected states from previous laps ... + statesCost::Array{Float64} # ... and their related costs + Np::Int64 # number of states to select from each previous lap + Nl::Int64 # number of previous laps to include in the convex hull + feature_Np::Int64 # this is for feature points selcted from history + feature_Nl::Int64 # this is for feature points selected from history + SelectedStates(selStates=Float64[],statesCost=Float64[],Np=10,Nl=2,feature_Np=30,feature_Nl=5) = new(selStates,statesCost,Np,Nl,feature_Np,feature_Nl) +end + +type MpcParams # parameters for MPC solver + N::Int64 + nz::Int64 + Q::Array{Float64,1} + Q_term::Array{Float64,1} + R::Array{Float64,1} + vPathFollowing::Float64 + QderivZ::Array{Float64,1} + QderivU::Array{Float64,1} + Q_term_cost::Float64 + delay_df::Int64 + delay_a::Int64 + Q_lane::Float64 # weight on the soft constraint for the lane + Q_vel::Float64 # weight on the soft constraint for the maximum velocity + Q_slack::Array{Float64,1} # weight on the slack variables for the terminal constraint + Q_obs::Array{Float64} # weight used to esclude some of the old trajectories from the optimization problem + MpcParams(N=0,nz=0,Q=Float64[],Q_term=Float64[],R=Float64[],vPathFollowing=1.0,QderivZ=Float64[],QderivU=Float64[],Q_term_cost=1.0,delay_df=0,delay_a=0,Q_lane=1.0,Q_vel=1.0,Q_slack=Float64[],Q_obs=Float64[])= + new(N,nz,Q,Q_term,R,vPathFollowing,QderivZ,QderivU,Q_term_cost,delay_df,delay_a,Q_lane,Q_vel,Q_slack,Q_obs) +end + +type PosInfo # current position information + s::Float64 + s_target::Float64 + PosInfo(s=0,s_target=0) = new(s,s_target) +end + +type MpcSol # MPC solution output + a_x::Float64 + d_f::Float64 + solverStatus::Symbol + u::Array{Float64} + z::Array{Float64} + cost::Array{Float64} + eps_alpha::Array{Float64} + costSlack::Array{Float64} + df_his::Array{Float64,1} + a_his::Array{Float64,1} + MpcSol(a_x=0.0,d_f=0.0,solverStatus=Symbol(),u=Float64[],z=Float64[],cost=Float64[],eps_alpha=Float64[],costSlack=Float64[],df_his=Float64[],a_his=Float64[]) = new(a_x,d_f,solverStatus,u,z,cost,eps_alpha,costSlack,df_his,a_his) +end + +type ModelParams + l_A::Float64 + l_B::Float64 + m::Float64 + I_z::Float64 + dt::Float64 + u_lb::Array{Float64} + u_ub::Array{Float64} + z_lb::Array{Float64} + z_ub::Array{Float64} + c0::Array{Float64} + c_f::Float64 + ModelParams(l_A=0.25,l_B=0.25,m=1.98,I_z=0.24,dt=0.1,u_lb=Float64[],u_ub=Float64[],z_lb=Float64[],z_ub=Float64[],c0=Float64[],c_f=0.0) = new(l_A,l_B,m,I_z,dt,u_lb,u_ub,z_lb,z_ub,c0,c_f) +end + +# THIS IS THE CORRESPONDING TRACK DATA IN JULIA +# WE NEED TO DEFINE THEM BOTH IN PYTHON AND JULIA + +function add_curve(theta::Array{Float64,1},curvature::Array{Float64,1},num_point,angle::Float64,ds::Float64) + d_theta = 0 + curve = 2*sum(1:num_point/2)#+num_point/2 + for i=1:num_point + if i <= num_point/2 + d_theta = d_theta + angle / curve + elseif i == num_point/2+1 + d_theta = d_theta + else + d_theta = d_theta - angle / curve + end + append!(theta,[theta[end]+d_theta]) + curv_curr = d_theta/ds + append!(curvature,[curv_curr]) + end + return theta,curvature +end + +type Track + xy::Array{Float64,2} + idx # not dummy idx vector, which will be useful when finding the nearest point in function "find_idx()" + bound1xy::Array{Float64,2} # bound data are only for plotting visualization + bound2xy::Array{Float64,2} # bound data are only for plotting visualization + curvature::Array{Float64,1} + max_curvature::Float64 + theta::Array{Float64,1} + ds::Float64 # track discretization distance + n_node::Int64 # number of points in the track + w::Float64 # track width + s::Float64 # track total length + function Track(track_data::Array{Float64,2}) + # object Initialization + track=new() + + xy=[0.0 0.0] # 1.x 2.y + if get_param("feature_flag") + theta=[pi/4] + else + theta=[0.0] + end + + curvature=[0.0] + ds=0.01 # length of each segment + width=0.8 + # bound1xy=[0.0 width/2] + # bound2xy=[0.0 -width/2] + bound1xy = xy + width/2*[cos(theta[1]+pi/2) sin(theta[1]+pi/2)] + bound2xy = xy - width/2*[cos(theta[1]+pi/2) sin(theta[1]+pi/2)] + + # create the track segment angle + for i=1:size(track_data,1) + (theta,curvature)=add_curve(theta,curvature,track_data[i,1],track_data[i,2],ds) + end + max_curvature=maximum(abs(curvature)) + # create the track segment by segment using the angle just calculated + for i=2:size(theta,1) # we will have one more point at the end of the array, which is the same as the original starting point + xy_next=[xy[end,1]+ds*cos(theta[i]) xy[end,2]+ds*sin(theta[i])] + bound1xy_next = xy_next + width/2*[cos(theta[i]+pi/2) sin(theta[i]+pi/2)] + bound2xy_next = xy_next - width/2*[cos(theta[i]+pi/2) sin(theta[i]+pi/2)] + xy=vcat(xy,xy_next) + bound1xy=vcat(bound1xy,bound1xy_next) + bound2xy=vcat(bound2xy,bound2xy_next) + end + + # object construction + track.xy=xy; track.bound1xy=bound1xy; track.bound2xy=bound2xy; + track.curvature=curvature; track.max_curvature=max_curvature; + track.theta=theta; track.ds=ds; track.w=width + track.n_node=size(xy,1); track.idx=1:size(xy,1) + track.s=(track.n_node-1)*track.ds # Important: exclude the last point from the track length! + return track + end + # The last point of this function is actually a dummy point +end + + diff --git a/workspace/src/barc/src/barc_lib/simModel.jl b/workspace/src/barc/src/barc_lib/simModel.jl new file mode 100644 index 00000000..359c1adf --- /dev/null +++ b/workspace/src/barc/src/barc_lib/simModel.jl @@ -0,0 +1,148 @@ +# z[1] = xDot +# z[2] = yDot +# z[3] = psiDot +# z[4] = ePsi +# z[5] = eY +# z[6] = s + +function simKinModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_a = modelParams.l_A + L_b = modelParams.l_B + c = ([z[1]^8 z[1]^7 z[1]^6 z[1]^5 z[1]^4 z[1]^3 z[1]^2 z[1] 1]*coeff)[1] # Polynomial + + bta = atan(L_a/(L_a+L_b)*tan(u[2]+abs(u[2])*u[2])) + dsdt = z[4]*cos(z[3]+bta)/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt*dsdt # s + zNext[2] = z[2] + dt*z[4] * sin(z[3] + bta) # eY + zNext[3] = z[3] + dt*(z[4]/L_a*sin(bta)-dsdt*c) # ePsi + zNext[4] = z[4] + dt*(u[1] - modelParams.c_f*z[4]) # v + + return zNext +end + +function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + # This function uses smaller steps to achieve higher fidelity than we would achieve using longer timesteps + z_final = copy(z) + u[1] = min(u[1],3) + u[1] = max(u[1],-3) + u[2] = min(u[2],pi/6) + u[2] = max(u[2],-pi/6) + dtn = dt/100 + for i=1:100 + z_final = simDynModel(z_final,u,dtn,coeff,modelParams) + end + return z_final +end + +function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + c0 = modelParams.c0 + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f + + a_F = 0 + a_R = 0 + if abs(z[1]) >= 0.1 + # a_F = atan((z[2] + L_f*z[3])/abs(z[1])) - z[8] + a_F = atan((z[2] + L_f*z[3])/abs(z[1])) - u[2] + a_R = atan((z[2] - L_r*z[3])/abs(z[1])) + end + if max(abs(a_F),abs(a_R))>30/180*pi + warn("Large tire angles: a_F = $a_F, a_R = $a_R, xDot = $(z[1]), d_F = $(z[8])") + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + c = ([z[1]^8 z[2]^7 z[3]^6 z[4]^5 z[5]^4 z[6]^3 z[7]^2 z[8] 1]*coeff)[1] # Polynomial for curvature + + dsdt = (z[1]*cos(z[4]) - z[2]*sin(z[4]))/(1-z[5]*c) + + zNext = copy(z) + # zNext[1] = z[1] + dt * (z[7] + z[2]*z[3] - c_f*z[1]) # xDot + zNext[1] = z[1] + dt * (u[1] + z[2]*z[3] - c_f*z[1]) # xDot + # zNext[2] = z[2] + dt * (2/m*(FyF*cos(z[8]) + FyR) - z[3]*z[1]) # yDot + zNext[2] = z[2] + dt * (2/m*(FyF*cos(u[2]) + FyR) - z[3]*z[1]) # yDot + zNext[3] = z[3] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[4] = z[4] + dt * (z[3]-dsdt*c) # ePsi + zNext[5] = z[5] + dt * (z[1]*sin(z[4]) + z[2]*cos(z[4])) # eY + zNext[6] = z[6] + dt * dsdt # s + zNext[7] = z[7] + dt * (u[1] - z[7]) * 100 # a + zNext[8] = z[8] + dt * (u[2] - z[8]) * 10 # d_f + + return zNext +end + +function pacejka(a) + # B = 1.0 # This value determines the steepness of the curve + # C = 1.25 + B = 6.0 + C = 1.6 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function simDynModel_exact_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + dtn = dt/10 + t = 0:dtn:dt + z_final = copy(z) + ang = zeros(2) + for i=1:length(t)-1 + z_final, ang = simDynModel_xy(z_final,u,dtn,modelParams) + end + return z_final, ang +end + +function simDynModel_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f + + a_F = 0.0 + a_R = 0.0 + if abs(z[3]) > 0.2 + # a_F = atan((z[4] + L_f*z[6])/abs(z[3])) - z[8] + a_F = atan((z[4] + L_f*z[6])/abs(z[3])) - u[2] + a_R = atan((z[4] - L_r*z[6])/abs(z[3])) + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + # println("FyF",FyF,"FyR",FyR) + + if abs(a_F) > 30/180*pi || abs(a_R) > 30/180*pi + warn("Large slip angles in simulation: a_F = $a_F, a_R = $a_R") + end + + zNext = copy(z) + # compute next state + zNext[1] = z[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) # x + zNext[2] = z[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) # y + # zNext[3] = zNext[3] + dt * (z[7] + z[4]*z[6] - c_f*z[3]) # v_x + zNext[3] = z[3] + dt * (u[1] + z[4]*z[6] - c_f*z[3]) # v_x + # zNext[4] = zNext[4] + dt * (1/m*(FyF*cos(z[8]) + FyR) - z[6]*z[3]) # v_y + zNext[4] = z[4] + dt * (1/m*(FyF*cos(u[2]) + FyR) - z[6]*z[3]) # v_y + zNext[5] = z[5] + dt * (z[6]) # psi + zNext[6] = z[6] + dt * (1/I_z*(L_f*FyF*cos(u[2]) - L_r*FyR)) # psiDot + zNext[7] = z[7] + dt * (u[1]-z[7])*100 # a + zNext[8] = z[8] + dt * (u[2]-z[8])*100 # d_f + + zNext[3] = max(0,zNext[3]) # limit speed to positive values (BARC specific) + return zNext, [a_F a_R] +end diff --git a/workspace/src/barc/src/barc_lib/simModel_lukas.jl b/workspace/src/barc/src/barc_lib/simModel_lukas.jl new file mode 100755 index 00000000..f12066b7 --- /dev/null +++ b/workspace/src/barc/src/barc_lib/simModel_lukas.jl @@ -0,0 +1,138 @@ +# z[1] = xDot +# z[2] = yDot +# z[3] = psiDot +# z[4] = ePsi +# z[5] = eY +# z[6] = s + +function simKinModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_a = modelParams.l_A + L_b = modelParams.l_B + c = ([z[1]^8 z[1]^7 z[1]^6 z[1]^5 z[1]^4 z[1]^3 z[1]^2 z[1] 1]*coeff)[1] # Polynomial + + bta = atan(L_a/(L_a+L_b)*tan(u[2]+abs(u[2])*u[2])) + dsdt = z[4]*cos(z[3]+bta)/(1-z[2]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt*dsdt # s + zNext[2] = z[2] + dt*z[4] * sin(z[3] + bta) # eY + zNext[3] = z[3] + dt*(z[4]/L_a*sin(bta)-dsdt*c) # ePsi + zNext[4] = z[4] + dt*(u[1] - modelParams.c_f*z[4]) # v + + return zNext +end + +function simDynModel_exact(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + # This function uses smaller steps to achieve higher fidelity than we would achieve using longer timesteps + z_final = copy(z) + u[1] = min(u[1],3) + u[1] = max(u[1],-3) + u[2] = min(u[2],pi/6) + u[2] = max(u[2],-pi/6) + dtn = dt/100 + for i=1:100 + z_final = simDynModel(z_final,u,dtn,coeff,modelParams) + end + return z_final +end + +function simDynModel(z::Array{Float64},u::Array{Float64},dt::Float64,coeff::Array{Float64},modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + c0 = modelParams.c0 + m = modelParams.m + I_z = modelParams.I_z + c_f = modelParams.c_f + + a_F = 0 + a_R = 0 + if abs(z[1]) >= 0.1 + a_F = atan((z[2] + L_f*z[3])/abs(z[1])) - z[8] + a_R = atan((z[2] - L_r*z[3])/abs(z[1])) + end + if max(abs(a_F),abs(a_R))>30/180*pi + # warn("Large tire angles: a_F = $a_F, a_R = $a_R, xDot = $(z[1]), d_F = $(z[8])") + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + c = ([z[1]^8 z[2]^7 z[3]^6 z[4]^5 z[5]^4 z[6]^3 z[7]^2 z[8] 1]*coeff)[1] # Polynomial for curvature + + dsdt = (z[1]*cos(z[4]) - z[2]*sin(z[4]))/(1-z[5]*c) + + zNext = copy(z) + zNext[1] = z[1] + dt * (z[7] + z[2]*z[3] - c_f*z[1]) # xDot + zNext[2] = z[2] + dt * (2/m*(FyF*cos(z[8]) + FyR) - z[3]*z[1]) # yDot + zNext[3] = z[3] + dt * (2/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[4] = z[4] + dt * (z[3]-dsdt*c) # ePsi + zNext[5] = z[5] + dt * (z[1]*sin(z[4]) + z[2]*cos(z[4])) # eY + zNext[6] = z[6] + dt * dsdt # s + zNext[7] = z[7] + dt * (u[1] - z[7]) * 100 # a + zNext[8] = z[8] + dt * (u[2] - z[8]) * 10 # d_f + + return zNext +end + +function pacejka(a) + B = 1.0 # This value determines the steepness of the curve + C = 1.25 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function simDynModel_exact_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + dtn = dt/10 + t = 0:dtn:dt + z_final = copy(z) + ang = zeros(2) + for i=1:length(t)-1 + z_final, ang = simDynModel_xy(z_final,u,dtn,modelParams) + end + return z_final, ang +end + +function simDynModel_xy(z::Array{Float64},u::Array{Float64},dt::Float64,modelParams::ModelParams) + + zNext::Array{Float64} + L_f = modelParams.l_A + L_r = modelParams.l_B + m = modelParams.m + I_z = modelParams.I_z + + a_F = 0.0 + a_R = 0.0 + if abs(z[3]) > 0.2 + a_F = atan((z[4] + L_f*z[6])/abs(z[3])) - z[8] + a_R = atan((z[4] - L_r*z[6])/abs(z[3])) + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + if abs(a_F) > 30/180*pi || abs(a_R) > 30/180*pi + # warn("Large slip angles in simulation: a_F = $a_F, a_R = $a_R") + end + + zNext = copy(z) + # compute next state + zNext[1] = zNext[1] + dt * (cos(z[5])*z[3] - sin(z[5])*z[4]) # x + zNext[2] = zNext[2] + dt * (sin(z[5])*z[3] + cos(z[5])*z[4]) # y + zNext[3] = zNext[3] + dt * (z[7] + z[4]*z[6] - 0.05*z[3]) # v_x + zNext[4] = zNext[4] + dt * (1/m*(FyF*cos(z[8]) + FyR) - z[6]*z[3]) # v_y + zNext[5] = zNext[5] + dt * (z[6]) # psi + zNext[6] = zNext[6] + dt * (1/I_z*(L_f*FyF - L_r*FyR)) # psiDot + zNext[7] = zNext[7] + dt * (u[1]-z[7])*100 # a + zNext[8] = zNext[8] + dt * (u[2]-z[8])*100 # d_f + + zNext[3] = max(0,zNext[3]) # limit speed to positive values (BARC specific) + return zNext, [a_F a_R] +end diff --git a/workspace/src/barc/src/barc_simulator_dyn.jl b/workspace/src/barc/src/barc_simulator_dyn.jl new file mode 100755 index 00000000..fb03561c --- /dev/null +++ b/workspace/src/barc/src/barc_simulator_dyn.jl @@ -0,0 +1,311 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, Vel_est, pos_info +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_imu_fusion +@rosimport std_msgs.msg: Header +rostypegen() +using barc.msg +using geometry_msgs.msg +using sensor_msgs.msg +using std_msgs.msg +using marvelmind_nav.msg +using JLD + +include("barc_lib/classes.jl") +include("barc_lib/simModel.jl") + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data + t_msg::Array{Float64} + z::Array{T} # measurement values +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +function ECU_callback(msg::ECU,u_current::Array{Float64},cmd_log::Measurements) + u_current[:] = convert(Array{Float64,1},[msg.motor, msg.servo]) + cmd_log.t_msg[cmd_log.i] = to_sec(get_rostime()) + cmd_log.t[cmd_log.i] = to_sec(get_rostime()) + cmd_log.z[cmd_log.i,:] = u_current + cmd_log.i += 1 +end + +function main() + u_current = zeros(Float64,2) # msg ECU is Float32 ! + + buffersize = 60000 + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_log = Measurements{Float64}(1,ones(buffersize)*Inf,ones(buffersize)*Inf,zeros(buffersize,2)) + z_real = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,8)) + slip_a = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + + # initiate node, set up publisher / subscriber topics + init_node("barc_sim") + pub_gps = Publisher("hedge_imu_fusion", hedge_imu_fusion, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_imu_fusion} + pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} + pub_vel = Publisher("vel_est", Vel_est, queue_size=1)::RobotOS.Publisher{barc.msg.Vel_est} + real_val = Publisher("real_val", pos_info, queue_size=1)::RobotOS.Publisher{barc.msg.pos_info} + + s1 = Subscriber("ecu", ECU, ECU_callback, (u_current,cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + + z_current = zeros(60000,8) + if get_param("feature_flag") + z_current[1,:] = [0.02 0.0 0.0 0.0 pi/4 0.0 0.0 0.0] + else + z_current[1,:] = [0.02 0.0 0.0 0.0 0.0 0.0 0.0 0.0] + end + + slip_ang = zeros(60000,2) + + dt = 0.01 + loop_rate = Rate(1/dt) + + i = 2 + + dist_traveled = randn(3) # encoder positions of three wheels + last_updated = 0.0 + + r_tire = 0.036 # radius from tire center to perimeter along magnets [m] + + imu_drift = 0.0 # simulates yaw-sensor drift over time (slow sine) + + modelParams = ModelParams() + run_id = get_param("run_id") + # modelParams.l_A = copy(get_param("L_a")) # always throws segmentation faults *after* execution!!! ?? + # modelParams.l_B = copy(get_param("L_a")) + # modelParams.m = copy(get_param("m")) + # modelParams.I_z = copy(get_param("I_z")) + + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.m = 1.98 + modelParams.I_z = 0.03#0.24 # using homogenous distributed mass over a cuboid + + println("Publishing sensor information. Simulator running.") + imu_data = Imu() + vel_est = Vel_est() + t0 = to_sec(get_rostime()) + gps_data = hedge_imu_fusion() + real_data = pos_info() + + + z_real.t_msg[1] = t0 + slip_a.t_msg[1] = t0 + z_real.t[1] = t0 + slip_a.t[1] = t0 + t = 0.0 + + sim_gps_interrupt = 0 # counter if gps interruption is simulated + vel_dist_update = 2*pi*0.036/2 # distance to travel until velocity is updated (half wheel rotation) + d_f_his = zeros(20) # The hardware has around 0.2 s delay and this node is 100Hz, which is about an array with length 20. + + gps_header = Header() + while ! is_shutdown() + t_ros = get_rostime() + t = to_sec(t_ros) + # # print(t) + if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 + u_current[2] = cmd_log.z[t.>=cmd_log.t+0.2,2][end] # artificial steering input delay + end + # update current state with a new row vector + + # z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) + + # # THIS PART CREATED A 0.2S DELAY IN THE SIMULATOR + # u_temp = d_f_his[1] + # d_f_his[1:end-1] = d_f_his[2:end] + # d_f_his[end] = u_current[2] + # u_current[2] = u_temp + u_current[1] *= 1 + # println(u_current) + + # println(d_f_his) + # println("input from simulator node",round(u_current,2)) + z_current[i,:],slip_ang[i,:] = simDynModel_xy(z_current[i-1,:], u_current', dt, modelParams) + + + z_real.t_msg[i] = t + z_real.t[i] = t + slip_a.t_msg[i] = t + slip_a.t[i] = t + + # IMU measurements + if i%1 == 0 # 100 Hz + imu_drift = 0#1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) + rand_yaw = 0.05*randn() + if rand_yaw > 0.1 + rand_yaw = 0.1 + elseif rand_yaw<-0.1 + rand_yaw=-0.1 + end + + yaw = z_current[i,5] + imu_drift # + rand_yaw#+ 0.002*randn() + + rand_psiDot = 0.05*randn() + if rand_psiDot > 0.1 + rand_psiDot = 0.1 + elseif rand_psiDot<-0.1 + rand_psiDot=-0.1 + end + psiDot = z_current[i,6] #+rand_psiDot#+ 0.001*randn() + imu_meas.t_msg[imu_meas.i] = t + imu_meas.t[imu_meas.i] = t + imu_meas.z[imu_meas.i,:] = [yaw psiDot] + imu_meas.i += 1 + imu_data.orientation = geometry_msgs.msg.Quaternion(0, 0, sin(yaw/2), cos(yaw/2)) + imu_data.angular_velocity = Vector3(0,0,psiDot) + imu_data.header.stamp = t_ros + + rand_accX = 1.0*randn() + if rand_accX > 1.5 + rand_accX = 1.5 + elseif rand_accX<-1.5 + rand_accX=-1.5 + end + + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i-1,6]*z_current[i-1,4] +rand_accX#+ randn()*0.3*1.0 + + rand_accY = 1.0*randn() + if rand_accY > 1.5 + rand_accY = 1.5 + elseif rand_accY<-1.5 + rand_accY=-1.5 + end + + # imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] +rand_accY#+ randn()*0.3*1.0 + imu_data.linear_acceleration.y = (z_current[i,4]-z_current[i-1,4])/dt + z_current[i-1,6]*z_current[i-1,3] +rand_accY#+ randn()*0.3*1.0 + publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" + # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) + # println("ay from simulator:",imu_data.linear_acceleration.y) + real_data.a_y = imu_data.linear_acceleration.y + end + + # real values + if i%6 == 0 + # ADDITIONAL NOISE ADDING + n=randn(3) + n_thre = [0.1,0.1,0.5]*0.01 + # IMPORTANT!!!: THE DATA NEEDS TO BE IN THE SAME ORDER AS Z_EST + # n_thre = 0.01*[0.02,0.005,0.05,0.005,0.005,0.001] + # s # 2.5% ey: 0.4 is set as the track width # epsi: 1.8 degree # vx # vy: on the track, max vy can be 0.1 # psidot: on the track, max psidot can be 1 + n.*=n_thre + n=min(n, n_thre) + n=max(n,-n_thre) + real_data.psiDot = z_current[i,6] + n[3] + real_data.psi = z_current[i,5] + real_data.v_x = z_current[i,3] + n[1] + # println("v from simulator",z_current[i,3]) + real_data.v_y = z_current[i,4] + n[2] + real_data.x = z_current[i,1] + real_data.y = z_current[i,2] + publish(real_val,real_data) + + end + + # Velocity measurements + dist_traveled += norm(diff(z_current[i-1:i,1:2])) + if i%5 == 0 # 20 Hz + if sum(dist_traveled .>= vel_dist_update)>=1 && z_current[i,3] > 0.1 # only update if at least one of the magnets has passed the sensor + dist_traveled[dist_traveled.>=vel_dist_update] = 0 + vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) + vel_est.vel_fl = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_fr = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_bl = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_br = convert(Float32,norm(z_current[i,3:4])) + end + vel_est.header.stamp = t_ros + publish(pub_vel, vel_est) + end + + # GPS measurements + if i%1 == 0 # 100 Hz + + rand_x = 0.02*randn() + if rand_x > 0.04 + rand_x = 0.04 + elseif rand_x<-0.04 + rand_x=-0.04 + end + + x = round(z_current[i,1] + rand_x,2)#0.002*randn(),2) # Indoor gps measures, rounded on cm + + rand_y = 0.02*randn() + if rand_y > 0.04 + rand_y = 0.04 + elseif rand_y<-0.04 + rand_y=-0.04 + end + + y = round(z_current[i,2] + rand_y,2)#0.002*randn(),2) + + if randn()>10 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) + x += 1#randn() # add random value to x and y + y -= 1#randn() + #sim_gps_interrupt = 6 # also do a little interruption + elseif randn()>10 && sim_gps_interrupt < 0 + sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) + end + if sim_gps_interrupt < 0 + gps_meas.t_msg[gps_meas.i] = t + gps_meas.t[gps_meas.i] = t + gps_meas.z[gps_meas.i,:] = [x y] + gps_meas.i += 1 + # gps_data.header.stamp = get_rostime() + gps_data.x_m = x + gps_data.y_m = y + # println("gps from simulator",x,y) + publish(pub_gps, gps_data) + end + sim_gps_interrupt -= 1 + end + # println("True state from LMPC nodes",[real_data.v_x,real_data.v_y,real_data.psiDot]) + + i += 1 + rossleep(loop_rate) + end + + # Clean up buffers + clean_up(gps_meas) + clean_up(imu_meas) + clean_up(cmd_log) + z_real.z[1:i-1,:] = z_current[1:i-1,:] + slip_a.z[1:i-1,:] = slip_ang[1:i-1,:] + z_real.i = i + slip_a.i = i + clean_up(z_real) + clean_up(slip_a) + + # Save simulation data to file + log_path = "$(homedir())/simulations/output-SIM-$(run_id[1:4]).jld" + # save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) + println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") + +end + +if ! isinteractive() + main() +end diff --git a/workspace/src/barc/src/barc_simulator_dyn.py b/workspace/src/barc/src/barc_simulator_dyn.py new file mode 100755 index 00000000..6371eef2 --- /dev/null +++ b/workspace/src/barc/src/barc_simulator_dyn.py @@ -0,0 +1,273 @@ +#!/usr/bin/env python +""" + File name: barc_simulator_dyn.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +#---------------------------------------------------------------------------- + +import rospy +import geometry_msgs.msg +from barc.msg import ECU, pos_info, Vel_est +from geometry_msgs.msg import Vector3 +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion +from numpy import tan, arctan, cos, sin, pi +from numpy.random import randn +from tf import transformations + +def main(): + rospy.init_node("simulator") + sim = Simulator() + imu = ImuClass() + gps = GpsClass() + enc = EncClass() + ecu = EcuClass() + + counter = 0 + a_his = [0.0]*int(rospy.get_param("simulator/delay_a")/rospy.get_param("simulator/dt")) + df_his = [0.0]*int(rospy.get_param("simulator/delay_df")/rospy.get_param("simulator/dt")) + while not rospy.is_shutdown(): + # Simulator delay + a_his.append(ecu.u[0]) + df_his.append(ecu.u[1]) + u = [a_his.pop(0), df_his.pop(0)] + + sim.f(u) + + imu.update(sim) + gps.update(sim) + enc.update(sim) + + sim.saveHistory() + + gps.gps_pub() + imu.imu_pub() + + counter += 1 + if counter == 5: + enc.enc_pub() + counter = 0 + sim.rate.sleep() + +class Simulator(object): + """ Object collecting GPS measurement data + Attributes: + Model params: + 1.L_f 2.L_r 3.m(car mass) 3.I_z(car inertial) 4.c_f(equivalent drag coefficient) + States: + 1.x 2.y 3.vx 4.vy 5.ax 6.ay 7.psiDot + States history: + 1.x_his 2.y_his 3.vx_his 4.vy_his 5.ax_his 6.ay_his 7.psiDot_his + Simulator sampling time: + 1. dt + Time stamp: + 1. time_his + Methods: + f(u): + System model used to update the states + pacejka(ang): + Pacejka lateral tire modeling + """ + def __init__(self): + + self.L_f = rospy.get_param("L_a") + self.L_r = rospy.get_param("L_b") + self.m = rospy.get_param("m") + self.I_z = rospy.get_param("I_z") + self.c_f = rospy.get_param("simulator/c_f") + + self.B = rospy.get_param("simulator/B") + self.C = rospy.get_param("simulator/C") + self.mu= rospy.get_param("simulator/mu") + self.g = rospy.get_param("simulator/g") + + self.x = 0.02 + self.y = 0.0 + self.vx = 0.0 + self.vy = 0.0 + self.ax = 0.0 + self.ay = 0.0 + + if rospy.get_param("feature_flag"): + self.yaw = pi/4 + else: + self.yaw = 0.0 + + self.psiDot = 0.0 + + self.x_his = [] + self.y_his = [] + self.vx_his = [] + self.vy_his = [] + self.ax_his = [] + self.ay_his = [] + self.psiDot_his = [] + + self.dt = rospy.get_param("simulator/dt") + self.rate = rospy.Rate(1.0/self.dt) + self.time_his = [] + + def f(self,u): + a_F = 0.0 + a_R = 0.0 + + if abs(self.vx) > 0.2: + a_F = arctan((self.vy + self.L_f*self.psiDot)/abs(self.vx)) - u[1] + a_R = arctan((self.vy - self.L_r*self.psiDot)/abs(self.vx)) + + FyF = -self.pacejka(a_F) + FyR = -self.pacejka(a_R) + + if abs(a_F) > 30.0/180.0*pi or abs(a_R) > 30.0/180.0*pi: + print "WARNING: Large slip angles in simulation" + + x = self.x + y = self.y + ax = self.ax + ay = self.ay + vx = self.vx + vy = self.vy + yaw = self.yaw + psiDot = self.psiDot + + self.x += self.dt*(cos(yaw)*vx - sin(yaw)*vy) + self.y += self.dt*(sin(yaw)*vx + cos(yaw)*vy) + self.vx += self.dt*(ax + psiDot*vy) + self.vy += self.dt*(ay - psiDot*vx) + self.ax = u[0] - self.c_f*vx - FyF*sin(u[1]) + self.ay = 1.0/self.m*(FyF*cos(u[1])+FyR) + self.yaw += self.dt*(psiDot) + self.psiDot += self.dt*(1.0/self.I_z*(self.L_f*FyF*cos(u[1]) - self.L_r*FyR)) + + self.vx = abs(self.vx) + + def pacejka(self,ang): + D = self.mu*self.m*self.g/2 + C_alpha_f = D*sin(self.C*arctan(self.B*ang)) + return C_alpha_f + + def saveHistory(self): + self.x_his.append(self.x) + self.y_his.append(self.y) + self.vx_his.append(self.vx) + self.vy_his.append(self.vy) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.psiDot_his.append(self.psiDot) + self.time_his.append(rospy.get_rostime().to_sec()) + +class ImuClass(object): + def __init__(self): + self.pub = rospy.Publisher("imu/data", Imu, queue_size=1) + self.ax = 0.0 + self.ay = 0.0 + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax_std = rospy.get_param("simulator/ax_std") + self.ay_std = rospy.get_param("simulator/ay_std") + self.psiDot_std = rospy.get_param("simulator/psiDot_std") + self.n_bound = rospy.get_param("simulator/n_bound") + + self.msg = Imu() + + def update(self,sim): + n = self.ax_std*randn() + n = min(n, self.ax_std*self.n_bound) + n = max(n,-self.ax_std*self.n_bound) + self.ax = sim.ax + n + + n = self.ay_std*randn() + n = min(n, self.ay_std*self.n_bound) + n = max(n,-self.ay_std*self.n_bound) + self.ay = sim.ay + n + + n = self.psiDot_std*randn() + n = min(n, self.psiDot_std*self.n_bound) + n = max(n,-self.psiDot_std*self.n_bound) + self.psiDot = sim.psiDot + n + + self.yaw = sim.yaw + + def imu_pub(self): + self.msg.linear_acceleration.x = self.ax + self.msg.linear_acceleration.y = self.ay + self.msg.angular_velocity = Vector3(0,0,self.psiDot) + self.orientation = geometry_msgs.msg.Quaternion(0, 0, sin(self.yaw/2), cos(self.yaw/2)) + self.pub.publish(self.msg) + +class GpsClass(object): + def __init__(self): + self.pub = rospy.Publisher("hedge_imu_fusion", hedge_imu_fusion, queue_size=1) + self.x = 0.0 + self.y = 0.0 + self.x_std = rospy.get_param("simulator/x_std") + self.y_std = rospy.get_param("simulator/y_std") + self.n_bound = rospy.get_param("simulator/n_bound") + + self.msg = hedge_imu_fusion() + + def update(self,sim): + n = self.x_std*randn() + n = min(n, self.x_std*self.n_bound) + n = max(n,-self.x_std*self.n_bound) + self.x = sim.x + n + + n = self.y_std*randn() + n = min(n, self.y_std*self.n_bound) + n = max(n,-self.y_std*self.n_bound) + self.y = sim.y + n + + def gps_pub(self): + self.msg.x_m = self.x + self.msg.y_m = self.y + self.pub.publish(self.msg) + +class EncClass(object): + def __init__(self): + self.pub = rospy.Publisher("vel_est", Vel_est, queue_size=1) + self.v = 0.0 + self.v_std = rospy.get_param("simulator/v_std") + self.n_bound = rospy.get_param("simulator/n_bound") + + self.msg = Vel_est() + + def update(self,sim): + n = self.v_std*randn() + n = min(n, self.v_std*self.n_bound) + n = max(n,-self.v_std*self.n_bound) + self.v = (sim.vx**2+sim.vy**2)**0.5 + n + + def enc_pub(self): + self.msg.vel_fl = self.v + self.msg.vel_fr = self.v + self.msg.vel_bl = self.v + self.msg.vel_br = self.v + self.msg.vel_est = self.v + self.pub.publish(self.msg) + +class EcuClass(object): + def __init__(self): + self.sub = rospy.Subscriber("ecu", ECU, self.ecu_callback, queue_size=1) + self.u = [0.0, 0.0] + + def ecu_callback(self,data): + self.u = [data.motor, data.servo] + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/barc_simulator_dyn_lukas.jl b/workspace/src/barc/src/barc_simulator_dyn_lukas.jl new file mode 100755 index 00000000..75e486e6 --- /dev/null +++ b/workspace/src/barc/src/barc_simulator_dyn_lukas.jl @@ -0,0 +1,312 @@ +#!/usr/bin/env julia + +#= + Licensing Information: You are free to use or extend these projects for + education or reserach purposes provided that (1) you retain this notice + and (2) you provide clear attribution to UC Berkeley, including a link + to http://barc-project.com + + Attibution Information: The barc project ROS code-base was developed + at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales + (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed + by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was + based on an open source project by Bruce Wootton +=# + +using RobotOS +@rosimport barc.msg: ECU, Vel_est, pos_info +@rosimport geometry_msgs.msg: Vector3 +@rosimport sensor_msgs.msg: Imu +@rosimport marvelmind_nav.msg: hedge_imu_fusion +@rosimport std_msgs.msg: Header +rostypegen() +using barc.msg +using geometry_msgs.msg +using sensor_msgs.msg +using std_msgs.msg +using marvelmind_nav.msg +using JLD + +include("barc_lib/classes.jl") +include("barc_lib/simModel_lukas.jl") + +# This type contains measurement data (time, values and a counter) +type Measurements{T} + i::Int64 # measurement counter + t::Array{Float64} # time data + t_msg::Array{Float64} + z::Array{T} # measurement values +end + +# This function cleans the zeros from the type above once the simulation is finished +function clean_up(m::Measurements) + m.t = m.t[1:m.i-1] + m.t_msg = m.t_msg[1:m.i-1] + m.z = m.z[1:m.i-1,:] +end + +function ECU_callback(msg::ECU,u_current::Array{Float64},cmd_log::Measurements) + u_current[:] = convert(Array{Float64,1},[msg.motor, msg.servo]) + cmd_log.t_msg[cmd_log.i] = to_sec(get_rostime()) + cmd_log.t[cmd_log.i] = to_sec(get_rostime()) + cmd_log.z[cmd_log.i,:] = u_current + cmd_log.i += 1 +end + +function main() + u_current = zeros(Float64,2) # msg ECU is Float32 ! + + buffersize = 60000 + gps_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + imu_meas = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + cmd_log = Measurements{Float64}(1,ones(buffersize)*Inf,ones(buffersize)*Inf,zeros(buffersize,2)) + z_real = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,8)) + slip_a = Measurements{Float64}(1,zeros(buffersize),zeros(buffersize),zeros(buffersize,2)) + + # initiate node, set up publisher / subscriber topics + init_node("barc_sim") + pub_gps = Publisher("hedge_imu_fusion", hedge_imu_fusion, queue_size=1)::RobotOS.Publisher{marvelmind_nav.msg.hedge_imu_fusion} + pub_imu = Publisher("imu/data", Imu, queue_size=1)::RobotOS.Publisher{sensor_msgs.msg.Imu} + pub_vel = Publisher("vel_est", Vel_est, queue_size=1)::RobotOS.Publisher{barc.msg.Vel_est} + real_val = Publisher("real_val", pos_info, queue_size=1)::RobotOS.Publisher{barc.msg.pos_info} + + s1 = Subscriber("ecu", ECU, ECU_callback, (u_current,cmd_log,), queue_size=1)::RobotOS.Subscriber{barc.msg.ECU} + + z_current = zeros(60000,8) + z_current[1,:] = [0.02 0.0 0.0 0.0 0.0 0.0 0.0 0.0] + # z_current[1,:] = [0.1 0.0 0.0 0.0 pi/6 0.0 0.0 0.0] + slip_ang = zeros(60000,2) + + dt = 0.01 + loop_rate = Rate(1/dt) + + i = 2 + + dist_traveled = randn(3) # encoder positions of three wheels + last_updated = 0.0 + + r_tire = 0.036 # radius from tire center to perimeter along magnets [m] + + imu_drift = 0.0 # simulates yaw-sensor drift over time (slow sine) + + modelParams = ModelParams() + run_id = get_param("run_id") + # modelParams.l_A = copy(get_param("L_a")) # always throws segmentation faults *after* execution!!! ?? + # modelParams.l_B = copy(get_param("L_a")) + # modelParams.m = copy(get_param("m")) + # modelParams.I_z = copy(get_param("I_z")) + + node_name = get_name() + if node_name[end] == "2" + modelParams.m = 1.75 + else + modelParams.m = 1.98 + end + modelParams.l_A = 0.125 + modelParams.l_B = 0.125 + modelParams.I_z = 0.03#0.24 # using homogenous distributed mass over a cuboid + + println("Publishing sensor information. Simulator running.") + imu_data = Imu() + vel_est = Vel_est() + t0 = to_sec(get_rostime()) + gps_data = hedge_imu_fusion() + real_data = pos_info() + + + z_real.t_msg[1] = t0 + slip_a.t_msg[1] = t0 + z_real.t[1] = t0 + slip_a.t[1] = t0 + t = 0.0 + + sim_gps_interrupt = 0 # counter if gps interruption is simulated + vel_dist_update = 2*pi*0.036/2 # distance to travel until velocity is updated (half wheel rotation) + d_f_his = zeros(20) # The hardware has around 0.2 s delay and this node is 100Hz, which is about an array with length 20. + + gps_header = Header() + while ! is_shutdown() + t_ros = get_rostime() + t = to_sec(t_ros) + # # print(t) + #if sizeof(cmd_log.z[t.>cmd_log.t+0.2,2]) >= 1 + # u_current[2] = cmd_log.z[t.>=cmd_log.t+0.2,2][end] # artificial steering input delay + #end + # update current state with a new row vector + + # z_current[i,:],slip_ang[i,:] = simDynModel_exact_xy(z_current[i-1,:], u_current', dt, modelParams) + + # # THIS PART CREATED A 0.2S DELAY IN THE SIMULATOR + # u_temp = d_f_his[1] + # d_f_his[1:end-1] = d_f_his[2:end] + # d_f_his[end] = u_current[2] + # u_current[2] = u_temp + u_current[1] *= 1 + # println(u_current) + + # println(d_f_his) + # println("input from simulator node",round(u_current,2)) + z_current[i,:],slip_ang[i,:] = simDynModel_xy(z_current[i-1,:], u_current', dt, modelParams) + + + z_real.t_msg[i] = t + z_real.t[i] = t + slip_a.t_msg[i] = t + slip_a.t[i] = t + + # IMU measurements + if i%2 == 0 # 50 Hz + imu_drift = 0#1+(t-t0)/100#sin(t/100*pi/2) # drifts to 1 in 100 seconds (and add random start value 1) + rand_yaw = 0.05*randn() + if rand_yaw > 0.1 + rand_yaw = 0.1 + elseif rand_yaw<-0.1 + rand_yaw=-0.1 + end + + yaw = z_current[i,5] + imu_drift + rand_yaw#+ 0.002*randn() + + rand_psiDot = 0.01*randn() + if rand_psiDot > 0.1 + rand_psiDot = 0.1 + elseif rand_psiDot<-0.1 + rand_psiDot=-0.1 + end + psiDot = z_current[i,6] #+rand_psiDot#+ 0.001*randn() + imu_meas.t_msg[imu_meas.i] = t + imu_meas.t[imu_meas.i] = t + imu_meas.z[imu_meas.i,:] = [yaw psiDot] + imu_meas.i += 1 + imu_data.orientation = geometry_msgs.msg.Quaternion(0, 0, sin(yaw/2), cos(yaw/2)) + imu_data.angular_velocity = Vector3(0,0,psiDot) + imu_data.header.stamp = t_ros + + rand_accX = 0.01*randn() + if rand_accX > 0.1 + rand_accX = 0.1 + elseif rand_accX<-0.1 + rand_accX=-0.1 + end + + imu_data.linear_acceleration.x = diff(z_current[i-1:i,3])[1]/dt - z_current[i-1,6]*z_current[i-1,4] +rand_accX#+ randn()*0.3*1.0 + + rand_accY = 0.01*randn() + if rand_accY > 0.1 + rand_accY = 0.1 + elseif rand_accY<-0.1 + rand_accY=-0.1 + end + + # imu_data.linear_acceleration.y = diff(z_current[i-1:i,4])[1]/dt + z_current[i,6]*z_current[i,3] +rand_accY#+ randn()*0.3*1.0 + imu_data.linear_acceleration.y = (z_current[i,4]-z_current[i-1,4])/dt + z_current[i-1,6]*z_current[i-1,3] +rand_accY#+ randn()*0.3*1.0 + publish(pub_imu, imu_data) # Imu format is defined by ROS, you can look it up by google "rosmsg Imu" + # It's sufficient to only fill the orientation part of the Imu-type (with one quaternion) + # println("ay from simulator:",imu_data.linear_acceleration.y) + real_data.a_y = imu_data.linear_acceleration.y + end + + # real values + if i%6 == 0 + # ADDITIONAL NOISE ADDING + n=randn(3) + n_thre = [0.1,0.1,0.5]*0.01 + # IMPORTANT!!!: THE DATA NEEDS TO BE IN THE SAME ORDER AS Z_EST + # n_thre = 0.01*[0.02,0.005,0.05,0.005,0.005,0.001] + # s # 2.5% ey: 0.4 is set as the track width # epsi: 1.8 degree # vx # vy: on the track, max vy can be 0.1 # psidot: on the track, max psidot can be 1 + n.*=n_thre + n=min(n, n_thre) + n=max(n,-n_thre) + real_data.psiDot = z_current[i,6] + n[3] + real_data.psi = z_current[i,5] + real_data.v_x = z_current[i,3] + n[1] + # println("v from simulator",z_current[i,3]) + real_data.v_y = z_current[i,4] + n[2] + real_data.x = z_current[i,1] + real_data.y = z_current[i,2] + publish(real_val,real_data) + + end + + # Velocity measurements + dist_traveled += norm(diff(z_current[i-1:i,1:2])) + if i%5 == 0 # 20 Hz + if sum(dist_traveled .>= vel_dist_update)>=1 && z_current[i,3] > 0.1 # only update if at least one of the magnets has passed the sensor + dist_traveled[dist_traveled.>=vel_dist_update] = 0 + vel_est.vel_est = convert(Float32,norm(z_current[i,3:4]))#+0.00*randn()) + vel_est.vel_fl = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_fr = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_bl = convert(Float32,norm(z_current[i,3:4])) + vel_est.vel_br = convert(Float32,norm(z_current[i,3:4])) + end + vel_est.header.stamp = t_ros + publish(pub_vel, vel_est) + end + + # GPS measurements + if i%6 == 0 # 16 Hz + + rand_x = 0.01*randn() + if rand_x > 0.1 + rand_x = 0.1 + elseif rand_x<-0.1 + rand_x=-0.1 + end + + x = round(z_current[i,1] + rand_x,2)#0.002*randn(),2) # Indoor gps measures, rounded on cm + + rand_y = 0.01*randn() + if rand_y > 0.1 + rand_y = 0.1 + elseif rand_y<-0.1 + rand_y=-0.1 + end + + y = round(z_current[i,2] + rand_y,2)#0.002*randn(),2) + + if randn()>10 # simulate gps-outlier (probability about 0.13% for randn()>3, 0.62% for randn()>2.5, 2.3% for randn()>2.0 ) + x += 1#randn() # add random value to x and y + y -= 1#randn() + #sim_gps_interrupt = 6 # also do a little interruption + elseif randn()>10 && sim_gps_interrupt < 0 + sim_gps_interrupt = 10 # simulate gps-interrupt (10 steps at 25 Hz is 0.4 seconds) + end + if sim_gps_interrupt < 0 + gps_meas.t_msg[gps_meas.i] = t + gps_meas.t[gps_meas.i] = t + gps_meas.z[gps_meas.i,:] = [x y] + gps_meas.i += 1 + # gps_data.header.stamp = get_rostime() + gps_data.x_m = x + gps_data.y_m = y + # println("gps from simulator",x,y) + publish(pub_gps, gps_data) + end + sim_gps_interrupt -= 1 + end + # println("True state from LMPC nodes",[real_data.v_x,real_data.v_y,real_data.psiDot]) + + i += 1 + rossleep(loop_rate) + end + + # Clean up buffers + clean_up(gps_meas) + clean_up(imu_meas) + clean_up(cmd_log) + z_real.z[1:i-1,:] = z_current[1:i-1,:] + slip_a.z[1:i-1,:] = slip_ang[1:i-1,:] + z_real.i = i + slip_a.i = i + clean_up(z_real) + clean_up(slip_a) + + # Save simulation data to file + log_path = "$(homedir())/simulations/output-SIM-$(run_id[1:4]).jld" + # save(log_path,"gps_meas",gps_meas,"z",z_real,"imu_meas",imu_meas,"cmd_log",cmd_log,"slip_a",slip_a) + println("Exiting node... Saving data to $log_path. Simulated $((i-1)*dt) seconds.") + +end + +if ! isinteractive() + main() +end diff --git a/workspace/src/barc/src/config_2.jl b/workspace/src/barc/src/config_2.jl new file mode 100755 index 00000000..6ae7cb1f --- /dev/null +++ b/workspace/src/barc/src/config_2.jl @@ -0,0 +1,86 @@ +#!/usr/bin/env julia + +#= + File name: config_2.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +# const MODE = "path_following" +# const MODE = "learning" +const MODE = "racing" + +if MODE == "racing" + # const NUM_AGENTS = 1 + const NUM_AGENTS = 2 +else + const NUM_AGENTS = 1 +end + +# const SYS_ID = true + +const INITIALIZATION_TYPE = "center" + +if MODE == "path_following" + const NUM_PF_LAPS = 5 + const LEARNING = false + const NUM_LAPS = NUM_PF_LAPS + const NUM_LOADED_LAPS = 0 +elseif MODE == "learning" + const NUM_PF_LAPS = 1 + const LEARNING = true + const NUM_LAPS = 30 + const NUM_LOADED_LAPS = 5 +elseif MODE == "racing" + const NUM_PF_LAPS = 1 + const LEARNING = true + const NUM_LAPS = 30 + # const NUM_LOADED_LAPS = 5 + 10 + const NUM_LOADED_LAPS = 5 + 30 + 30 + 30 + + # const NUM_LOADED_LAPS = 5 +end + +const HORIZON = 10 +const NUM_CONSIDERED_LAPS = 4 +const NUM_HORIZONS = 2.0 +# const SELECTION_SHIFT = 0 +const SELECTION_SHIFT = Int64(HORIZON / 2) # round(Int64, 1 * HORIZON) +# const NUM_CONSIDERED_STATES = 2 * HORIZON * NUM_CONSIDERED_LAPS +const NUM_CONSIDERED_STATES = round(Int64, NUM_HORIZONS * HORIZON) * NUM_CONSIDERED_LAPS +const NUM_STATES_BUFFER = 30 +const SYS_ID_BEFORE = 15 +const SYS_ID_AFTER = 15 +@assert NUM_STATES_BUFFER > NUM_CONSIDERED_STATES / NUM_CONSIDERED_LAPS +@assert NUM_STATES_BUFFER > SYS_ID_BEFORE +@assert NUM_STATES_BUFFER > SYS_ID_AFTER +const MAXIMUM_NUM_ITERATIONS = 2000 +const INIT_STATES = [0.0 0.0 0.0 0.0 0.0 0.0] +const V_MAX = [2.0; 2.5] +const COLOR = ["red"; "blue"] +# const TRACK_NAME = "track_3" +# const TRACK_NAME = "oval" +const TRACK_NAME = "l_shape" +const TRACK_WIDTH = 1.2 +const TRACK_DIR = "/home/lukas/tracks/" +const POLYNOMIAL_CURVATURE = false + +const V_INIT = 1.2 +const EY_CENTER = 0.0 +const EY_OFFSET = 0.75 * TRACK_WIDTH / 2 +const EY_INNER = - EY_OFFSET +const EY_OUTER = EY_OFFSET + +if INITIALIZATION_TYPE == "center" + const CURRENT_REFERENCE = [0.0 EY_CENTER 0.0 V_INIT] +elseif INITIALIZATION_TYPE == "inner" + const CURRENT_REFERENCE = [0.0 EY_INNER 0.0 V_INIT] +elseif INITIALIZATION_TYPE == "outer" + const CURRENT_REFERENCE = [0.0 EY_OUTER 0.0 V_INIT] +else + error("INITIALIZATION_TYPE $(INITIALIZATION_TYPE) not available.") +end + +const FOCUSED_PLOT = false +const SAVE_PLOTS = false \ No newline at end of file diff --git a/workspace/src/barc/src/controller_circular.py b/workspace/src/barc/src/controller_circular.py deleted file mode 100755 index 3c06ed07..00000000 --- a/workspace/src/barc/src/controller_circular.py +++ /dev/null @@ -1,78 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from rospy import init_node, Subscriber, Publisher, get_param -from rospy import Rate, is_shutdown, ROSInterruptException -from barc.msg import ECU -from numpy import pi -import rospy - -############################################################# -def circular(t_i, t_0, t_f, d_f_target, FxR_target): - # rest - if t_i < t_0: - d_f = 0 - FxR = 0 - - # start moving - elif (t_i < t_f): - d_f = d_f_target - FxR = FxR_target - - # stop experiment - else: - d_f = 0 - FxR = 0 - - return (FxR, d_f) - - -############################################################# -def main_auto(): - # initialize ROS node - init_node('auto_mode', anonymous=True) - nh = Publisher('ecu', ECU, queue_size = 10) - - # set node rate - rateHz = get_param("controller/rate") - rate = Rate(rateHz) - dt = 1.0 / rateHz - t_i = 0 - - # get experiment parameters - t_0 = get_param("controller/t_0") # time to start test - t_f = get_param("controller/t_f") # time to end test - FxR_target = get_param("controller/FxR_target") - d_f_target = pi/180*get_param("controller/d_f_target") - - # main loop - while not is_shutdown(): - # get command signal - (FxR, d_f) = circular(t_i, t_0, t_f, d_f_target, FxR_target) - - # send command signal - ecu_cmd = ECU(FxR, d_f) - nh.publish(ecu_cmd) - - # wait - t_i += dt - rate.sleep() - -############################################################# -if __name__ == '__main__': - try: - main_auto() - except ROSInterruptException: - pass diff --git a/workspace/src/barc/src/controller_low_level.py b/workspace/src/barc/src/controller_low_level.py index 15fcdda3..21743459 100755 --- a/workspace/src/barc/src/controller_low_level.py +++ b/workspace/src/barc/src/controller_low_level.py @@ -18,61 +18,172 @@ # Subscribes: steering and motor commands on 'ecu' # Publishes: combined ecu commands as 'ecu_pwm' -from rospy import init_node, Subscriber, Publisher, get_param +from rospy import init_node, Subscriber, Publisher, get_param, get_rostime from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown -from barc.msg import ECU -from numpy import pi +from barc.msg import ECU, pos_info import rospy -motor_pwm = 90 -servo_pwm = 90 -str_ang_max = 35 -str_ang_min = -35 +def ESCacc(vel, Acc): -def pwm_converter_callback(msg): - global motor_pwm, servo_pwm, b0 - global str_ang_max, str_ang_min + # Acc_1 = 0.9043 * Acc + 0.09113 + Acc_1 = 0.7043 * Acc + 0.09113 - # translate from SI units in vehicle model - # to pwm angle units (i.e. to send command signal to actuators) + if vel < 0.1: + PWM = (26.149 + Acc_1) / 0.2627 - # convert desired steering angle to degrees, saturate based on input limits - str_ang = max( min( 180.0/pi*msg.servo, str_ang_max), str_ang_min) - servo_pwm = 92.0558 + 1.8194*str_ang - 0.0104*str_ang**2 + elif (0.1 <= vel < 0.3): + PWM = (25.396 + Acc_1) / 0.2566 + + elif (0.3 <= vel < 0.5): + PWM = (24.581 + Acc_1) / 0.2453 + + elif (0.5 <= vel < 0.7): + PWM = (24.455 + Acc_1) / 0.243 + + elif (0.7 <= vel < 0.9): + PWM = (24.163 + Acc_1) / 0.2391 + + elif (0.9 <= vel < 1.1): + PWM = (23.75 + Acc_1) / 0.234 + + elif (1.1 <= vel < 1.3): + PWM = (23.048 + Acc_1) / 0.2262 + + elif (1.3 <= vel < 1.5): + PWM = (22.493 + Acc_1) / 0.2198 + + elif (1.5 <= vel < 1.7): + PWM = (22.368 + Acc_1) / 0.2174 + + elif (1.7 <= vel < 1.9): + PWM = (21.134 + Acc_1) / 0.2045 + + elif (1.9 <= vel < 2.1): + PWM = (19.881 + Acc_1) / 0.1915 + + elif (2.1 <= vel < 2.3): + PWM = (18.258 + Acc_1) / 0.1751 + + elif (2.3 <= vel < 2.5): + PWM = (13.228 + Acc_1) / 0.1265 + + else: + PWM = (10.602 + Acc_1) / 0.1006 + + return PWM + +########################################### + +def ESCdec(vel, Acc): + + Acc_1 = 1.4971 * Acc + 0.1971 + + if vel < 0.1: + PWM = 88.34 * Acc_1 + 145.41 + + elif (0.1 <= vel < 0.3): + PWM = 70.92 * Acc_1 + 128.22 + + elif (0.3 <= vel < 0.5): + PWM = 58.14 * Acc_1 + 126.61 + + elif (0.5 <= vel < 0.7): + PWM = 37.45 * Acc_1 + 102.98 + + elif (0.7 <= vel < 0.9): + PWM = 23.31 * Acc_1 + 92.23 + + elif (0.9 <= vel < 1.1): + PWM = 6.58 * Acc_1 + 79.61 + + elif (1.1 <= vel < 1.3): + PWM = 29.85 * Acc_1 + 107.69 + + elif (1.3 <= vel < 1.5): + PWM = 30.03 * Acc_1 + 108.73 + + elif (1.5 <= vel < 1.7): + PWM = 37.45 * Acc_1 + 115.3 - # compute motor command - FxR = float(msg.motor) - if FxR == 0: - motor_pwm = 90.0 - elif FxR > 0: - motor_pwm = FxR/b0 + 95.0 else: - motor_pwm = 90.0 - update_arduino() + PWM = 22 * Acc_1 + 100 + + return PWM -def neutralize(): - global motor_pwm +class low_level_control(object): motor_pwm = 90 servo_pwm = 90 - update_arduino() - -def update_arduino(): - global motor_pwm, servo_pwm, ecu_pub - ecu_cmd = ECU(motor_pwm, servo_pwm) - ecu_pub.publish(ecu_cmd) + str_ang_max = 35 + str_ang_min = -35 + ecu_pub = 0 + ecu_cmd = ECU() + vx = 0 + + def vx_callback(self, data): + self.vx = data.v_x + + def pwm_converter_callback(self, msg): + # translate from SI units in vehicle model + # to pwm angle units (i.e. to send command signal to actuators) + # convert desired steering angle to degrees, saturate based on input limits + + # Old servo control: + # self.servo_pwm = 91.365 + 105.6*float(msg.servo) + # New servo Control + # if msg.servo < 0.0: # right curve + # self.servo_pwm = 95.5 + 118.8*float(msg.servo) + # elif msg.servo > 0.0: # left curve + # # self.servo_pwm = 90.8 + 78.9*float(msg.servo) + # self.servo_pwm = 90.0 + 89.0*float(msg.servo) + self.servo_pwm = 90.0 + 89.0*float(msg.servo) + + # compute motor command + FxR = float(msg.motor) + if FxR == 0: + self.motor_pwm = 90.0 + elif FxR > 0: + #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + # self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 7.5*FxR # using writeMicroseconds() in Arduino + # self.motor_pwm = ESCacc(self.vx, FxR) + # self.motor_pwm = max(94,90.74 + 6.17*FxR) + #self.motor_pwm = 90.74 + 6.17*FxR + + #self.motor_pwm = max(94,90.12 + 5.24*FxR) + #self.motor_pwm = 90.12 + 5.24*FxR + # Note: Barc doesn't move for u_pwm < 93 + else: # motor break / slow down + self.motor_pwm = 93.5 + 46.73*FxR + # self.motor_pwm = ESCdec(self.vx, FxR) + + # self.motor_pwm = 98.65 + 67.11*FxR + #self.motor = 69.95 + 68.49*FxR + self.update_arduino() + def neutralize(self): + self.motor_pwm = 40 # slow down first + self.servo_pwm = 90 + self.update_arduino() + rospy.sleep(1) # slow down for 1 sec + self.motor_pwm = 90 + self.update_arduino() + def update_arduino(self): + self.ecu_cmd.header.stamp = get_rostime() + self.ecu_cmd.motor = self.motor_pwm + self.ecu_cmd.servo = self.servo_pwm + self.ecu_pub.publish(self.ecu_cmd) def arduino_interface(): - global ecu_pub, b0 - # launch node, subscribe to motorPWM and servoPWM, publish ecu init_node('arduino_interface') - b0 = get_param("input_gain") + llc = low_level_control() + + Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size = 1) + Subscriber('pos_info', pos_info, llc.vx_callback, queue_size=1) - Subscriber('ecu', ECU, pwm_converter_callback, queue_size = 10) - ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 10) + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) # Set motor to neutral on shutdown - on_shutdown(neutralize) + on_shutdown(llc.neutralize) # process callbacks and keep alive spin() diff --git a/workspace/src/barc/src/controller_low_level_lukas.py b/workspace/src/barc/src/controller_low_level_lukas.py new file mode 100755 index 00000000..59962cf8 --- /dev/null +++ b/workspace/src/barc/src/controller_low_level_lukas.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed at UC +# Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu) and Greg Marcil (grmarcil@berkeley.edu). The cloud +# services integation with ROS was developed by Kiet Lam +# (kiet.lam@berkeley.edu). The web-server app Dator was based on an open source +# project by Bruce Wootton +# --------------------------------------------------------------------------- + +# README: This node serves as an outgoing messaging bus from odroid to arduino +# Subscribes: steering and motor commands on 'ecu' +# Publishes: combined ecu commands as 'ecu_pwm' + +from rospy import init_node, Subscriber, Publisher, get_param, get_rostime +from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown +from barc.msg import ECU, pos_info +from state_estimation_SensorKinematicModel_lukas import EncClass, ImuClass +import rospy +import numpy as np +import os + +class low_level_control(object): + motor_pwm = 89 + servo_pwm = 90 + str_ang_max = 35 + str_ang_min = -35 + ecu_pub = 0 + ecu_cmd = ECU() + def pwm_converter_callback(self, msg): + # translate from SI units in vehicle model + # to pwm angle units (i.e. to send command signal to actuators) + # convert desired steering angle to degrees, saturate based on input limits + + # Old servo control: + # self.servo_pwm = 91.365 + 105.6*float(msg.servo) + # New servo Control + # if msg.servo < 0.0: # right curve + # self.servo_pwm = 95.5 + 118.8*float(msg.servo) + # elif msg.servo > 0.0: # left curve + # self.servo_pwm = 90.8 + 78.9*float(msg.servo) + + # self.servo_pwm = 90.0 + 89.0*float(msg.servo) + self.servo_pwm = 83.3 + 103.1 * float(msg.servo) + + + # compute motor command + FxR = float(msg.motor) + if FxR == 0: + self.motor_pwm = 90.0 + elif FxR > 0: + #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + + # self.motor_pwm = max(94,90.74 + 6.17*FxR) + #self.motor_pwm = 90.74 + 6.17*FxR + + #self.motor_pwm = max(94,90.12 + 5.24*FxR) + #self.motor_pwm = 90.12 + 5.24*FxR + # Note: Barc doesn't move for u_pwm < 93 + else: # motor break / slow down + self.motor_pwm = 93.5 + 46.73*FxR + # self.motor_pwm = 98.65 + 67.11*FxR + #self.motor = 69.95 + 68.49*FxR + self.update_arduino() + def neutralize(self): + self.motor_pwm = 40 # slow down first + self.servo_pwm = 90 + self.update_arduino() + rospy.sleep(1) # slow down for 1 sec + self.motor_pwm = 80 + self.update_arduino() + def update_arduino(self): + self.ecu_cmd.header.stamp = get_rostime() + self.ecu_cmd.motor = self.motor_pwm + self.ecu_cmd.servo = self.servo_pwm + self.ecu_pub.publish(self.ecu_cmd) + + def pwm_convert_acc(self, acc): + FxR = float(acc) + if FxR == 0: + motor_pwm = 90.0 + elif FxR > 0: + motor_pwm = 91 + 6.5 * FxR # using writeMicroseconds() in Arduino + + else: # motor break / slow down + motor_pwm = 93.5 + 46.73 * FxR + + return motor_pwm + + def pwm_convert_steering(self, steering): + return 81.4 + 89.1 * float(steering) + # return 83.3 + 103.1 * float(steering) + +def arduino_interface(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + init_node('arduino_interface') + llc = low_level_control() + + Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size = 1) + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + # process callbacks and keep alive + spin() + +def steering_mapping(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + # init_node('arduino_interface') + + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering_pwm = rospy.get_param(node_name + "/steering_pwm") + acceleration_pwm = rospy.get_param(node_name + "/acceleration_pwm") + acceleration_neg_pwm = rospy.get_param(node_name + "/acceleration_neg_pwm") + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + acceleration_time = 3 # accelerate for four seconds + max_counts = loop_rate * acceleration_time + + """ + if mode == "steering": + max_counts = 100 + + for j in range(60, 130, 10): + i = 0 + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = j + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + i += 1 + rate.sleep() + """ + + if mode == "steering": + max_counts = 1500 + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + i += 1 + rate.sleep() + + elif mode == "acceleration": + + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + if enc.v_rr > 1e-5: + i += 1 + rate.sleep() + + while not rospy.is_shutdown(): + llc.motor_pwm = acceleration_neg_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + if mode == "acceleration": + directory_string = str(steering_pwm) + "_" + str(acceleration_pwm) + "_" + str(acceleration_neg_pwm) + else: + directory_string = str(steering_pwm) + "_" + str(acceleration_pwm) + + directory = directory + directory_string + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + +def corner_stiffness(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering = rospy.get_param(node_name + "/steering") + acceleration = rospy.get_param(node_name + "/acceleration") + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + duration = 60 # accelerate for 60 seconds + max_counts = loop_rate * duration + + pwm_acc = llc.pwm_convert_acc(acceleration) + pwm_steering = llc.pwm_convert_steering(steering) + + print pwm_steering + + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = pwm_acc + llc.servo_pwm = pwm_steering + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + if enc.v_rr > 1e-5: + enc.saveHistory() + imu.saveHistory() + i += 1 + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + directory_string = str(steering) + "_" + str(acceleration) + directory = directory + directory_string + + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + +def increasing_acc(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering = rospy.get_param(node_name + "/steering") + acceleration_1 = rospy.get_param(node_name + "/acceleration_1") + acceleration_2 = rospy.get_param(node_name + "/acceleration_2") + acceleration_3 = rospy.get_param(node_name + "/acceleration_3") + + acc = [acceleration_1, acceleration_2, acceleration_3] + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + duration = 10 # accelerate for 60 seconds + max_counts = loop_rate * duration + + pwm_acc = llc.pwm_convert_acc(acceleration) + pwm_steering = llc.pwm_convert_steering(steering) + + i = 0 + j = 0 + + while not rospy.is_shutdown(): + if i <= max_counts: + llc.motor_pwm = llc.pwm_convert_acc(acc[j]) + llc.servo_pwm = llc.pwm_convert_steering(steering) + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + if enc.v_rr > 1e-5: + enc.saveHistory() + imu.saveHistory() + i += 1 + + if i == max_counts and j < 3: + i = 0 + j += 1 + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + directory_string = str(steering) + "_" + str(acc[0]) + "_" + str(acc[1]) + "_" + str(acc[2]) + directory = directory + directory_string + + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + +############################################################# +if __name__ == '__main__': + try: + init_node('arduino_interface') + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + + if mode == "corner_stiffness": + corner_stiffness() + elif mode == "acceleration" or mode == "steering": + steering_mapping() + elif mode == "increasing_acc": + increasing_acc() + else: + print "Chosen mode unavailable." + # arduino_interface() + except ROSInterruptException: + pass diff --git a/workspace/src/barc/src/controller_mpc.jl b/workspace/src/barc/src/controller_mpc.jl deleted file mode 100755 index 5ad0b972..00000000 --- a/workspace/src/barc/src/controller_mpc.jl +++ /dev/null @@ -1,109 +0,0 @@ -#!/usr/bin/env julia - -#= - Licensing Information: You are free to use or extend these projects for - education or reserach purposes provided that (1) you retain this notice - and (2) you provide clear attribution to UC Berkeley, including a link - to http://barc-project.com - - Attibution Information: The barc project ROS code-base was developed - at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales - (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed - by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was - based on an open source project by Bruce Wootton -=# - -using RobotOS -@rosimport barc.msg: ECU, Encoder, Ultrasound, Z_KinBkMdl -@rosimport data_service.msg: TimeData -@rosimport geometry_msgs.msg: Vector3 -rostypegen() -using barc.msg -using data_service.msg -using geometry_msgs.msg -using JuMP -using Ipopt - -# define model parameters -L_a = 0.125 # distance from CoG to front axel -L_b = 0.125 # distance from CoG to rear axel -dt = 0.1 # time step of system -a_max = 1 # maximum acceleration - -# preview horizon -N = 5 - -# define targets [generic values] -x_ref = 10 -y_ref = 0 - -# define decision variables -# states: position (x,y), yaw angle, and velocity -# inputs: acceleration, steering angle -println("Creating kinematic bicycle model ....") -mdl = Model(solver = IpoptSolver(print_level=3)) -@defVar( mdl, x[1:(N+1)] ) -@defVar( mdl, y[1:(N+1)] ) -@defVar( mdl, psi[1:(N+1)] ) -@defVar( mdl, v[1:(N+1)] ) -@defVar( mdl, a[1:N] ) -@defVar( mdl, d_f[1:N] ) - -# define objective function -@setNLObjective(mdl, Min, (x[N+1] - x_ref)^2 + (y[N+1] - y_ref)^2 ) - -# define constraints -# define system dynamics -# Reference: R.Rajamani, Vehicle Dynamics and Control, set. Mechanical Engineering Series, -# Spring, 2011, page 26 -@defNLParam(mdl, x0 == 0); @addNLConstraint(mdl, x[1] == x0); -@defNLParam(mdl, y0 == 0); @addNLConstraint(mdl, y[1] == y0); -@defNLParam(mdl, psi0 == 0); @addNLConstraint(mdl, psi[1] == psi0 ); -@defNLParam(mdl, v0 == 0); @addNLConstraint(mdl, v[1] == v0); -@defNLExpr(mdl, bta[i = 1:N], atan( L_a / (L_a + L_b) * tan(d_f[i]) ) ) -for i in 1:N - @addNLConstraint(mdl, x[i+1] == x[i] + dt*(v[i]*cos( psi[i] + bta[i] )) ) - @addNLConstraint(mdl, y[i+1] == y[i] + dt*(v[i]*sin( psi[i] + bta[i] )) ) - @addNLConstraint(mdl, psi[i+1] == psi[i] + dt*(v[i]/L_b * sin(bta[i])) ) - @addNLConstraint(mdl, v[i+1] == v[i] + dt*(a[i]) ) - @addConstraint(mdl, 0 <= a[i] <= a_max ) -end - -# status update -println("initial solve ...") -solve(mdl) -println("finished initial solve!") - -function SE_callback(msg::Z_KinBkMdl) - # update mpc initial condition - setValue(x0, msg.x) - setValue(y0, msg.y) - setValue(psi0, msg.psi) - setValue(v0, msg.v) -end - -function main() - # initiate node, set up publisher / subscriber topics - init_node("mpc") - pub = Publisher("ecu", ECU, queue_size=10) - sub = Subscriber("state_estimate", Z_KinBkMdl, SE_callback, queue_size=10) - loop_rate = Rate(10) - - while ! is_shutdown() - # run mpc, publish command - solve(mdl) - - # get optimal solutions - a_opt = getValue(a[1]) - d_f_opt = getValue(d_f[1]) - cmd = ECU(a_opt, d_f_opt) - - # publish commands - publish(pub, cmd) - rossleep(loop_rate) - end -end - -if ! isinteractive() - main() -end diff --git a/workspace/src/barc/src/controller_straight.py b/workspace/src/barc/src/controller_straight.py deleted file mode 100755 index 3a910206..00000000 --- a/workspace/src/barc/src/controller_straight.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from rospy import init_node, Subscriber, Publisher, get_param -from rospy import Rate, is_shutdown, ROSInterruptException -from barc.msg import ECU, Z_KinBkMdl -from sensor_msgs.msg import Imu -from math import pi -from numpy import zeros, array -from numpy import unwrap -from tf import transformations -from pid import PID -import numpy as np -import rospy - -# raw measurement variables -yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) -yaw_prev = 0 -yaw_local = 0 -read_yaw0 = False - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global yaw - global yaw_prev, yaw0, read_yaw0, yaw_local - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (_,_, yaw) = transformations.euler_from_quaternion(quaternion) - - # save initial measurements - if not read_yaw0: - read_yaw0 = True - yaw_prev = yaw - yaw0 = yaw - - # unwrap measurement - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw - yaw_local = yaw - yaw0 - - # extract angular velocity and linear acceleration data - # w_z = data.angular_velocity.z - - -############################################################ -def se_callback(data): - global yaw, read_yaw0 - yaw = data.psi - - if not read_yaw0: - read_yaw0 = True - -############################################################# -def straight(t_i, pid, time_params,FxR_target): - global yaw_local - - # unpack parameters - (t_0, t_f, dt) = time_params - - # rest - if t_i < t_0: - d_f = 0 - FxR = 0 - - # start moving - elif (t_i < t_f): - d_f = pid.update(yaw_local, dt) - step_up = float(t_i - t_0) / 50.0 - FxR = np.min([ step_up, FxR_target]) - - # stop experiment - else: - d_f = 0 - FxR = 0 - - return (FxR, d_f) - -############################################################# -def main_auto(): - global read_yaw0, yaw_local - - # initialize ROS node - init_node('auto_mode', anonymous=True) - ecu_pub = Publisher('ecu', ECU, queue_size = 10) - se_sub = Subscriber('imu/data', Imu, imu_callback) - - # set node rate - rateHz = get_param("controller/rate") - rate = Rate(rateHz) - dt = 1.0 / rateHz - - # get PID parameters - p = get_param("controller/p") - i = get_param("controller/i") - d = get_param("controller/d") - pid = PID(P=p, I=i, D=d) - setReference = False - - # get experiment parameters - t_0 = get_param("controller/t_0") # time to start test - t_f = get_param("controller/t_f") # time to end test - FxR_target = get_param("controller/FxR_target") - t_params = (t_0, t_f, dt) - - while not is_shutdown(): - - # OPEN LOOP - if read_yaw0: - # set reference angle - if not setReference: - pid.setPoint(yaw_local) - setReference = True - t_i = 0.0 - # apply open loop command - else: - (FxR, d_f) = straight(t_i, pid, t_params, FxR_target) - ecu_cmd = ECU(FxR, d_f) - ecu_pub.publish(ecu_cmd) - t_i += dt - - # wait - rate.sleep() - -############################################################# -if __name__ == '__main__': - try: - main_auto() - except ROSInterruptException: - pass diff --git a/workspace/src/barc/src/corner_stiffness.jl b/workspace/src/barc/src/corner_stiffness.jl new file mode 100644 index 00000000..acb9d37e --- /dev/null +++ b/workspace/src/barc/src/corner_stiffness.jl @@ -0,0 +1,157 @@ +using NPZ +using JuMP +using Ipopt + +type CornerStiffnessIdentification + mdl::JuMP.Model + + v_x::Array{JuMP.NonlinearParameter,1} + d_f::Array{JuMP.NonlinearParameter, 1} + psi_dot::Array{JuMP.NonlinearParameter,1} + + c_f::JuMP.Variable + c_r::JuMP.Variable + beta::Array{JuMP.Variable, 1} + + alpha_f::Array{JuMP.NonlinearExpression, 1} + alpha_r::Array{JuMP.NonlinearExpression, 1} + F_yf::Array{JuMP.NonlinearExpression, 1} + F_yr::Array{JuMP.NonlinearExpression, 1} + + cost::JuMP.NonlinearExpression + + function CornerStiffnessIdentification(num_experiments::Int64) + println("Starting identification of corner stiffness") + m = new() + + l_f = 0.125 + l_r = 0.125 + mass = 2.0 # old Barc + # mass = 1.75 # new Barc + I_z = 0.24 + + # Create Model + mdl = Model(solver = IpoptSolver(print_level=0, linear_solver="ma27")) + + # Create variables (these are going to be optimized) + @variable(mdl, c_f >= 0, start = 0) + @variable(mdl, c_r >= 0, start = 0) + @variable(mdl, beta[1 : num_experiments], start = 0) + + # Set bounds + beta_lb = ones(num_experiments) * (- 1.0) # lower bound on beta + beta_ub = ones(num_experiments) * 1.0 # upper bound on beta + + for i = 1 : num_experiments + setlowerbound(beta[i], beta_lb[i]) + setupperbound(beta[i], beta_ub[i]) + end + + @NLparameter(mdl, v_x[i = 1 : num_experiments] == 0) + @NLparameter(mdl, d_f[i = 1 : num_experiments] == 0) + @NLparameter(mdl, psi_dot[i = 1 : num_experiments] == 0) + + @NLexpression(mdl, alpha_f[i = 1 : num_experiments], atan(beta[i] + l_f * psi_dot[i] / v_x[i]) - d_f[i]) + @NLexpression(mdl, alpha_r[i = 1 : num_experiments], atan(beta[i] - l_r * psi_dot[i] / v_x[i])) + @NLexpression(mdl, F_yf[i = 1 : num_experiments], - alpha_f[i] * c_f) + @NLexpression(mdl, F_yr[i = 1 : num_experiments], - alpha_r[i] * c_r) + + @NLexpression(mdl, cost_translation, sum{(1 / (mass * v_x[i]) * (F_yf[i] + F_yr[i]) + psi_dot[i])^2, i = 1 : num_experiments}) + @NLexpression(mdl, cost_rotation, sum{(1 / I_z * (l_f * F_yf[i] - l_r * F_yr[i]))^2, i = 1 : num_experiments}) + + @NLobjective(mdl, Min, cost_translation + cost_rotation) + + m.mdl = mdl + m.c_f = c_f + m.c_r = c_r + m.beta = beta + m.v_x = v_x + m.d_f = d_f + m.psi_dot = psi_dot + m.alpha_f = alpha_f + m.alpha_r = alpha_r + m.F_yf = F_yf + m.F_yr = F_yr + return m + end +end + +function identifiy_corner_stiffness(mdl::CornerStiffnessIdentification, v_x::Array{Float64}, + psi_dot::Array{Float64}, d_f::Array{Float64}) + mass = 1.75 + sol_status::Symbol + optimal_cf::Float64 + optimal_cr::Float64 + + setvalue(mdl.v_x, v_x) + setvalue(mdl.d_f, d_f) + setvalue(mdl.psi_dot, psi_dot) + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + optimal_cf = getvalue(mdl.c_f) + optimal_cr = getvalue(mdl.c_r) + + println("max alpha_f: ", findmax(getvalue(mdl.alpha_f))[1]) + println("max alpha_r: ", findmax(getvalue(mdl.alpha_r))[1]) + # beta_order = sortperm(getvalue(mdl.beta)) + # println("beta: ", getvalue(mdl.beta)[beta_order]) + println("beta: ", getvalue(mdl.beta)) + beta = getvalue(mdl.beta) + + + println("Solved, status = $(sol_status)") + println("Optimal C_f: ", optimal_cf) + println("Optimal C_r: ", optimal_cr) + + indices = abs(beta) .< 0.7 + println(beta[indices]) + alpha_f = getvalue(mdl.alpha_f)[indices] + alpha_r = getvalue(mdl.alpha_r)[indices] + F_yf = getvalue(mdl.F_yf)[indices] + F_yr = getvalue(mdl.F_yr)[indices] + println(getvalue(mdl.alpha_f)[indices]) + println(getvalue(mdl.alpha_r)[indices]) + + println("Max Force: ", mass * 9.81 / 2) + println("Fyf: ", findmax(F_yf)[1]) + println("Fyr: ", findmax(F_yr)[1]) + + return indices +end + + +dir = homedir() * "/barc_debugging/corner_stiffness/" +folders = readdir(dir) +num_experiments = size(folders, 1) + +average_vx = zeros(num_experiments) +average_psidot = zeros(num_experiments) +average_df = zeros(num_experiments) + +for i = 1 : num_experiments + # old Barc: + data_vx = 0.5 * (npzread(dir * folders[i] * "/estimator_enc.npz")["v_rr_his"] + npzread(dir * folders[i] * "/estimator_enc.npz")["v_rl_his"]) + # new Barc: + # data_vx = npzread(dir * folders[i] * "/estimator_enc.npz")["v_rr_his"] + data_psidot = npzread(dir * folders[i] * "/estimator_imu.npz")["psiDot_his"] + + average_vx[i] = mean(data_vx[100 : end - 50]) + average_psidot[i] = mean(data_psidot[100 : end - 50]) + average_df[i] = float(split(folders[i], "_")[1]) + + println(size(data_vx)) +end + +mdl = CornerStiffnessIdentification(num_experiments) +# mdl.CornerStiffnessIdentification(num_experiments) + +indices = identifiy_corner_stiffness(mdl, average_vx, average_psidot, average_df) + +num_experiments = size(average_vx[indices], 1) +mdl2 = CornerStiffnessIdentification(num_experiments) + +println(average_vx[indices]) +println(average_psidot[indices]) +println(average_df[indices]) +identifiy_corner_stiffness(mdl2, average_vx[indices], average_psidot[indices], average_df[indices]) diff --git a/workspace/src/barc/src/create_low_level_map.py b/workspace/src/barc/src/create_low_level_map.py new file mode 100755 index 00000000..97a15e88 --- /dev/null +++ b/workspace/src/barc/src/create_low_level_map.py @@ -0,0 +1,260 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +import pdb + +PLOT = False + +l_f = 0.125 +l_r = 0.125 + +homedir = os.path.expanduser("~") + +steering_str = homedir + "/barc_debugging/steering/" +acc_str = homedir + "/barc_debugging/acceleration/" +steering_dir = os.listdir(steering_str) +acc_dir = os.listdir(acc_str) + +X = 0. +d_f = 0. +pwm_entries = 0. + +first = True + +for file in steering_dir: + print file + pathSave = steering_str + file + "/estimator_imu.npz" + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + + pathSave = steering_str + file + "/estimator_enc.npz" + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + v_meas_his = npz_enc["v_meas_his"] + enc_time = npz_enc["enc_time"] + + v_his = 0.5 * (v_rl_his + v_rr_his) + # v_his = v_rr_his + + """ + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + npz_ecu = np.load(pathSave) + a_his = npz_ecu["a_his"] + df_his = npz_ecu["df_his"] + ecu_time = npz_ecu["ecu_time"] + """ + + # Calculation of steering angle + steering_pwm = float(file.split("_")[0]) + + mean_vel = np.mean(v_his) + first_index = np.argmax(v_his > mean_vel) + enc_time = enc_time[first_index:-25] + imu_time = imu_time[first_index:-25] + psi_dot = psiDot_his[first_index:-25] + v_x = v_his[first_index:-25] + + v_x = np.interp(imu_time, enc_time, v_x) + + d_f_ = np.arctan(psi_dot * (l_f + l_r) / v_x) + + pwm_entries_ = steering_pwm * np.ones_like(d_f_) + X_ = np.zeros((len(pwm_entries_), 2)) + X_[:, 0] = pwm_entries_ + X_[:, 1] = np.ones_like(d_f_) + + if first: + X = X_ + d_f = d_f_ + pwm_entries = pwm_entries_ + first = False + else: + X = np.vstack((X, X_)) + d_f = np.hstack((d_f, d_f_)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + if PLOT: + # FIGURE 2 plotting of IMU data + fig = plt.figure("Imu") + ax2 = fig.add_subplot(1,1,1,ylabel="IMU acc & psidot") + ax2.plot(imu_time, psi_dot, label="psiDot") + ax2.legend() + ax2.grid() + + # enc plot + fig = plt.figure("encoder") + ax4 = fig.add_subplot(1,1,1, ylabel="ax") + # ax4.plot(enc_time, v_fl_his, "--", label="fl") + # ax4.plot(enc_time, v_fr_his, "--", label="fr") + # ax4.plot(enc_time, v_rl_his, "-", label="rl") + ax4.plot(enc_time, v_x, "-", label="rr") + # ax4.plot(enc_time, v_meas_his, "-", label="meas") + ax4.legend() + ax4.grid() + + plt.show() + +theta = np.matmul(np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)), d_f) + +print theta + +theta_2 = np.zeros_like(theta) +theta_2[0] = 1.0 / theta[0] +theta_2[1] = - theta[1] / theta[0] + +print theta_2 + +pwms = np.array([50, 140]) +calc_steering = theta[0] * pwms + theta[1] + +# enc plot +fig = plt.figure("Steering Angle") +ax_df = fig.add_subplot(1,1,1, ylabel="ax") +ax_df.plot(pwm_entries, d_f, "*", label="d_f") +ax_df.plot(pwms, calc_steering, "-", label="calc_df") +ax_df.legend() +ax_df.grid() + +plt.show() + +v_dot = 0. +V = 0. +pwm_entries = 0. +C1_pos = 0. +C1_neg = 0. +C2_pos = 0. +C2_neg = 0. +CM = 0. +X = 0. + +first = True +positive = True + +for file in acc_dir: + print file + pathSave = acc_str + file + "/estimator_imu.npz" + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + + pathSave = acc_str + file + "/estimator_enc.npz" + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + v_meas_his = npz_enc["v_meas_his"] + enc_time = npz_enc["enc_time"] + + acc_pwm = float(file.split("_")[1]) + breaking_pwm = float(file.split("_")[2]) + + acc_start = np.argmax(v_rr_his > 0.5) + acc_max = np.argmax(v_rr_his) + acc_end = len(v_rr_his) - np.argmax(v_rr_his[:: -1] > 0.6) + + dummy_vrr = v_rr_his[acc_max : acc_end] + dummy_vrr2 = v_rr_his[acc_start : acc_max] + + for i in range(len(dummy_vrr2 - 1))[::-1]: + if dummy_vrr2[i - 1] == dummy_vrr2[i]: + dummy_vrr2 = np.delete(dummy_vrr2, i) + + for i in range(len(dummy_vrr - 1))[::-1]: + if dummy_vrr[i - 1] == dummy_vrr[i]: + dummy_vrr = np.delete(dummy_vrr, i) + + acc_pos = np.diff(dummy_vrr2) + acc_neg = np.diff(dummy_vrr) + + v_pos = dummy_vrr2[:-1] + v_neg = dummy_vrr[:-1] + + pwm_entries_ = acc_pwm * np.ones_like(acc_pos) + c1_pos = np.ones_like(acc_pos) + + X_ = np.transpose(np.vstack((pwm_entries_, c1_pos))) + + print X_.shape + + if positive: + if first: + v_dot = acc_pos + X = X_ + pwm_entries = pwm_entries_ + first = False + else: + print X_.shape + print X.shape + X = np.vstack((X, X_)) + v_dot = np.hstack((v_dot, acc_pos)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + c1_neg = np.ones_like(acc_neg) + pwm_entries_ = breaking_pwm * np.ones_like(acc_neg) + X_ = np.transpose(np.vstack((pwm_entries_, c1_neg))) + + if not positive: + if first: + v_dot = acc_neg + X = X_ + pwm_entries = pwm_entries_ + first = False + else: + print X_.shape + print X.shape + X = np.vstack((X, X_)) + v_dot = np.hstack((v_dot, acc_neg)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + PLOT = True + if PLOT: + + # enc plot + fig = plt.figure("encoder") + ax4 = fig.add_subplot(1,1,1,ylabel="ax") + # ax4.plot(enc_time, v_fl_his, "--", label="fl") + # ax4.plot(enc_time, v_fr_his, "--", label="fr") + # ax4.plot(enc_time, v_rl_his, "-", label="rl") + ax4.plot(enc_time, v_rr_his, "-", label="rr") + ax4.plot(enc_time[acc_start], v_rr_his[acc_start], "ro") + ax4.plot(enc_time[acc_max], v_rr_his[acc_max], "ko") + ax4.plot(enc_time[acc_end], v_rr_his[acc_end], "mo") + + # ax4.plot(enc_time, v_meas_his, "-", label="meas") + ax4.legend() + ax4.grid() + + plt.show() + +theta = np.matmul(np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)), v_dot) + +print theta + +pwms = np.array([40, 120]) +# Assuming velocity of 1.0 +calc_acc = theta[0] * pwms + theta[1] * 1 + +# enc plot +fig = plt.figure("Acceleration") +ax_df = fig.add_subplot(1,1,1, ylabel="ax") +ax_df.plot(pwm_entries, v_dot, "*", label="d_f") +ax_df.plot(pwms, calc_acc, "-", label="calc_df") +ax_df.legend() +ax_df.grid() + +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/estimator_max.py b/workspace/src/barc/src/estimator_max.py new file mode 100755 index 00000000..eb6a2050 --- /dev/null +++ b/workspace/src/barc/src/estimator_max.py @@ -0,0 +1,340 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +# from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos, hedge_imu_fusion +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size +from observers import ekf +from models_max import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0.0 + cmd_motor = 0.0 + cmd_t = 0.0 + + # IMU + yaw_prev = 0.0 + yaw0 = 0.0 # yaw at t = 0 + yaw_meas = 0.0 + psiDot_meas = 0.0 + a_x_meas = 0.0 + a_y_meas = 0.0 + imu_updated = False + att = (0.0,0.0,0.0) # attitude + + # Velocity + vel_meas = 0.0 + vel_updated = False + vel_prev = 0.0 + vel_count = 0.0 # this counts how often the same vel measurement has been received + + # GPS + x_meas = 0.0 + y_meas = 0.0 + gps_updated = False + x_hist = zeros(15) + y_hist = zeros(15) + t_gps = zeros(15) + c_X = array([0,0,0]) + c_Y = array([0,0,0]) + + # Estimator data + x_est = 0.0 + y_est = 0.0 + + # General variables + t0 = 0.0 # Time when the estimator was started + running = False # bool if the car is driving + + prev_time = 0.0 + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # ultrasound gps data + def gps_callback(self, data): + """This function is called when a new GPS signal is received.""" + # units: [rad] and [rad/s] + # get current time stamp + t_now = rospy.get_rostime().to_sec()-self.t0 + + #t_msg = data.timestamp_ms/1000.0 - self.t0 + + t_msg = t_now + + # if abs(t_now - t_msg) > 0.1: + # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + + # get current gps measurement + self.x_meas = data.x_m + self.y_meas = data.y_m + #print "Received coordinates : (%f, %f)" % (self.x_meas, self.y_meas) + + # check if we have good measurement + # compute distance we have travelled from previous estimate to current measurement + # if we've travelled more than 1 m, the GPS measurement is probably junk, so ignore it + # otherwise, store measurement, and then perform interpolation + dist = (self.x_est-data.x_m)**2 + (self.y_est-data.y_m)**2 + if dist < 1.0: + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_msg) + + # self.x_hist = delete(self.x_hist,0) + # self.y_hist = delete(self.y_hist,0) + # self.t_gps = delete(self.t_gps,0) + + # Keep only the last second worth of coordinate data in the x_hist and y_hist buffer + # These buffers are used for interpolation + # without overwriting old data, the arrays would grow unbounded + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + sz = size(self.t_gps, 0) + + # perform interpolation for (x,y) as a function of time t + # getting two function approximations x(t) and y(t) + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # use least squares to get the coefficients for this function approximation + # using (x,y) coordinate data from the past second (time) + if sz > 4: + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T # input matrix: [ t^2 t 1 ] + self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] + self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] + self.gps_updated = True + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + if self.prev_time > 0: + self.yaw_meas += self.psiDot_meas * (current_t - self.prev_time) + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + + """ + yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # from this point on 'yaw' will be definitely unwrapped (smooth)! + if not self.running: + self.yaw0 = yaw # set yaw0 to current yaw + self.yaw_meas = 0 # and current yaw to zero + else: + self.yaw_meas = yaw - self.yaw0 + #print "yaw measured: %f" % self.yaw_meas * 180 / math.pi + """ + + # extract angular velocity and linear acceleration data + #w_x = data.angular_velocity.x + #w_y = data.angular_velocity.y + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + # The next two lines 'project' the measured linear accelerations to a horizontal plane + self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + #self.a_x_meas = a_x + #self.a_y_meas = a_y + self.att = (roll_raw,pitch_raw,yaw_raw) + self.imu_updated = True + + self.prev_time = current_t + + def encoder_vel_callback(self, data): + if data.vel_est != self.vel_prev: + self.vel_meas = data.vel_est + self.vel_updated = True + self.vel_prev = data.vel_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + # self.vel_meas = data.vel_est + # self.vel_updated = True + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback) + rospy.Subscriber('vel_est', Vel_est, se.encoder_vel_callback) + rospy.Subscriber('ecu', ECU, se.ecu_callback) + rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1) + # rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, se.gps_callback, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() # set initial time + + z_EKF = zeros(14) # x, y, psi, v, psi_drift + P = eye(14) # initial dynamics coveriance matrix + + qa = 1000.0 + qp = 1000.0 + + Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.1, 0.2,0.2,1.0,1.0,0.1]) + R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + R = diag([4*5.0,4*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 4*5.0,4*5.0,10.0,1.0, 10.0,10.0]) + R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) + + # Set up track parameters + # l = Localization() + # l.create_track() + #l.prepare_trajectory(0.06) + + d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_f_lp = 0.0 + a_lp = 0.0 + + t_now = 0.0 + + # Estimation variables + (x_est, y_est, a_x_est, a_y_est, v_est_2) = [0]*5 + bta = 0.0 + v_est = 0.0 + psi_est = 0.0 + + est_counter = 0 + acc_f = 0.0 + vel_meas_est = 0.0 + + msg = pos_info() + + while not rospy.is_shutdown(): + t_now = rospy.get_rostime().to_sec()-se.t0 + + se.x_meas = polyval(se.c_X, t_now) + se.y_meas = polyval(se.c_Y, t_now) + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering + a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + #u = [a_lp, d_f_hist.pop(0)] + u = [se.cmd_motor, d_f_hist.pop(0)] + # u = [se.cmd_motor, se.cmd_servo] + + bta = 0.5 * u[1] + + # print "V, V_x and V_y : (%f, %f, %f)" % (se.vel_meas,cos(bta)*se.vel_meas, sin(bta)*se.vel_meas) + + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) + + + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, 0) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) + # Read values + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, + x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + + se.x_est = x_est_2 + se.y_est = y_est_2 + #print "V_x and V_y : (%f, %f)" % (v_x_est, v_y_est) + + """ + # Update track position + l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + + + # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) + if est_counter%4 == 0: + l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + state_pub_pos.publish(pos_info(Header(stamp=ros_t), l.s, l.ey, l.epsi, v_est_2, l.s_start, l.x, l.y, l.v_x, l.v_y, + l.psi, l.psiDot, se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, se.psiDot_meas, + psi_drift_est, a_x_est, a_y_est, se.a_x_meas, se.a_y_meas, se.cmd_motor, se.cmd_servo, + (0,), (0,), (0,), l.coeffCurvature.tolist())) + """ + + + ros_t = rospy.get_rostime() + msg.x = x_est + msg.y = y_est + msg.v_x = v_x_est + msg.v_y = v_y_est + msg.psi = psi_est + msg.psiDot = psi_dot_est + msg.u_a = se.cmd_motor + msg.u_df = se.cmd_servo + + state_pub_pos.publish(msg) + + # wait + est_counter += 1 + rate.sleep() + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/estimator_messy_lukas.py b/workspace/src/barc/src/estimator_messy_lukas.py new file mode 100755 index 00000000..7ae5ed69 --- /dev/null +++ b/workspace/src/barc/src/estimator_messy_lukas.py @@ -0,0 +1,695 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +# from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos, hedge_imu_fusion, hedge_imu_raw, hedge_pos_ang +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math +import numpy as np +import os + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0.0 + cmd_motor = 0.0 + cmd_t = 0.0 + + # IMU + yaw_prev = 0.0 + yaw0 = 0.0 # yaw at t = 0 + yaw_meas = 0.0 + psiDot_meas = 0.0 + a_x_meas = 0.0 + a_y_meas = 0.0 + imu_updated = False + att = (0.0,0.0,0.0) # attitude + # history data storage for debugging + roll_raw_his = [0.0] + pitch_raw_his = [0.0] + yaw_raw_his = [0.0] + yaw_his = [0.0] # yaw after unwrap + yaw0_his = [0.0] + yaw_meas_his = [0.0] + psidot_raw_his = [0.0] + a_x_raw_his = [0.0] + a_y_raw_his = [0.0] + a_x_meas_his = [0.0] + a_y_meas_his = [0.0] + imu_time = [0.0] + imu_prev_time = 0.0 + + # Velocity + vel_meas = 0.0 + vel_updated = False + vel_prev = 0.0 + vel_count = 0.0 # this counts how often the same vel measurement has been received + + # GPS + x_meas = 0.0 + y_meas = 0.0 + x2_meas = 0.0 + y2_meas = 0.0 + gps_updated = False + x_hist = zeros(15) + y_hist = zeros(15) + t_gps = zeros(15) + c_X = array([0,0,0]) + c_Y = array([0,0,0]) + yaw_gps_meas_his = [0.0] + yaw_gps_meas = 0.0 + # GPS IMU FUSION + gps_x_his = [0.0] + gps_y_his = [0.0] + gps_time = [0.0] + + qx_his = [0.0] + qy_his = [0.0] + qz_his = [0.0] + qw_his = [0.0] + ax_his = [0.0] + ay_his = [0.0] + az_his = [0.0] + vx_his = [0.0] + vy_his = [0.0] + vz_his = [0.0] + gps_imu_fusion_time = [0.0] + + # GPS IMU RAW + acc_x_his = [0.0] + acc_y_his = [0.0] + acc_z_his = [0.0] + gyro_x_his = [0.0] + gyro_y_his = [0.0] + gyro_z_his = [0.0] + compass_x_his = [0.0] + compass_y_his = [0.0] + compass_z_his = [0.0] + gps_imu_raw_time = [0.0] + + # real_val: true data + x_true = 0.0 + y_true = 0.0 + vx_true = 0.0 + vy_true = 0.0 + yaw_true = 0.0 + psidot_true = 0.0 + yaw_true_his = [0.0] + psiDot_true_his = [0.0] + + # Estimator data + x_est = 0.0 + y_est = 0.0 + + # General variables + t0 = 0.0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # TRUE CALL BACK: only for simulator + def true_callback(self, data): + self.x_true = data.x + self.y_true = data.y + self.vx_true = data.v_x + self.vy_true = data.v_y + self.yaw_true = data.psi + self.psidot_true = data.psiDot + # FOR DEBUGGING purpose + self.yaw_true_his.append(data.psi) + self.psiDot_true_his.append(data.psiDot) + + # ultrasound gps data + def gps_callback(self, data): + """This function is called when a new GPS signal is received.""" + # units: [rad] and [rad/s] + # get current time stamp + t_now = rospy.get_rostime().to_sec()-self.t0 + + #t_msg = data.timestamp_ms/1000.0 - self.t0 + + t_msg = t_now + + # if abs(t_now - t_msg) > 0.1: + # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + + # get current gps measurement + # self.x_meas = (data.x_m + self.x2_meas)/2 + # self.y_meas = (data.y_m + self.y2_meas)/2 + self.x_meas = data.x_m + self.y_meas = data.y_m + #print "Received coordinates : (%f, %f)" % (self.x_meas, self.y_meas) + + # check if we have good measurement + # compute distance we have travelled from previous estimate to current measurement + # if we've travelled more than 1 m, the GPS measurement is probably junk, so ignore it + # otherwise, store measurement, and then perform interpolation + dist = (self.x_est-data.x_m)**2 + (self.y_est-data.y_m)**2 + if dist < 1.0: + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_msg) + + # self.x_hist = delete(self.x_hist,0) + # self.y_hist = delete(self.y_hist,0) + # self.t_gps = delete(self.t_gps,0) + + # Keep only the last second worth of coordinate data in the x_hist and y_hist buffer + # These buffers are used for interpolation + # without overwriting old data, the arrays would grow unbounded + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + sz = size(self.t_gps, 0) + + # perform interpolation for (x,y) as a function of time t + # getting two function approximations x(t) and y(t) + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # use least squares to get the coefficients for this function approximation + # using (x,y) coordinate data from the past second (time) + if sz > 4: + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T # input matrix: [ t^2 t 1 ] + self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] + self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] + self.gps_updated = True + + self.gps_x_his.append(data.x_m) + self.gps_y_his.append(data.y_m) + # self.gps_time.append(rospy.get_rostime().to_sec()-self.t0) + + # def gps2_callback(self, data): + # self.x2_meas = data.x_m + # self.y2_meas = data.y_m + + def gps_ang_callback(self, data): + yaw = math.radians(data.angle+90) + if not self.running: + self.yaw_prev = 0 + else: + self.yaw_gps_meas = np.unwrap([self.yaw_prev,yaw])[1] + self.yaw_prev = self.yaw_gps_meas + self.gps_time.append(rospy.get_rostime().to_sec()-self.t0) + self.yaw_gps_meas_his.append(self.yaw_gps_meas) + + + def gps_imu_fusion_callback(self,data): + # GPS IMU FUSION + self.gps_imu_fusion_time.append(rospy.get_rostime().to_sec()-self.t0) + self.qx_his.append(data.qx) + self.qy_his.append(data.qy) + self.qz_his.append(data.qz) + self.qw_his.append(data.qw) + self.ax_his.append(data.ax) + self.ay_his.append(data.ay) + self.az_his.append(data.az) + self.vx_his.append(data.vx) + self.vy_his.append(data.vy) + self.vz_his.append(data.vz) + + def gps_imu_raw_callback(self,data): + # GPS IMU RAW + self.gps_imu_raw_time.append(rospy.get_rostime().to_sec()-self.t0) + self.acc_x_his.append(data.acc_x) + self.acc_y_his.append(data.acc_y) + self.acc_z_his.append(data.acc_z) + self.gyro_x_his.append(data.gyro_x) + self.gyro_y_his.append(data.gyro_y) + self.gyro_z_his.append(data.gyro_z) + self.compass_x_his.append(data.compass_x) + self.compass_y_his.append(data.compass_y) + self.compass_z_his.append(data.compass_z) + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + + # yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + # self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # if not self.running: + # self.yaw0 = yaw # set yaw0 to current yaw + # self.yaw_meas = 0 # and current yaw to zero + # else: + # self.yaw_meas = yaw - self.yaw0 + + # self.yaw_meas = yaw - self.yaw0 + + if self.imu_prev_time > 0: + self.yaw_meas += self.psiDot_meas * (current_t-self.imu_prev_time) + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.att = (roll_raw,pitch_raw,yaw_raw) + self.imu_updated = True + + # FOR DEBUGGING PURPOSE + self.imu_time.append(current_t-self.t0) + self.psidot_raw_his.append(w_z) + self.a_x_raw_his.append(a_x) + self.a_y_raw_his.append(a_y) + self.a_x_meas_his.append(self.a_x_meas) + self.a_y_meas_his.append(self.a_y_meas) + self.roll_raw_his.append(roll_raw) + self.pitch_raw_his.append(pitch_raw) + self.yaw_raw_his.append(yaw_raw) + # self.yaw_his.append(yaw) # yaw after unwrap() + # self.yaw0_his.append(self.yaw0) # yaw after unwrap() + self.yaw_meas_his.append(self.yaw_meas) + + self.imu_prev_time = current_t + + + def encoder_vel_callback(self, data): + # if data.vel_est != self.vel_prev: + # self.vel_meas = data.vel_est + # self.vel_updated = True + # self.vel_prev = data.vel_est + # self.vel_count = 0 + # else: + # self.vel_count = self.vel_count + 1 + # if self.vel_count > 10: # if 10 times in a row the same measurement + # self.vel_meas = 0 # set velocity measurement to zero + # self.vel_updated = True + # self.vel_meas = data.vel_est + # self.vel_updated = True + + # Average both rear speed + v_est = (data.vel_bl + data.vel_br)/2 + if v_est != self.vel_prev: + self.vel_meas = v_est + self.vel_updated = True + self.vel_prev = v_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + + + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback, queue_size=1) + rospy.Subscriber('vel_est', Vel_est, se.encoder_vel_callback, queue_size=1) + rospy.Subscriber('ecu', ECU, se.ecu_callback, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # Simulations and Experiment will subscribe to different GPS topis + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, se.gps_callback, queue_size=1) + # rospy.Subscriber('hedge_imu_fusion_2', hedge_imu_fusion, se.gps2_callback, queue_size=1) + rospy.Subscriber('hedge_pos_ang', hedge_pos_ang, se.gps_ang_callback, queue_size=1) + + + # rospy.Subscriber('real_val', pos_info, se.true_callback, queue_size=1) + # rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1) + + # for debugging to collect the data + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, se.gps_imu_fusion_callback, queue_size=1) + rospy.Subscriber('hedge_imu_raw', hedge_imu_raw, se.gps_imu_raw_callback, queue_size=1) + + + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() # set initial time + + # z_EKF = zeros(14) # x, y, psi, v, psi_drift + # P = eye(14) # initial dynamics coveriance matrix + z_EKF = zeros(9) # x, y, psi, v, psi_drift + P = eye(9) # initial dynamics coveriance matrix + + qa = 1000.0 + qp = 1000.0 + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v + #Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) + + # experiemnt + # drift_1 + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 0.001, 0.2,0.2,1.0,1.0,0.1]) + # R = 0.1*diag([100.0,100.0,1.0,1.0,1.0,100.0,100.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + # # without yaw_meas + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 1, 0.2,0.2,1.0,1.0,0.1]) + # R = 0.1*diag([5.0,5.0,1.0, 1.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + + # experiemnt: single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa, 1/3*dt**3*qp, dt*qp, 0.001]) + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 1.0, 100.0, 100.0, 10.0]) + + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa, 1/3*dt**3*qp, dt*qp]) + # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 100.0, 100.0, 10.0]) + + """ + Q = diag([0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0]) + # R = diag([10.0, 10.0, 0.1, 0.1, 10.0, 10.0, 0.01]) + R = diag([0.5 * 10.0, 0.5 * 10.0, 0.1, 0.1, 10.0, 10.0, 0.01]) + """ + + Q = diag([0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 10. * 1.0]) + R = diag([0.1 * 0.1 * 10.0, 0.1 * 0.1 * 10.0, 0.1 * 0.1, 1.0 * 0.1, 0.1, 10.0, 10.0, 0.01]) + + + + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 1.0, 100.0, 100.0, 10.0, 10.0]) + + # experiemnt: single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = diag([1e-2, 1e-2, 1e-2, 1e-2, 10, 10, 1e-3, 10, 1e-3 ])**2 + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = diag([1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1.0, 1.0, 0.01])**2 + + # # simulation + # Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 0.1, 0.2,0.2,1.0,1.0,0.1]) + # R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + # simulation single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp, dt*qp, 0.1]) + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = diag([1.0, 1.0, 1.0, 10.0, 1.0, 10.0, 10.0]) + + + + # R = diag([4*5.0,4*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 4*5.0,4*5.0,10.0,1.0, 10.0,10.0]) + # R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) + # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v + # R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,50.0,1.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) + + # Set up track parameters + # l = Localization() + # l.create_track() + # l.create_feature_track() + # l.create_race_track() + + d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_a_hist = [0.0]*5 + d_f_lp = 0.0 + a_lp = 0.0 + + t_now = 0.0 + + # Estimation variables + (x_est, y_est, a_x_est, a_y_est, v_est_2) = [0]*5 + bta = 0.0 + v_est = 0.0 + psi_est = 0.0 + # psi_est=3.1415926 + # z_EKF[11]=psi_est + + est_counter = 0 + acc_f = 0.0 + vel_meas_est = 0.0 + + psi_drift_est_his = [0.0] + psi_drift_est_2_his = [0.0] + psi_est_his = [0.0] + psi_est_2_his = [0.0] + vx_est_his = [0.0] + vy_est_his = [0.0] + ax_est_his = [0.0] + ay_est_his = [0.0] + psi_dot_est_his = [0.0] + v2_est_his = [0.0] + vel_meas_his = [0.0] + a_his = [0.0] + df_his = [0.0] + a_lp_his = [0.0] + df_lp_his = [0.0] + estimator_time = [0.0] + x_est_his = [0.0] + y_est_his = [0.0] + x_est_2_his = [0.0] + y_est_2_his = [0.0] + + msg = pos_info() + + while not rospy.is_shutdown(): + t_now = rospy.get_rostime().to_sec()-se.t0 + + se.x_meas = polyval(se.c_X, t_now) + se.y_meas = polyval(se.c_Y, t_now) + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + # d_a_hist.append(se.cmd_motor) # this is for a 0.2 seconds delay of steering + + # d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering + # a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + # u = [a_lp, d_f_hist.pop(0)] + + + d_f = d_f_hist.pop(0) + # d_f_lp = d_f_lp + 0.3*(d_f-d_f_lp) # low pass filter on steering + # # a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + # u = [se.cmd_motor, d_f_lp] + + u = [se.cmd_motor, d_f] # this is with 0.2s delay for steering without low pass filter + # u = [se.cmd_motor, se.cmd_servo] + # u = [d_a_hist.pop(0), d_f_hist.pop(0)] # this is with 0.2s delay for steering without low pass filter + + # u = [se.cmd_motor, se.cmd_servo] + + # u = [ 0, 0] + + bta = 0.5 * u[1] + + # print "V, V_x and V_y : (%f, %f, %f)" % (se.vel_meas,cos(bta)*se.vel_meas, sin(bta)*se.vel_meas) + + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) + + # measurement without yaw_meas from imu + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.vel_meas, sin(bta)*se.vel_meas]) + + # y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + # y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas]) + # y = array([se.x_meas, se.y_meas, se.vel_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + + + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, 0) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) + # Read values + # (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, + # x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + + # psi_drift_est = 0 + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est) = z_EKF # note, r = EKF estimate yaw rate + + # dummy + x_est_2=0 + y_est_2=0 + psi_est_2=0 + v_est_2=0 + psi_drift_est_2=0 + + + # sections to save the data and for debugging + estimator_time.append(t_now) + # psi_drift_est_his.append(psi_drift_est) + psi_drift_est_2_his.append(psi_drift_est_2) + psi_est_his.append(psi_est) + psi_est_2_his.append(psi_est_2) + vx_est_his.append(v_x_est) + vy_est_his.append(v_y_est) + ax_est_his.append(a_x_est) + ay_est_his.append(a_y_est) + psi_dot_est_his.append(psi_dot_est) + v2_est_his.append(v_est_2) + vel_meas_his.append(se.vel_meas) # measurment data from encoder + a_his.append(u[0]) + df_his.append(u[1]) + a_lp_his.append(se.cmd_motor) + df_lp_his.append(d_f) + + x_est_his.append(x_est) + y_est_his.append(y_est) + x_est_2_his.append(x_est_2) + y_est_2_his.append(y_est_2) + + + se.x_est = x_est + se.y_est = y_est + #print "V_x and V_y : (%f, %f)" % (v_x_est, v_y_est) + + # Update track position + # l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + # l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) + # l.set_pos(x_est, y_est, se.yaw_meas, se.vel_meas, v_y_est, psi_dot_est) + + + # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) + # if est_counter%4 == 0: + # l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + msg.x = x_est + msg.y = y_est + msg.v_x = v_x_est + msg.v_y = v_y_est + msg.psi = psi_est + msg.psiDot = psi_dot_est + msg.u_a = se.cmd_motor + msg.u_df = se.cmd_servo + + state_pub_pos.publish(msg) + + # wait + est_counter += 1 + rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_true_his = se.yaw_true_his, + psiDot_true_his = se.psiDot_true_his, + x_est_his = x_est_his, + y_est_his = y_est_his, + x_est_2_his = x_est_2_his, + y_est_2_his = y_est_2_his, + psi_drift_est_his = psi_drift_est_his, + psi_drift_est_2_his = psi_drift_est_2_his, + psi_est_his = psi_est_his, + psi_est_2_his = psi_est_2_his, + vx_est_his = vx_est_his, + vy_est_his = vy_est_his, + ax_est_his = ax_est_his, + ay_est_his = ay_est_his, + psi_dot_est_his = psi_dot_est_his, + v2_est_his = v2_est_his, + vel_meas_his = vel_meas_his, + a_his = a_his, + df_his = df_his, + a_lp_his = a_lp_his, + df_lp_his = df_lp_his, + estimator_time = estimator_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psidot_raw_his = se.psidot_raw_his, + a_x_raw_his = se.a_x_raw_his, + a_y_raw_his = se.a_y_raw_his, + a_x_meas_his = se.a_x_meas_his, + a_y_meas_his = se.a_y_meas_his, + roll_raw_his = se.roll_raw_his, + pitch_raw_his = se.pitch_raw_his, + yaw_raw_his = se.yaw_raw_his, + yaw_his = se.yaw_his, + yaw0_his = se.yaw0_his, + yaw_meas_his = se.yaw_meas_his, + imu_time = se.imu_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_hedge_imu_fusion.npz") + np.savez(pathSave,gps_x_his = se.gps_x_his, + gps_y_his = se.gps_y_his, + qx_his = se.qx_his, + qy_his = se.qy_his, + qz_his = se.qz_his, + qw_his = se.qw_his, + ax_his = se.ax_his, + ay_his = se.ay_his, + az_his = se.az_his, + vx_his = se.vx_his, + vy_his = se.vy_his, + vz_his = se.vz_his, + gps_time = se.gps_time, + yaw_gps_meas_his = se.yaw_gps_meas_his, + gps_imu_fusion_time = se.gps_imu_fusion_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_hedge_imu_raw.npz") + np.savez(pathSave,acc_x_his = se.acc_x_his, + acc_y_his = se.acc_y_his, + acc_z_his = se.acc_z_his, + gyro_x_his = se.gyro_x_his, + gyro_y_his = se.gyro_y_his, + gyro_z_his = se.gyro_z_his, + compass_x_his = se.compass_x_his, + compass_y_his = se.compass_y_his, + compass_z_his = se.compass_z_his, + gps_imu_raw_time = se.gps_imu_raw_time) + + print "finishing saveing data" + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/estimator_multiple_beacons.py b/workspace/src/barc/src/estimator_multiple_beacons.py new file mode 100755 index 00000000..660198af --- /dev/null +++ b/workspace/src/barc/src/estimator_multiple_beacons.py @@ -0,0 +1,753 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton + +# This estimator was developed by Shuqi Xu (shuqixu@berkeley.edu) +# --------------------------------------------------------------------------- + +import rospy +# from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos_ang +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import os + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = 0.0 + df_delay = 0.0 + loop_rate = 50.0 + + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0., 1.]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.2 * 10., 0.2 * 10., 0.1, 10., 1., 0.1, 10 * 0.01]) + + """ + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0., 1.]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.1 * 10., 0.1 * 10., 0.1, 10., 10., 0.1, 100.0 * 0.01]) + """ + """ + + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0.1, 1.]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.5 * 10., 0.5 * 10., 0.1, 10., 10., 0.1, 0.01]) + """ + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + # track = Track(0.01,0.8) + # track.createRaceTrack() + estMsg = pos_info() + + while not rospy.is_shutdown(): + """ + if ecu.a != 0: + est.estimateState(imu,gps,enc,ecu,est.ukf) + else: + est.x_est = 0.1 + """ + + est.estimateState(imu,gps,enc,ecu,est.ekf) + + + # estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + + imu.saveHistory() + gps.saveHistory() + enc.saveHistory() + ecu.saveHistory() + + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + estimator_time = est.time_his, + x_his_sync = gps.x_his_sync, + y_his_sync = gps.y_his_sync, + angle_his_sync = gps.angle_his_sync, + v_fl_his_sync = enc.v_fl_his_sync, + v_fr_his_sync = enc.v_fr_his_sync, + v_rl_his_sync = enc.v_rl_his_sync, + v_rr_his_sync = enc.v_rr_his_sync, + v_meas_his_sync = enc.v_meas_his_sync, + ax_his_sync = imu.ax_his_sync, + ay_his_sync = imu.ay_his_sync, + psiDot_his_sync = imu.psiDot_his_sync, + a_his_sync = ecu.a_his_sync, + df_his_sync = ecu.df_his_sync) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + angle_his = gps.angle_his, + gps_time = gps.time_his, + x_his_1 = gps.x_his_1, + y_his_1 = gps.y_his_1, + x_his_2 = gps.x_his_2, + y_his_2 = gps.y_his_2, + gps_time_1 = gps.time_his_1, + gps_time_2 = gps.time_his_2) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = 0.0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + # u = [self.a_his.pop(0), self.df_his.pop(0)] + u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, sin(bta)*enc.v_meas]) + + gps.x_his_sync = np.append(gps.x_his_sync, y[0]) + gps.y_his_sync = np.append(gps.y_his_sync, y[1]) + gps.angle_his_sync = np.append(gps.angle_his_sync, gps.angle) + enc.v_fl_his_sync.append(enc.v_fl) + enc.v_fr_his_sync.append(enc.v_fr) + enc.v_rl_his_sync.append(enc.v_rl) + enc.v_rr_his_sync.append(enc.v_fr) + enc.v_meas_his_sync.append(y[2]) + imu.ax_his_sync.append(y[3]) + imu.ay_his_sync.append(y[4]) + imu.psiDot_his_sync.append(y[5]) + ecu.a_his_sync.append(u[0]) + ecu.df_his_sync.append(u[1]) + + # y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + + KF(y,u) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est, _ ) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + # zNext[8] = z[8] # psidot_drift + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + + + + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [0.0] + self.psiDot_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.roll_his = [0.0] + self.pitch_his = [0.0] + + # Imu measurement history synchronized to estimator + self.psiDot_his_sync = [0.0] + self.ax_his_sync = [0.0] + self.ay_his_sync = [0.0] + + # time stamp + self.t0 = t0 + self.time_his = [0.0] + + # Time for yawDot integration + self.curr_time = 0.0 + self.prev_time = 0.0 + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.prev_time = self.curr_time + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('hedge_pos_ang', hedge_pos_ang, self.gps_callback, queue_size=1) + # rospy.Subscriber('hedge_imu_fusion_2', hedge_imu_fusion, self.gps_callback_2, queue_size=1) + + # GPS measurement + self.x_1 = 0.0 + self.y_1 = 0.0 + self.x_2 = 0.0 + self.y_2 = 0.0 + + self.x = 0.0 + self.y = 0.0 + self.angle = 0.0 + + self.curr_time = 0.0 + self.time_his = np.array([0.0]) + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + self.angle_his = np.array([0.0]) + + self.x_his_sync = np.array([0.0]) + self.y_his_sync_ = np.array([0.0]) + self.angle_his_sync = np.array([0.0]) + + # GPS measurement history + self.x_his_1 = np.array([0.0]) + self.y_his_1 = np.array([0.0]) + + self.x_his_2 = np.array([0.0]) + self.y_his_2 = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his_1 = np.array([0.0]) + self.curr_time_1 = 0.0 + self.time_his_2 = np.array([0.0]) + self.curr_time_2 = 0.0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + + if data.address == 16: + self.curr_time_1 = rospy.get_rostime().to_sec() - self.t0 + + self.x_1 = data.x_m + self.y_1 = data.y_m + + self.angle = data.angle + + if data.address == 26: + self.curr_time_2 = rospy.get_rostime().to_sec() - self.t0 + + self.x_2 = data.x_m + self.y_2 = data.y_m + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x = 0.5 * (self.x_1 + self.x_2) + self.y = 0.5 * (self.y_1 + self.y_2) + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # n_intplt = 20 + # if size(self.x_his,0) > n_intplt: # do interpolation when there is enough points + # x_intplt = self.x_his[-n_intplt:] + # y_intplt = self.y_his[-n_intplt:] + # t_intplt = self.time_his[-n_intplt:] + # t_matrix = vstack([t_intplt**2, t_intplt, ones(sz)]).T + # self.c_X = linalg.lstsq(t_matrix, x_intplt)[0] + # self.c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + # self.x = polyval(self.c_X, self.curr_time) + # self.y = polyval(self.c_Y, self.curr_time) + + def saveHistory(self): + self.time_his_1 = np.append(self.time_his_1, self.curr_time_1) + + self.x_his_1 = np.append(self.x_his_1,self.x_1) + self.y_his_1 = np.append(self.y_his_1,self.y_1) + + self.angle_his = np.append(self.angle_his, self.angle) + + self.time_his_2 = np.append(self.time_his_2,self.curr_time_2) + + self.x_his_2 = np.append(self.x_his_2,self.x_2) + self.y_his_2 = np.append(self.y_his_2,self.y_2) + + self.time_his = np.append(self.time_his, self.curr_time) + + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.use_both_encoders = False + else: + self.use_both_encoders = True + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [0.0] + self.v_fr_his = [0.0] + self.v_rl_his = [0.0] + self.v_rr_his = [0.0] + self.v_meas_his = [0.0] + + # ENC measurement history synchronized to estimator + self.v_fl_his_sync = [0.0] + self.v_fr_his_sync = [0.0] + self.v_rl_his_sync = [0.0] + self.v_rr_his_sync = [0.0] + self.v_meas_his_sync = [0.0] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [0.0] + self.curr_time = 0.0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + + if self.use_both_encoders: + v_est = (self.v_rl + self.v_rr)/2 + else: + v_est = self.v_rr + + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # ECU measurement history synchronized to estimator + self.a_his_sync = [] + self.df_his_sync = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = 0.0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/estimator_ugo.py b/workspace/src/barc/src/estimator_ugo.py new file mode 100755 index 00000000..5805bd97 --- /dev/null +++ b/workspace/src/barc/src/estimator_ugo.py @@ -0,0 +1,1324 @@ +#!/usr/bin/env python +""" + File name: state_estimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- +import sys +sys.path.append(sys.path[0]+'/ControllersObject') +sys.path.append(sys.path[0]+'/Utilities') +import rospy +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import os +import pdb + +def main(): + # node initialization + rospy.init_node("state_estimation") + + node_name = rospy.get_name() + a_delay = rospy.get_param(node_name + "/delay_a") + df_delay = rospy.get_param(node_name + "/delay_df") + loop_rate = 50.0 + + # low velocity + Q = eye(8) + Q[0,0] = 0.01 # x + Q[1,1] = 0.01 # y + Q[2,2] = 0.5 # vx + Q[3,3] = 0.5 # vy + Q[4,4] = 5.0 # ax + Q[5,5] = 1.0 # ay + Q[6,6] = 0.01 # psi + Q[7,7] = 10.0 # psiDot + R = eye(7) + R[0,0] = 0.5 # x + R[1,1] = 0.5 # y + R[2,2] = 0.1 # vx + R[3,3] = 0.01 # ax + R[4,4] = 10.0 # ay + R[5,5] = 20.0 # psiDot + R[6,6] = 0.001 # vy + thReset = 1.4 + + Q_1 = eye(8) + Q_1[0,0] = 0.01 # x + Q_1[1,1] = 0.01 # y + Q_1[2,2] = 0.5 # vx + Q_1[3,3] = 0.5 # vy + Q_1[4,4] = 1.0 # ax + Q_1[5,5] = 1.0 # ay + Q_1[6,6] = 0.01 # psi + Q_1[7,7] = 10.0 # psiDot + R_1 = eye(7) + R_1[0,0] = 5.0 # x + R_1[1,1] = 5.0 # y + R_1[2,2] = 1.0 # vx + R_1[3,3] = 0.0001 # ax + R_1[4,4] = 10.0 # ay + R_1[5,5] = 20.0 # psiDot + R_1[6,6] = 0.001 # vy + thReset_1 = 0.4 + + # high velocity + Q_noVy = eye(8) + Q_noVy[0,0] = 0.01 # x + Q_noVy[1,1] = 0.01 # y + Q_noVy[2,2] = 0.01 # vx + Q_noVy[3,3] = 0.01 # vy + Q_noVy[4,4] = 1.0 # ax + Q_noVy[5,5] = 1.0 # ay + Q_noVy[6,6] = 10.0 # psi + Q_noVy[7,7] = 10.0 # psidot + # Q[8,8] = 0.0 # psiDot in the model + R_noVy = eye(6) + R_noVy[0,0] = 5.0 # x + R_noVy[1,1] = 5.0 # y + R_noVy[2,2] = 0.1 # vx + R_noVy[3,3] = 10.0 # ax + R_noVy[4,4] = 30.0 # ay + R_noVy[5,5] = 0.1 # psiDot + thReset_noVy = 0.8 + + node_name = rospy.get_name() + if node_name[-1] == "2": + # Agent 2 Tuning + Q_dyn = eye(8) + Q_dyn[0,0] = 0.01 # x + Q_dyn[1,1] = 0.01 # y + Q_dyn[2,2] = 0.01 # vx + Q_dyn[3,3] = 0.1 # pretty good # 0.01 # vy + Q_dyn[4,4] = 1.0 * 1.0 # ax + Q_dyn[5,5] = 1.0 * 1.0 # ay + Q_dyn[6,6] = 0.0001 # 0.05 # 0.01 # 0.1 * 1.0 # 0.01 # not bad, but drifting# 20.0 # psi + Q_dyn[7,7] = 0.1 * 0.1 * 1.0 # psiDot + R_dyn = eye(6) + R_dyn[0,0] = 7.0 # 0.2 * 5.0 * 10.0 # x + R_dyn[1,1] = 7.0 # 0.2 * 5.0 * 10.0 # y + R_dyn[2,2] = 1.0 * 0.1 # vx + R_dyn[3,3] = 0.5 * 10.0 # ax + R_dyn[4,4] = 1.0 * 10.0 # ay + R_dyn[5,5] = 0.1 # psiDot + thReset = 10000.0 + else: + # Agent 1 Tuning + Q_dyn = eye(8) + Q_dyn[0,0] = 0.01 # x + Q_dyn[1,1] = 0.01 # y + Q_dyn[2,2] = 0.01 # vx + Q_dyn[3,3] = 0.1 # pretty good # 0.01 # vy + Q_dyn[4,4] = 1.0 * 1.0 # ax + Q_dyn[5,5] = 1.0 * 1.0 # ay + Q_dyn[6,6] = 0.0005 # 0.05 # 0.01 # 0.1 * 1.0 # 0.01 # not bad, but drifting# 20.0 # psi + Q_dyn[7,7] = 0.1 * 0.1 * 1.0 # psiDot + R_dyn = eye(6) + R_dyn[0,0] = 7.0 # 0.2 * 5.0 * 10.0 # x + R_dyn[1,1] = 7.0 # 0.2 * 5.0 * 10.0 # y + R_dyn[2,2] = 1.0 * 0.1 # vx + R_dyn[3,3] = 0.5 * 10.0 # ax + R_dyn[4,4] = 1.0 * 10.0 # ay + R_dyn[5,5] = 0.1 # psiDot + thReset = 10000.0 + + + # Q_dyn = eye(8) + # Q_dyn[0,0] = 0.01 # x + # Q_dyn[1,1] = 0.01 # y + # Q_dyn[2,2] = 0.01 # vx + # Q_dyn[3,3] = 0.01 # 0.1 # pretty good # 0.01 # vy + # Q_dyn[4,4] = 1.0 * 1.0 # ax + # Q_dyn[5,5] = 1.0 * 1.0 # ay + # Q_dyn[6,6] = 0.1 * 1.0 # 0.01 # not bad, but drifting# 20.0 # psi + # Q_dyn[7,7] = 0.1 * 0.1 * 1.0 # psiDot + # R_dyn = eye(6) + # R_dyn[0,0] = 0.2 * 5.0 * 10.0 # x + # R_dyn[1,1] = 0.2 * 5.0 * 10.0 # y + # R_dyn[2,2] = 1.0 * 0.1 # vx + # R_dyn[3,3] = 0.5 * 10.0 # ax + # R_dyn[4,4] = 1.0 * 10.0 # ay + # R_dyn[5,5] = 0.1 # psiDot + # thReset = 10000.0 + + twoEstimators = False + switchEstimators = False + + + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + + est = Estimator(t0,loop_rate,a_delay,df_delay,Q, R, thReset) + est_1 = Estimator(t0,loop_rate,a_delay,df_delay,Q_1,R_1, thReset_1) + estNoVy = EstimatorNoVy(t0,loop_rate,a_delay,df_delay,Q_noVy,R_noVy, thReset_noVy) + + est_dyn = Estimator(t0,loop_rate,a_delay,df_delay,Q_dyn, R_dyn, thReset, dynamic_model=True) + + estMsg = pos_info() + + saved_x_est = [] + saved_y_est = [] + saved_vx_est = [] + saved_vy_est = [] + saved_psi_est = [] + saved_psiDot_est = [] + saved_ax_est = [] + saved_ay_est = [] + + while not rospy.is_shutdown(): + + est.estimateState(imu,gps,enc,ecu,est.ekf) + if est.vx_est > 0.5: + est_dyn.estimateState(imu, gps, enc, ecu, est_dyn.ekf) + + if twoEstimators == True: + est_1.estimateState(imu,gps,enc,ecu,est_1.ekf) + + if switchEstimators == True: + estNoVy.estimateState(imu, gps, enc, ecu, estNoVy.ekf) + + estMsg.header.stamp = rospy.get_rostime() + + if est.vx_est > 0.5: + est.z = est_dyn.z + print "DYNAMIC ESTIMATOR" + estMsg.v = np.sqrt(est_dyn.vx_est**2 + est_dyn.vy_est**2) + estMsg.x = est_dyn.x_est + estMsg.y = est_dyn.y_est + estMsg.v_x = est_dyn.vx_est + estMsg.v_y = est_dyn.vy_est + estMsg.psi = est_dyn.yaw_est + estMsg.psiDot = est_dyn.psiDot_est + estMsg.a_x = est_dyn.ax_est + estMsg.a_y = est_dyn.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + else: + est_dyn.z = est.z + # print "KINEMATIC ESTIMATOR" + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + """ + if switchEstimators == True: + if (est.vx_est + 0.0 * est.psiDot_est) > 1.3 or (np.abs(est.psiDot_est) > 2.0): + print "HIGH VELOCITY ESTIMATOR KICKED IN" + estMsg.v = np.sqrt(estNoVy.vx_est**2 + estNoVy.vy_est**2) + estMsg.x = estNoVy.x_est + estMsg.y = estNoVy.y_est + estMsg.v_x = estNoVy.vx_est + estMsg.v_y = estNoVy.vy_est + estMsg.psi = estNoVy.yaw_est + estMsg.psiDot = estNoVy.psiDot_est + estMsg.a_x = estNoVy.ax_est + estMsg.a_y = estNoVy.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + else: + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + elif twoEstimators == True: + estMsg.v = np.sqrt(est_1.vx_est**2 + est_1.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est_1.vx_est + estMsg.v_y = est_1.vy_est + estMsg.psi = est_1.yaw_est + estMsg.psiDot = est_1.psiDot_est + estMsg.a_x = est_1.ax_est + estMsg.a_y = est_1.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + else: + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + """ + + est.state_pub_pos.publish(estMsg) + + # Save estimator output. + # NEED TO DO IT HERE AS THERE ARE MULTIPLE ESTIMATOR RUNNING IN PARALLEL + saved_x_est.append(estMsg.x) + saved_y_est.append(estMsg.y) + saved_vx_est.append(estMsg.v_x) + saved_vy_est.append(estMsg.v_y) + saved_psi_est.append(estMsg.psi) + saved_psiDot_est.append(estMsg.psiDot) + saved_ax_est.append(estMsg.a_x) + saved_ay_est.append(estMsg.a_y) + + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_data/estimator_output.npz") + np.savez(pathSave,yaw_est_his = saved_psi_est, + psiDot_est_his = saved_psiDot_est, + x_est_his = saved_x_est, + y_est_his = saved_y_est, + vx_est_his = saved_vx_est, + vy_est_his = saved_vy_est, + ax_est_his = saved_ax_est, + ay_est_his = saved_ay_est, + gps_time = est.gps_time, + imu_time = est.imu_time, + enc_time = est.enc_time, + inp_x_his = est.x_his, + inp_y_his = est.y_his, + inp_v_meas_his = est.v_meas_his, + inp_ax_his = est.ax_his, + inp_ay_his = est.ay_his, + inp_psiDot_his = est.psiDot_his, + inp_a_his = est.inp_a_his, + inp_df_his = est.inp_df_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + gps_time = gps.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R,thReset,dynamic_model=False): + """ Initialization + Arguments: + t0: starting measurement time + """ + self.dynamic_model = dynamic_model + + self.l_f = 0.125 + self.l_r = 0.125 + self.I_z = 0.24 + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.front_corner_stiffness = 13.03 + self.rear_corner_stiffness = 10.06 + self.mass = 1.75 + else: + self.front_corner_stiffness = 12.38 + self.rear_corner_stiffness = 9.6 + self.mass = 2.0 + + self.thReset = thReset + + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + if dynamic_model: + self.z[2] = 0.1 + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() + + self.x_est_his = [] + self.y_est_his = [] + self.vx_est_his = [] + self.vy_est_his = [] + self.v_est_his = [] + self.ax_est_his = [] + self.ay_est_his = [] + self.yaw_est_his = [] + self.psiDot_est_his = [] + self.time_his = [] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [] + self.y_his = [] + self.v_meas_his = [] + self.ax_his = [] + self.ay_his = [] + self.psiDot_his = [] + self.inp_a_his = [] + self.inp_df_his = [] + + self.gps_time = [] + self.enc_time = [] + self.imu_time = [] + + self.oldGPS_x = 0.0 + self.oldGPS_y = 0.0 + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + # u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + dist = np.sqrt(( self.x_est - gps.x )**2 + ( self.y_est - gps.y )**2) + + """ + if vx_est > 0.5: + self.dynamic_model = True + else: + self.dynamic_model = False + """ + + # if ( dist >= 1 ) or ( (gps.x == self.oldGPS_x) and (gps.x == self.oldGPS_y) ): + # if ( (gps.x == self.oldGPS_x) and (gps.x == self.oldGPS_y) ): + if self.dynamic_model: + modeGPS = True + y = np.array([gps.x, # x + gps.y, # y + enc.v_meas, # v_x + imu.ax, # a_x + imu.ay, # a_y + imu.psiDot]) # psi_dot + elif 0 == 1: + modeGPS = False + y = np.array([enc.v_meas, imu.ax, imu.ay, imu.psiDot, bta * enc.v_meas]) + else: + modeGPS = True + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, bta * enc.v_meas]) + + + self.oldGPS_x = gps.x + self.oldGPS_y = gps.y + + if np.abs(imu.psiDot) < self.thReset and not self.dynamic_model: + self.z[3] = 0 + + KF(y,u, modeGPS) + + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(gps.x) + self.y_his.append(gps.y) + self.v_meas_his.append(enc.v_meas) + self.ax_his.append(imu.ax) + self.ay_his.append(imu.ay) + self.psiDot_his.append(imu.psiDot) + self.inp_a_his.append(u[0]) + self.inp_df_his.append(u[1]) + + self.gps_time.append(gps.curr_time) + self.imu_time.append(imu.curr_time) + self.enc_time.append(enc.curr_time) + # SAVE output KF given the above measurements + self.saveHistory() + + def ekf(self, y, u, modeGPS): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u, modeGPS) # linearize process model about current state + + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + + my_kp1 = self.h(mx_kp1, u, modeGPS) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u, modeGPS) # linearize measurement model about predicted next state + + P12 = dot(P_kp1, H.T) # cross covariance + + if modeGPS == True: + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + else: + K = dot(P12, inv( dot(H,P12) + self.R[2:,2:])) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + + if modeGPS == True: + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + else: + self.P = dot(dot(K,self.R[2:,2:]),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u, modeGPS): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u, modeGPS) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u, modeGPS) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u, modeGPS) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u, modeGPS=True): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + + if self.dynamic_model: + x, y, v_x, v_y, a_x, a_y, psi, psi_dot = z + c_f = self.front_corner_stiffness + c_r = self.rear_corner_stiffness + l_f = self.l_f + l_r = self.l_r + m = self.mass + I_z = self.I_z + x_next = x + dt * (cos(psi) * v_x - sin(psi) * v_y) + y_next = y + dt * (sin(psi) * v_x + cos(psi) * v_y) + vx_next = v_x + dt * (a_x + psi_dot * v_y) + vy_dot = ((- c_f - c_r) / (m * v_x)) * v_y + \ + (- v_x - (l_f * c_f - l_r * c_r) / (m * v_x)) * psi_dot + \ + (c_f / m) * u[1] + vy_next = v_y + dt * vy_dot + ax_next = a_x + ay_next = a_y + psi_next = psi + dt * psi_dot + psidot_dot = ((- l_f * c_f + l_r * c_r) / (I_z * v_x)) * v_y + \ + ((- l_f**2 * c_f - l_r**2 * c_r) / (I_z * v_x)) * psi_dot + \ + (l_f * c_f / I_z) * u[1] + psidot_next = psi_dot + dt * psidot_dot + return np.array([x_next, y_next, vx_next, vy_next, ax_next, ay_next, psi_next, psidot_next]) + else: + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u, modeGPS): + """ This is the measurement model to the kinematic<->sensor model above """ + if self.dynamic_model: + c_f = self.front_corner_stiffness + c_r = self.rear_corner_stiffness + l_f = self.l_f + l_r = self.l_r + m = self.mass + x, y, v_x, v_y, a_x, a_y, psi, psi_dot = x + x_meas = x + y_meas = y + vx_meas = v_x + ax_meas = a_x + ay_meas = ((- c_f - c_r) / (m * v_x)) * v_y + \ + ((- l_f * c_f + l_r * c_r) / (m * v_x)) * psi_dot + \ + (c_f / m) * u[1] + psidot_meas = psi_dot + return np.array([x_meas, y_meas, vx_meas, ax_meas, ay_meas, psidot_meas]) + elif modeGPS: + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + else: + y = [0]*5 + y[0] = x[2] # vx + y[1] = x[4] # a_x + y[2] = x[5] # a_y + y[3] = x[7] # psiDot + y[4] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +# ======================================================================================================================================== +# ======================================================= ESTIMATOR NO VY ================================================================ +# ======================================================================================================================================== +class EstimatorNoVy(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R,thReset): + """ Initialization + Arguments: + t0: starting measurement time + """ + self.thReset = thReset + + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() + + self.x_est_his = [] + self.y_est_his = [] + self.vx_est_his = [] + self.vy_est_his = [] + self.v_est_his = [] + self.ax_est_his = [] + self.ay_est_his = [] + self.yaw_est_his = [] + self.psiDot_est_his = [] + self.time_his = [] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [] + self.y_his = [] + self.v_meas_his = [] + self.ax_his = [] + self.ay_his = [] + self.psiDot_his = [] + self.inp_a_his = [] + self.inp_df_his = [] + + self.gps_time = [] + self.enc_time = [] + self.imu_time = [] + + self.oldGPS_x = 0.0 + self.oldGPS_y = 0.0 + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + # u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + dist = np.sqrt(( self.x_est - gps.x )**2 + ( self.y_est - gps.y )**2) + + # if ( dist >= 1 ) or ( (gps.x == self.oldGPS_x) and (gps.x == self.oldGPS_y) ): + # if ( (gps.x == self.oldGPS_x) and (gps.x == self.oldGPS_y) ): + if 0 == 1: + modeGPS = False + y = np.array([enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + else: + modeGPS = True + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + + + self.oldGPS_x = gps.x + self.oldGPS_y = gps.y + + if np.abs(imu.psiDot) < self.thReset: + self.z[3] = 0 + + KF(y,u, modeGPS) + + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(gps.x) + self.y_his.append(gps.y) + self.v_meas_his.append(enc.v_meas) + self.ax_his.append(imu.ax) + self.ay_his.append(imu.ay) + self.psiDot_his.append(imu.psiDot) + self.inp_a_his.append(u[0]) + self.inp_df_his.append(u[1]) + + self.gps_time.append(gps.curr_time) + self.imu_time.append(imu.curr_time) + self.enc_time.append(enc.curr_time) + # SAVE output KF given the above measurements + self.saveHistory() + + def ekf(self, y, u, modeGPS): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u, modeGPS) # linearize process model about current state + + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + + my_kp1 = self.h(mx_kp1, u, modeGPS) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u, modeGPS) # linearize measurement model about predicted next state + + P12 = dot(P_kp1, H.T) # cross covariance + + if modeGPS == True: + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + else: + K = dot(P12, inv( dot(H,P12) + self.R[2:,2:])) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + + if modeGPS == True: + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + else: + self.P = dot(dot(K,self.R[2:,2:]),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u, modeGPS): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u, modeGPS) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u, modeGPS) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u, modeGPS) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u, modeGPS=True): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u, modeGPS): + """ This is the measurement model to the kinematic<->sensor model above """ + if modeGPS: + y = [0]*6 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + # y[6] = x[3] # vy + else: + + y[0] = x[2] # vx + y[1] = x[4] # a_x + y[2] = x[5] # a_y + y[3] = x[7] # psiDot + # y[4] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + + +# ======================================================================================================================================== +# ======================================================= SENSOR CLASSES ================================================================= +# ======================================================================================================================================== + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() + + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.prev_time = self.curr_time + + self.saveHistory() + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('hedge_pos', hedge_pos, self.gps_callback, queue_size=1) + # rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + + # GPS measurement history + self.x_his = np.array([]) + self.y_his = np.array([]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([]) + self.curr_time = rospy.get_rostime().to_sec() + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + if 2 == data.flags: + self.curr_time = rospy.get_rostime().to_sec() + + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # n_intplt = 20 + # if size(self.x_his,0) > n_intplt: # do interpolation when there is enough points + # x_intplt = self.x_his[-n_intplt:] + # y_intplt = self.y_his[-n_intplt:] + # t_intplt = self.time_his[-n_intplt:] + # t_matrix = vstack([t_intplt**2, t_intplt, ones(sz)]).T + # self.c_X = linalg.lstsq(t_matrix, x_intplt)[0] + # self.c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + # self.x = polyval(self.c_X, self.curr_time) + # self.y = polyval(self.c_Y, self.curr_time) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.use_both_encoders = False + else: + self.use_both_encoders = True + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + + if self.use_both_encoders: + v_est = (self.v_rl + self.v_rr) / 2.0 + else: + v_est = self.v_rr + + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10 and self.v_meas < 0.5: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/estimator_ugo_2.py b/workspace/src/barc/src/estimator_ugo_2.py new file mode 100755 index 00000000..d70248a1 --- /dev/null +++ b/workspace/src/barc/src/estimator_ugo_2.py @@ -0,0 +1,779 @@ +#!/usr/bin/env python +""" + File name: state_estimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- +import sys +sys.path.append(sys.path[0]+'/ControllersObject') +sys.path.append(sys.path[0]+'/Utilities') +import rospy +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import os +import pdb + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator_2/delay_a") + df_delay = rospy.get_param("state_estimator_2/delay_df") + loop_rate = 50.0 + + + Q = eye(8) + Q[0,0] = 0.5 # x + Q[1,1] = 0.5 # y + Q[2,2] = 10.0 # vx + Q[3,3] = 10.0 # vy + Q[4,4] = 1.0 # ax + Q[5,5] = 1.0 # ay + Q[6,6] = 10 + 80.0 # psi + Q[7,7] = 2 * 50 * 10.0 # psiDot + R = eye(7) + R[0,0] = 10 + 40.0 # x + R[1,1] = 10 + 40.0 # y + R[2,2] = 0.1 # vx + R[3,3] = 30 + 10.0 # ax + R[4,4] = 40.0 # ay + R[5,5] = 5 * 5 * 2 * 10 * 0.1 # psiDot + R[6,6] = 0.01 # vy + thReset = 0.4 + vSwitch = 1.0 + psiSwitch = 0.5 * 2.0 + + Q = eye(8) + Q[0,0] = 0.5 # x + Q[1,1] = 0.5 # y + Q[2,2] = 10.0 # vx + Q[3,3] = 10.0 # vy + Q[4,4] = 1.0 # ax + Q[5,5] = 1.0 # ay + Q[6,6] = 80.0 # psi + Q[7,7] = 2 * 10.0 # psiDot + R = eye(7) + R[0,0] = 40.0 # x + R[1,1] = 40.0 # y + R[2,2] = 0.1 # vx + R[3,3] = 10.0 # ax + R[4,4] = 40.0 # ay + R[5,5] = 5 * 5 * 2 * 0.1 # psiDot + R[6,6] = 0.01 # vy + thReset = 0.4 + vSwitch = 1.0 + psiSwitch = 0.5 * 2.0 + + + # Q_noVy = eye(8) + # Q_noVy[0,0] = 0.01 # x + # Q_noVy[1,1] = 0.01 # y + # Q_noVy[2,2] = 0.01 # vx + # Q_noVy[3,3] = 0.01 # vy + # Q_noVy[4,4] = 1.0 # ax + # Q_noVy[5,5] = 1.0 # ay + # Q_noVy[6,6] = 10.0 # psi + # Q_noVy[7,7] = 10.0 # psidot + # # Q[8,8] = 0.0 # psiDot in the model + # R_noVy = eye(6) + # R_noVy[0,0] = 20.0 # x + # R_noVy[1,1] = 20.0 # y + # R_noVy[2,2] = 0.1 # vx + # R_noVy[3,3] = 10.0 # ax + # R_noVy[4,4] = 30.0 # ay + # R_noVy[5,5] = 0.10 # psiDot + # R[6,6] = 0.001 # vy + # thReset_noVy = 0.8 + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + + est = Estimator(t0,loop_rate,a_delay,df_delay,Q, R, thReset) + + estMsg = pos_info() + + saved_x_est = [] + saved_y_est = [] + saved_vx_est = [] + saved_vy_est = [] + saved_psi_est = [] + saved_psiDot_est = [] + saved_ax_est = [] + saved_ay_est = [] + saved_switch = [] + + while not rospy.is_shutdown(): + + if (est.vx_est > vSwitch or np.abs(est.psiDot_est) > psiSwitch): + flagVy = True + # print "================ Not using vy! ==============" + else: + flagVy = False + est.Q[6,6] = 10 + # print "================ Using vy! ==============" + + est.estimateState(imu,gps,enc,ecu,est.ekf,flagVy) + + estMsg.header.stamp = rospy.get_rostime() + + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + + # Save estimator output. + # NEED TO DO IT HERE AS THERE ARE MULTIPLE ESTIMATOR RUNNING IN PARALLEL + saved_x_est.append(estMsg.x) + saved_y_est.append(estMsg.y) + saved_vx_est.append(estMsg.v_x) + saved_vy_est.append(estMsg.v_y) + saved_psi_est.append(estMsg.psi) + saved_psiDot_est.append(estMsg.psiDot) + saved_ax_est.append(estMsg.a_x) + saved_ay_est.append(estMsg.a_y) + saved_switch.append(int(flagVy)) + + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_data/estimator_output.npz") + np.savez(pathSave,yaw_est_his = saved_psi_est, + psiDot_est_his = saved_psiDot_est, + x_est_his = saved_x_est, + y_est_his = saved_y_est, + vx_est_his = saved_vx_est, + vy_est_his = saved_vy_est, + ax_est_his = saved_ax_est, + ay_est_his = saved_ay_est, + gps_time = est.gps_time, + imu_time = est.imu_time, + enc_time = est.enc_time, + inp_x_his = est.x_his, + inp_y_his = est.y_his, + inp_v_meas_his = est.v_meas_his, + inp_ax_his = est.ax_his, + inp_ay_his = est.ay_his, + inp_psiDot_his = est.psiDot_his, + inp_a_his = est.inp_a_his, + inp_df_his = est.inp_df_his, + flagVy = saved_switch) + + pathSave = os.path.join(homedir,"barc_data/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + gps_time = gps.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_data/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R,thReset): + """ Initialization + Arguments: + t0: starting measurement time + """ + self.thReset = thReset + + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() + + self.x_est_his = [] + self.y_est_his = [] + self.vx_est_his = [] + self.vy_est_his = [] + self.v_est_his = [] + self.ax_est_his = [] + self.ay_est_his = [] + self.yaw_est_his = [] + self.psiDot_est_his = [] + self.time_his = [] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [] + self.y_his = [] + self.v_meas_his = [] + self.ax_his = [] + self.ay_his = [] + self.psiDot_his = [] + self.inp_a_his = [] + self.inp_df_his = [] + + self.gps_time = [] + self.enc_time = [] + self.imu_time = [] + + self.oldGPS_x = 0.0 + self.oldGPS_y = 0.0 + + # ecu command update + def estimateState(self, imu, gps, enc, ecu, KF, flagVy): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + # u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + dist = np.sqrt(( self.x_est - gps.x )**2 + ( self.y_est - gps.y )**2) + + if flagVy == False: + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, bta * enc.v_meas]) + else: + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + + if np.abs(imu.psiDot) < self.thReset: + self.z[3] = 0.0 + + KF(y,u, flagVy) + + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(gps.x) + self.y_his.append(gps.y) + self.v_meas_his.append(enc.v_meas) + self.ax_his.append(imu.ax) + self.ay_his.append(imu.ay) + self.psiDot_his.append(imu.psiDot) + self.inp_a_his.append(u[0]) + self.inp_df_his.append(u[1]) + + self.gps_time.append(gps.curr_time) + self.imu_time.append(imu.curr_time) + self.enc_time.append(enc.curr_time) + # SAVE output KF given the above measurements + self.saveHistory() + + def ekf(self, y, u, flagVy): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + numericalDiffActive = True + + xDim = self.z.size # dimension of the state + + mx_kp1, Aa = self.f(self.z, u) # predict next state + An = self.numerical_jac(self.f, self.z, u, flagVy) # linearize process model about current state + + if numericalDiffActive == True: + A = An + else: + A = Aa + + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + + my_kp1, Ha = self.h(mx_kp1, u, flagVy) # predict future output + Hn = self.numerical_jac(self.h, mx_kp1, u, flagVy) # linearize measurement model about predicted next state + + if numericalDiffActive == True: + H = Hn + else: + H = Ha + + P12 = dot(P_kp1, H.T) # cross covariance + + if flagVy == False: + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + else: + K = dot(P12, inv( dot(H,P12) + self.R[:-1,:-1])) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + + if flagVy == False: + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + else: + self.P = dot(dot(K,self.R[:-1,:-1]),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u, flagVy): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y, _ = func(x,u, flagVy) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi, _ = func(xp, u, flagVy) + xp[i] = x[i] - eps/2.0 + ylo, _ = func(xp, u, flagVy) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u, flagVy=True): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + + jac = np.array([[1.0, 0.0, dt*cos(z[6]), -dt*sin(z[6]), 0.0, 0.0, dt*(-sin(z[6])*z[2]-cos(z[6])*z[3]), 0.0], + [0.0, 1.0, dt*sin(z[6]), dt*cos(z[6]), 0.0, 0.0, dt*( cos(z[6])*z[2]-sin(z[6])*z[3]), 0.0], + [0.0, 0.0, 1.0, dt*z[7], dt, 0.0, 0.0, dt*z[3]], + [0.0, 0.0, -dt*z[7], 1.0, 0.0, dt, 0.0,-dt*z[2]], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, dt], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) + + return np.array(zNext), jac + + def h(self, x, u, flagVy): + """ This is the measurement model to the kinematic<->sensor model above """ + if flagVy == False: + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + + jac = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0,], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0,], + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0,]]) + else: + y = [0]*6 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + + jac = np.array([[1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]]) + + return np.array(y), jac + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + + +# ======================================================================================================================================== +# ======================================================= SENSOR CLASSES ================================================================= +# ======================================================================================================================================== + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() + + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.prev_time = self.curr_time + + self.saveHistory() + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + # rospy.Subscriber('hedge_pos', hedge_pos, self.gps_callback, queue_size=1) + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + + # GPS measurement history + self.x_his = np.array([]) + self.y_his = np.array([]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([]) + self.curr_time = rospy.get_rostime().to_sec() + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() + + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # n_intplt = 20 + # if size(self.x_his,0) > n_intplt: # do interpolation when there is enough points + # x_intplt = self.x_his[-n_intplt:] + # y_intplt = self.y_his[-n_intplt:] + # t_intplt = self.time_his[-n_intplt:] + # t_matrix = vstack([t_intplt**2, t_intplt, ones(sz)]).T + # self.c_X = linalg.lstsq(t_matrix, x_intplt)[0] + # self.c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + # self.x = polyval(self.c_X, self.curr_time) + # self.y = polyval(self.c_Y, self.curr_time) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + v_est = self.v_rr + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/filesystem_helpers.jl b/workspace/src/barc/src/filesystem_helpers.jl new file mode 100755 index 00000000..e43b3c0a --- /dev/null +++ b/workspace/src/barc/src/filesystem_helpers.jl @@ -0,0 +1,152 @@ +#!/usr/bin/env julia + +#= + File name: filesystem_helpers.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + + +function get_most_recent_file() + cd("/home/mpcubuntu/simulations/lukas") + + folders = [] + # Get all the folders + for folder_string in readdir() + if !contains(folder_string, ".") + folders = [folders; folder_string] + end + end + + # Turn string into number + folder_numbers = map(x->parse(Int64, x), folders) + # Find most recent folder + max_index = findmax(folder_numbers)[2] + chosen_folder = folders[max_index] + + cd(chosen_folder) + files = [] + time_index = [] + # Check all files in the folder + for file in readdir() + files = [files; file] + time_index = [time_index; parse(Int64, split(file, "_", limit=2)[1])] + end + + max_time_index = findmax(time_index)[2] + chosen_file = files[max_time_index] + + return pwd() * "/" * chosen_file +end + +function find_file(time_stamp_string) + cd("/home/mpcubuntu/simulations/lukas") + + if length(time_stamp_string) == 4 + # Take most recently recorded file from the folder indicated by the + # first four digits + cd(time_stamp_string) + files = [] + time_index = [] + # Check all files in the folder + for file in readdir() + files = [files; file] + time_index = [time_index; parse(Int64, split(file, "_", limit=2)[1])] + end + + max_time_index = findmax(time_index)[2] + chosen_file = files[max_time_index] + + return pwd() * "/" * chosen_file + elseif length(time_stamp_string) == 10 + folder = time_stamp_string[1 : 4] + cd(folder) + + file_string = time_stamp_string[5 : end] + + for file in readdir() + if contains(file, file_string) + return pwd() * "/" * file + end + end + + error("The specified string does not match any file.") + + else + error("String not matching the folder or file structure.") + end +end + + +function create_window_name(filename) + splitted_string = split(filename, "/") + last_bits = splitted_string[end - 1 : end] + + file_name = split(split(last_bits[2], ".")[1], "_") + time_stamp = file_name[1][1 : 2] * ":" * file_name[1][3 : 4] * ":" * + file_name[1][5 : 6] + + agent_str = ucfirst(file_name[2]) + index_str = file_name[3] + mode_str = uppercase(file_name[4]) + init_str = ucfirst(file_name[5]) + + month_day = last_bits[1][1 : 2] * "/" * last_bits[1][3 : 4] + + figure_string = agent_str * " " * index_str * " " * mode_str * " " * + init_str * " " * month_day * " " * time_stamp + + return figure_string +end + + +function get_index(filename) + if contains(filename, "agent_1") + return 1 + # return parse(Int64, split(filename, "_")[end - 2]) + elseif contains(filename, "agent_2") + return 2 + end +end + + +function get_most_recent(node_name, mode, initialization) + cd("/home/mpcubuntu/simulations/lukas") + + folders = [] + # Get all the folders + for folder_string in readdir() + if !contains(folder_string, ".") + folders = [folders; folder_string] + end + end + + # Turn string into number + folder_numbers = map(x->parse(Int64, x), folders) + # reverse the order, because we want the most recent one + sorted_indeces = sortperm(folder_numbers)[end : - 1 : 1] + + for index in sorted_indeces + cd(folders[index]) + files = [] + time_index = [] + # Check all files in the folder + for file in readdir() + if contains(file, mode) && contains(file, initialization) && + contains(file, node_name) + files = [files; file] + time_index = [time_index; parse(Int64, split(file, "_", limit=2)[1])] + end + end + + if size(files, 1) > 0 + max_time_index = findmax(time_index)[2] + chosen_file = files[max_time_index] + + return pwd() * "/" * chosen_file + end + + cd("..") + end +end \ No newline at end of file diff --git a/workspace/src/barc/src/filtering.py b/workspace/src/barc/src/filtering.py deleted file mode 100644 index 01ce5282..00000000 --- a/workspace/src/barc/src/filtering.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from numpy import array, dot, zeros, roll - -class filteredSignal: - def __init__(self, y0 = 0, a = 1.0, n = 200, method = None): - # initialize system - self.filter = method # filtering method - self.alpha = a # smoothing parameter - self.n = float(n) # size of (weighted) moving average filter - - self.y_t = y0 # current (filtered) signal - self.y_t_RAW = y0 # current un-filtered signal - self.y_inertia = 0 # inertia of signal history - self.y_hist = zeros(n) # signal history - self.mvsig = False # multivariate signal - - # vector for signal history - if isinstance(y0, list): - self.mvsig = True - self.y_t = array(y0) - self.y_hist = zeros((len(y0), n)) - self.y_inertia = zeros(len(y0)) - - self.wgt = array(range(n)) # moving average weights - self.wgt_sum = float(sum(self.wgt)) - - # ensure a valid filtering method - if isinstance(method,str): - method = method.lower() - self.filter = method - if method not in [None, 'lp', 'mvg', 'wmvg']: - raise ValueError('Please enter valid filtering method') - - def update(self, y_new): - self.y_t_RAW = y_new - - if self.filter == None: - self.y_t = y_new - - if self.filter == 'lp': - self.lowpass(y_new) - - if self.filter == 'mvg': - self.moving_avg(y_new) - - if self.filter == 'wmvg': - self.wgt_moving_avg(y_new) - - def lowpass(self, y_new): - """ - Low-pass filter - y_new := new measurement contribution - y_inertia := inertia from pervious sensor outputs - alpha := filtering factor, (heavy filter) 0 <= a <= 1 (no filter) - """ - if not self.mvsig: - y_new = y_new - self.y_t = self.alpha*y_new + (1-self.alpha)*self.y_inertia - self.y_inertia = self.y_t - else: - y_new = array(y_new) - self.y_t = self.alpha*y_new + (1-array(self.alpha))*self.y_inertia - self.y_inertia = self.y_t - - def moving_avg(self, y_new): - - if not self.mvsig: - self.y_hist = roll(self.y_hist, -1) - self.y_hist[-1] = y_new - self.y_t = self.y_t + (1/self.n)*(y_new - self.y_hist[0]) - else: - self.y_hist = roll(self.y_hist, -1) - self.y_hist[:,-1] = y_new - self.y_t = self.y_t + (1/self.n)*(y_new - self.y_hist[:,0]) - - def wgt_moving_avg(self, y_new): - self.y_hist = roll(self.y_hist, -1, axis = 1) - self.y_hist[-1] = y_new - self.y_t = dot(self.y_hist, self.wgt) / self.wgt_sum - - def getFilteredSignal(self): - return self.y_t - - def getRawSignal(self): - return self.y_t_RAW diff --git a/workspace/src/barc/src/gps_interpolation.py b/workspace/src/barc/src/gps_interpolation.py new file mode 100644 index 00000000..8aadf824 --- /dev/null +++ b/workspace/src/barc/src/gps_interpolation.py @@ -0,0 +1,405 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from track import Track +import pdb +import copy + +def interpolate_gps(gps_x, gps_y, gps_time, time_now): + seconds_used = 1.0 + + start = np.argmax(gps_time >= time_now - seconds_used) + end = np.argmax(gps_time >= time_now) - 1 + + t_gps = gps_time[start : end] - gps_time[start] + x_hist = gps_x[start : end] + y_hist = gps_y[start : end] + + t_matrix = np.vstack([t_gps**2, t_gps, np.ones(end - start)]).T + c_X = np.linalg.lstsq(t_matrix, x_hist)[0] + c_Y = np.linalg.lstsq(t_matrix, y_hist)[0] + + x_interpolated = np.polyval(c_X, time_now - gps_time[start]) + y_interpolated = np.polyval(c_Y, time_now - gps_time[start]) + + return x_interpolated, y_interpolated + +""" +def interpolate_yaw(yaw, gps_time, time_now): + seconds_used = 1.0 + + start = np.argmax(gps_time >= time_now - seconds_used) + end = np.argmax(gps_time >= time_now) - 1 + + print start + print end + + t_gps = gps_time[start : end] - gps_time[start] + yaw_hist = yaw[start : end] + + print yaw.shape + + t_matrix = np.vstack([t_gps**2, t_gps, np.ones(end - start)]).T + + print t_matrix.shape + print yaw_hist.shape + c_yaw = np.linalg.lstsq(t_matrix, yaw_hist)[0] + + yaw_interpolated = np.polyval(c_yaw, time_now - gps_time[start]) + + return yaw_interpolated +""" + + +def approximate_yaw(x, y, time, time_now): + index = np.argmin(abs(time - time_now)) + + x_0 = x[index - 1] + y_0 = y[index - 1] + + x_1 = x[index] + y_1 = y[index] + + argument_y = y_1 - y_0 + argument_x = x_1 - x_0 + # angle = np.arctan(argument_y / argument_x) + + angle = 0.0 + + if argument_x > 0.0: + angle = np.arctan(argument_y / argument_x) + + if argument_y >= 0.0 and argument_x < 0.0: + angle = np.pi + np.arctan(argument_y / argument_x) + + if argument_y < 0.0 and argument_x < 0.0: + angle = - np.pi + np.arctan(argument_y / argument_x) + + if argument_y > 0.0 and argument_x == 0.0: + angle = np.pi / 2.0 + + if argument_y < 0.0 and argument_x == 0.0: + angle = - np.pi / 2.0 + + if angle < 0.0: + angle += 2.0 * np.pi + + # angle = np.arctan2(argument_y, argument_x) + + return angle + +def identify_single_jumps(yaw): + num_points_neg = 20 + num_points_pos = 5 + for i in range(1, len(yaw) - num_points_neg - 1): + if any(yaw[i] - yaw[i + 1 : i + 20]) > np.pi / 2 and yaw[i] - yaw[i - 1] > np.pi / 2: + yaw[i] -= np.pi + elif any(yaw[i] - yaw[i + 1 : i + 1] < - np.pi / 2) and yaw[i] - yaw[i - 1] < - np.pi / 2: + yaw[i] += np.pi + + return yaw + +def unwrap(yaw): + for i in range(0, len(yaw) - 1): + if yaw[i] - yaw[i + 1] > np.pi / 2: + yaw[i + 1:] += np.pi + + return yaw + + + + +track = Track(ds=0.1, shape="oval", width=1.2) + +# from Localization_helpers import Track +# l = Track(0.01,0.8) +# l.createRaceTrack() + +homedir = os.path.expanduser("~") +# directory = homedir + "/barc_debugging/Estimator_Data/4_high_velocity/" +directory = homedir + "/barc_debugging/Estimator_Data/7_high_velocity_new_car/" + +# FIGURE 1 plotting of estimator output data +pathSave = directory + "estimator_output.npz" +npz_output = np.load(pathSave) +x_est_his = npz_output["x_est_his"] +y_est_his = npz_output["y_est_his"] +vx_est_his =npz_output["vx_est_his"] +vy_est_his =npz_output["vy_est_his"] +ax_est_his =npz_output["ax_est_his"] +ay_est_his =npz_output["ay_est_his"] +psiDot_est_his =npz_output["psiDot_est_his"] +yaw_est_his =npz_output["yaw_est_his"] +estimator_time =npz_output["estimator_time"] + +pathSave = directory + "estimator_imu.npz" +npz_imu = np.load(pathSave) +psiDot_his = npz_imu["psiDot_his"] +roll_his = npz_imu["roll_his"] +pitch_his = npz_imu["pitch_his"] +yaw_his = npz_imu["yaw_his"] +ax_his = npz_imu["ax_his"] +ay_his = npz_imu["ay_his"] +imu_time = npz_imu["imu_time"] + +pathSave = directory + "estimator_gps.npz" +npz_gps = np.load(pathSave) +x_his = npz_gps["x_his"] +y_his = npz_gps["y_his"] +gps_time = npz_gps["gps_time"] + +pathSave = directory + "estimator_enc.npz" +npz_enc = np.load(pathSave) +v_fl_his = npz_enc["v_fl_his"] +v_fr_his = npz_enc["v_fr_his"] +v_rl_his = npz_enc["v_rl_his"] +v_rr_his = npz_enc["v_rr_his"] +enc_time = npz_enc["enc_time"] + +pathSave = directory + "estimator_ecu.npz" +npz_ecu = np.load(pathSave) +a_his = npz_ecu["a_his"] +df_his = npz_ecu["df_his"] +ecu_time = npz_ecu["ecu_time"] + +""" +# FIGURE 1 plotting of estimator data +num_col_plt = 3 +num_row_plt = 1 +fig = plt.figure("Estimator") +ax1 = fig.add_subplot(num_col_plt,num_row_plt,1,ylabel="yaw_estimation") +ax1.plot(estimator_time,yaw_est_his,label="yaw_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_col_plt,num_row_plt,2,ylabel="v estimation") +ax2.plot(estimator_time,vx_est_his,label="vx_est") +ax2.plot(estimator_time,vy_est_his,label="vy_est") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_col_plt,num_row_plt,3,ylabel="acc & psidot estimation") +ax3.plot(estimator_time,ax_est_his,label="ax") +ax3.plot(estimator_time,ay_est_his,label="ay") +ax3.plot(estimator_time,psiDot_est_his,label="psiDot") +ax3.legend() +ax3.grid() +""" + +""" +# FIGURE 2 plotting of IMU data +num_plot = 3 +fig = plt.figure("Imu") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="IMU yaw") +ax1.plot(imu_time,yaw_his, label="yaw") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="IMU acc & psidot") +ax2.plot(imu_time,psiDot_his,label="psiDot") +ax2.plot(imu_time,ax_his,label="ax") +ax2.plot(imu_time,ay_his,label="ay") +print np.mean(ax_his), np.mean(ay_his) +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,3,ylabel="pitch & roll angle") +ax3.plot(imu_time,roll_his,label="roll angle") +ax3.plot(imu_time,pitch_his,label="pitch angle") +ax3.legend() +ax3.grid() +""" + +""" +# ecu plot +fig = plt.figure("input") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(ecu_time, df_his, "-", label="cmd.df") +ax4.plot(ecu_time, a_his, "--", label="cmd.a") +ax4.legend() +ax4.grid() + +# enc plot +fig = plt.figure("encoder") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(enc_time, v_fl_his, "--", label="fl") +ax4.plot(enc_time, v_fr_his, "--", label="fr") +ax4.plot(enc_time, v_rl_his, "-", label="rl") +ax4.plot(enc_time, v_rr_his, "-", label="rr") +ax4.legend() +ax4.grid() +""" + +print(len(x_his)) + +index = 2550 +# num_points = 800 +# num_points = 2000 +num_points = 7000 + +# index = 5000 +# num_points = 400 + +# index = 5250 +# num_points = 400 + +interpolated_x = np.zeros((num_points)) +interpolated_y = np.zeros((num_points)) +interpolated_yaw = np.zeros((num_points)) +gps_yaw = np.zeros((num_points)) + +start_time = gps_time[index] +end_time = gps_time[index + num_points] + +times = np.linspace(start_time, end_time, num_points) + +print gps_time[index : 10 + index] + +gps_t, indices = np.unique(gps_time, return_index=True) + +for i in range(num_points): + x_interp, y_interp = interpolate_gps(x_his[indices], y_his[indices], gps_t, times[i]) + interpolated_x[i] = x_interp + interpolated_y[i] = y_interp + gps_yaw[i] = approximate_yaw(x_his[indices], y_his[indices], gps_t, gps_time[index + i]) + + interpolated_yaw[i] = approximate_yaw(interpolated_x, interpolated_y, times, gps_time[index + i]) + +""" +poly_yaw = np.zeros((num_points)) + +for i in range(num_points): + poly_yaw[i] = interpolate_yaw(gps_yaw, gps_t, gps_time[index + i]) +""" + +""" +unwrapped_gps_yaw = copy.copy(gps_yaw) +unwrapped_gps_yaw = identify_single_jumps(unwrapped_gps_yaw) +# unwrapped_gps_yaw = unwrap(unwrapped_gps_yaw) + +unwrapped_interp_yaw = copy.copy(interpolated_yaw) +unwrapped_interp_yaw = identify_single_jumps(unwrapped_interp_yaw) +# unwrapped_interp_yaw = unwrap(unwrapped_interp_yaw) +""" + +# trajectory + +est_start = np.argmax(estimator_time >= start_time) +est_end = np.argmax(gps_time >= end_time) - 1 + +imu_start = np.argmax(imu_time >= start_time) +imu_end = np.argmax(imu_time >= end_time) - 1 + +start_angle = yaw_est_his[est_start] +num_full_rotations = np.floor(start_angle / (2 * np.pi)) +add_rotations = num_full_rotations * 2 * np.pi + +fig = plt.figure("track x-y plot") +ax1 = fig.add_subplot(1,1,1,ylabel="track x-y plot") +ax1.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], color="grey",linestyle="--", alpha=0.3) +ax1.plot(track.xy_inner[:, 0], track.xy_inner[:, 1], color="red",alpha=0.3) +ax1.plot(track.xy_outer[:, 0], track.xy_outer[:, 1],color="red",alpha=0.3) +ax1.axis("equal") +ax1.plot(x_est_his[est_start : est_end], y_est_his[est_start : est_end],color="green", label="estimator") +# ax1.plot(x_his, y_his, color="red") +ax1.plot(x_his[index : index + num_points], y_his[index : index + num_points], "o", color="red", label="gps") +ax1.plot(interpolated_x, interpolated_y, "-", color="blue", label="interpolation") +ax1.legend() + +# GPS comparison +num_plot = 2 +fig = plt.figure("GPS") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="x") +ax1.plot(gps_time, x_his, label="x") +ax1.plot(estimator_time, x_est_his, label="x_est") +ax1.plot(times, interpolated_x, "-", color="red", label="interpolation") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="y") +ax2.plot(gps_time, y_his, label="y") +ax2.plot(estimator_time, y_est_his, label="y_est") +ax2.plot(times, interpolated_y, "-", color="red", label="interpolation") +ax2.legend() +ax2.grid() + +# Yaw comparison +fig = plt.figure("Yaw") +ax1 = fig.add_subplot(1,1,1,ylabel="x") +# ax1.plot(gps_time[index : index + num_points], unwrapped_gps_yaw, label="gps2") +ax1.plot(gps_time[index : index + num_points], np.unwrap(gps_yaw) + 2 * np.pi, label="gps") +ax1.plot(times, np.unwrap(interpolated_yaw) , label="from interpolated") +ax1.plot(imu_time[imu_start : imu_end], yaw_his[imu_start : imu_end], label="imu") +# ax1.plot(times, unwrapped_interp_yaw, label="interpolated2") +# ax1.plot(times, poly_yaw, label="interpolated") +ax1.plot(estimator_time[est_start : est_end], yaw_est_his[est_start : est_end], "o", color="red", label="estimator") +ax1.legend() +ax1.grid() + + +imu_zeros = np.where(np.diff(imu_time) == 0.)[0] +imu_zeros.tolist() +indices = np.array(range(len(np.diff(imu_time)))) +time_diff = np.diff(imu_time) + +# Timing differences +fig = plt.figure("Timing") +ax1 = fig.add_subplot(3,1,1,ylabel="delta t") +ax1.plot(range(len(np.diff(imu_time))), np.diff(imu_time), label="imu") +ax1.plot(indices[imu_zeros], time_diff[imu_zeros], "r*") +ax1.legend() +ax1.grid() + +""" +# Timing differences +fig = plt.figure("Timing") +ax1 = fig.add_subplot(3,1,1,ylabel="delta t") +ax1.plot(range(len(imu_time)), ax_his, label="imu") +ax1.plot(indices[imu_zeros], ax_his[imu_zeros], "r*") +ax1.plot(indices[imu_zeros + 1], ax_his[imu_zeros + 1], "r*") +ax1.legend() +ax1.grid() +""" + +enc_zeros = np.where(np.diff(enc_time) == 0.)[0] +enc_zeros.tolist() +indices = np.array(range(len(np.diff(enc_time)))) +time_diff = np.diff(enc_time) +ax2 = fig.add_subplot(3,1,2,ylabel="delta t") +ax2.plot(range(len(np.diff(enc_time))), np.diff(enc_time), label="encoders") +ax2.plot(indices[enc_zeros], time_diff[enc_zeros], "r*") +ax2.legend() +ax2.grid() + +gps_zeros = np.where(np.diff(gps_time) == 0.)[0] +gps_zeros.tolist() +indices = np.array(range(len(np.diff(gps_time)))) +time_diff = np.diff(gps_time) +ax3 = fig.add_subplot(3,1,3,ylabel="delta t") +ax3.plot(range(len(np.diff(gps_time))), np.diff(gps_time), label="gps") +ax3.plot(indices[gps_zeros], time_diff[gps_zeros], "r*") +ax3.legend() +ax3.grid() + +""" +# raw data and estimation data comparison +num_plot = 3 +fig = plt.figure("raw data and est data comparison") +ax2 = fig.add_subplot(num_plot,1,1,ylabel="ax") +ax2.plot(imu_time, ax_his, ".", label="ax_meas") +ax2.plot(estimator_time, ax_est_his, label="ax_est") +num_points = min(len(estimator_time), len(a_his)) +ax2.plot(estimator_time[: num_points - 1], a_his[: num_points - 1], "--", label="cmd.acc") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,2,ylabel="ay") +ax3.plot(imu_time, ay_his, ".", label="ay_meas") +ax3.plot(estimator_time, ay_est_his, label="ay_est") +num_points = min(len(estimator_time), len(df_his)) +ax3.plot(estimator_time[: num_points - 1], df_his[: num_points - 1], "--", label="cmd.df") +ax3.legend() +ax3.grid() +ax4 = fig.add_subplot(num_plot,1,3,ylabel="psidot") +ax4.plot(imu_time, psiDot_his, ".", label="psidot_meas") +ax4.plot(estimator_time,psiDot_est_his,label="psidot_est") +ax4.plot(estimator_time[: num_points - 1], df_his[: num_points - 1], "--", label="cmd.df") +ax4.legend() +ax4.grid() +""" + +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/low_level_controller.jl b/workspace/src/barc/src/low_level_controller.jl new file mode 100755 index 00000000..a1269da0 --- /dev/null +++ b/workspace/src/barc/src/low_level_controller.jl @@ -0,0 +1,93 @@ + #!/usr/bin/env julia + + #= + File name: low_level_controller.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed at UC +# Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu) and Greg Marcil (grmarcil@berkeley.edu). The cloud +# services integation with ROS was developed by Kiet Lam +# (kiet.lam@berkeley.edu). The web-server app Dator was based on an open source +# project by Bruce Wootton +# --------------------------------------------------------------------------- + +using RobotOS + + +type LowLevelController + motor_pwm::Float64 + servo_pwm::Float64 + ecu_pub::RobotOS.Publisher{barc.msg.ECU} + ecu_cmd::ECU + + LowLevelController() = new() +end + +function init!(low_level_controller::LowLevelController) + low_level_controller.motor_pwm = 90 + low_level_controller.servo_pwm = 90 + low_level_controller.ecu_pub = Publisher("ecu_pwm", ECU, + queue_size = 1)::RobotOS.Publisher{barc.msg.ECU} + low_level_controller.ecu_cmd = ECU() +end + +function pwm_converter!(low_level_controller::LowLevelController, + acceleration::Float64, steering_angle::Float64) + # translate from SI units in vehicle model + # to pwm angle units (i.e. to send command signal to actuators) + # convert desired steering angle to degrees, saturate based on input limits + + node_name = get_name()[2 : end] + + if node_name == "agent_1" + # println("AGENT 1 STEERING!!!!") + # low_level_controller.servo_pwm = 90.0 + 89.0 * Float64(steering_angle) + low_level_controller.servo_pwm = 81.4 + 89.1 * Float64(steering_angle) + else + # println("AGENT 2 STEERING!!!!") + low_level_controller.servo_pwm = 83.3 + 103.1 * Float64(steering_angle) + end + + # compute motor command + FxR = Float64(acceleration) + + if FxR == 0 + low_level_controller.motor_pwm = 90.0 + elseif FxR > 0 + # using writeMicroseconds() in Arduino + low_level_controller.motor_pwm = 91 + 6.5 * FxR + # Note: Barc doesn't move for u_pwm < 93 + else # motor break / slow down + low_level_controller.motor_pwm = 93.5 + 46.73 * FxR + end + + update_arduino!(low_level_controller) +end + +function neutralize!(low_level_controller::LowLevelController) + println("Slowing down.") + low_level_controller.motor_pwm = 40 # slow down first + low_level_controller.servo_pwm = 90 + update_arduino!(low_level_controller) + sleep(1) # slow down for 1 sec + println("Stopping.") + low_level_controller.motor_pwm = 90 + update_arduino!(low_level_controller) +end + +function update_arduino!(low_level_controller::LowLevelController) + low_level_controller.ecu_cmd.header.stamp = get_rostime() + low_level_controller.ecu_cmd.motor = low_level_controller.motor_pwm + low_level_controller.ecu_cmd.servo = low_level_controller.servo_pwm + publish(low_level_controller.ecu_pub, low_level_controller.ecu_cmd) +end diff --git a/workspace/src/barc/src/models_max.py b/workspace/src/barc/src/models_max.py new file mode 100644 index 00000000..3725b2ef --- /dev/null +++ b/workspace/src/barc/src/models_max.py @@ -0,0 +1,114 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +from numpy import sin, cos, tan, arctan, array, dot +from numpy import sign, sqrt +import math + +def f_KinBkMdl(z, u, vhMdl, dt, est_mode): + """ + process model + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] + output: state at next time step z[k+1] + Does not account for drift in psi estimation! + -> Either put low trust on measured psi-values or add extra state for drift estimation! + """ + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + + # get states / inputs + x = z[0] + y = z[1] + psi = z[2] + v = z[3] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_a, L_b) = vhMdl + + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + + # compute next state + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + + return array([x_next, y_next, psi_next, v_next]) + +def h_KinBkMdl(x, u, vhMdl, dt, est_mode): + """ + Measurement model + """ + if est_mode==1: # GPS, IMU, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==2: # IMU, Enc + C = array([[0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==3: # GPS + C = array([[1, 0, 0, 0], + [0, 1, 0, 0]]) + elif est_mode==4: # GPS, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]) + else: + print("Wrong est_mode") + return dot(C, x) + +def f_SensorKinematicModel(z, u, vhMdl, dt, est_mode): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + (l_A,l_B) = vhMdl + bta = math.atan2(l_A*tan(u[1]),l_A+l_B) + zNext = [0]*14 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # drift_psi + zNext[9] = z[9] + dt*(z[12]*cos(z[11] + bta)) # x + zNext[10] = z[10] + dt*(z[12]*sin(z[11] + bta)) # y + zNext[11] = z[11] + dt*(z[12]/l_B*sin(bta)) # psi + zNext[12] = z[12] + dt*(u[0] - 0.5*z[12]) # v + zNext[13] = z[13] # drift_psi_2 + return array(zNext) + +def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*13 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = sqrt(x[2]**2+x[3]**2) # v + y[3] = x[6]+x[8] # psi + y[4] = x[7] # psiDot + y[5] = x[4] # a_x + y[6] = x[5] # a_y + y[7] = x[9] # x + y[8] = x[10] # y + y[9] = x[11]+x[13] # psi + y[10] = x[12] # v + y[11] = x[2] # v_x + y[12] = x[3] # v_y + return array(y) \ No newline at end of file diff --git a/workspace/src/barc/src/mpc_test.jl b/workspace/src/barc/src/mpc_test.jl new file mode 100755 index 00000000..fd49015e --- /dev/null +++ b/workspace/src/barc/src/mpc_test.jl @@ -0,0 +1,1558 @@ +using Polynomials + +include("optimizer.jl") + + +type MpcModel_pF + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + curvature::Array{JuMP.NonlinearParameter, 1} + z_Ref::Array{JuMP.NonlinearParameter,2} + + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + derivCost::JuMP.NonlinearExpression + costZ::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + + uPrev::Array{JuMP.NonlinearParameter,2} + + function MpcModel_pF(agent::Agent, track::Track) + println("Starting creation of pf model") + m = new() + dt = agent.dt + L_a = agent.l_front + L_b = agent.l_rear + + N = size(agent.optimal_inputs, 1) + # Q = [5.0, 0.0, 0.0, 0.1, 50.0, 0.0] # Q (only for path following mode) + Q = [0.0, 20.0, 1.0, 10.0] # Q (only for path following mode) + R = 2 * [1.0, 1.0] # put weights on a and d_f + # QderivZ = 10.0 * [1, 1, 1, 1, 1, 1] # cost matrix for derivative cost of states + QderivZ = 0.1 * [0, 1, 1, 1, 1, 1] # cost matrix for derivative cost of states + QderivU = 0.1 * [1, 0.1] #NOTE Set this to [5.0, 0/40.0] # cost matrix for derivative cost of inputs + # delay_df = 3 # steering delay + # delay_a = 1 # acceleration delay + # delay_df = 1 # steering delay + # delay_a = 1 # acceleration delay + + delay_a = agent.delay_a + delay_df = agent.delay_df + + vPathFollowing = 1.0 # reference speed for first lap of path following + # Q_term = 1.0 * [20.0, 1.0, 10.0, 20.0, 50.0] # weights for terminal constraints (LMPC, for xDot,yDot,psiDot,ePsi,eY).Not used if using convex hull + + # Q_term_cost = 3 # scaling of Q-function + # Q_lane = 1 # weight on the soft constraint for the lane + # Q_vel = 1 # weight on the soft constraint for the maximum velocity + # Q_slack = 1 * [20.0, 1.0, 10.0, 30.0, 80.0, 50.0] #[20.0,10.0,10.0,30.0,80.0,50.0] #vx,vy,psiDot,ePsi,eY,s + # Q_obs = ones(Nl * selectedStates.Np) # weight to esclude some of the old trajectories + + println("prediction h = ", N) + + v_ref = vPathFollowing + acc_f = 1.0 + + # n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree of curvature approximation + + # Create function-specific parameters + z_ref::Array{Float64,2} + z_ref = cat(2, zeros(N + 1, 3), v_ref * ones(N + 1, 1)) # Reference trajectory: path following -> stay on line and keep constant velocity + u_Ref = zeros(N, 2) + + # Create Model + mdl = Model(solver = IpoptSolver(print_level=0, max_cpu_time=0.09, + linear_solver="ma27")) + #,linear_solver="ma57",print_user_options="yes")) + + # Create variables (these are going to be optimized) + @variable(mdl, z_Ol[1 : (N + 1), 1 : 4], start = 0) # z = s, ey, epsi, v + # @variable(mdl, z_Ol[1 : (N + 1), 1 : 5], start = 0) + @variable(mdl, u_Ol[1 : N, 1 : 2], start = 0) + + # Set bounds + z_lb_4s = ones(N + 1, 1) * [-Inf -Inf -Inf -0.5] # lower bounds on states + z_ub_4s = ones(N + 1, 1) * [Inf Inf Inf 1.5] # upper bounds + u_lb_4s = ones(N, 1) * [0 -0.4] # lower bounds on steering + u_ub_4s = ones(N, 1) * [1.2 0.4] # upper bounds + + for i = 1 : 2 + for j = 1 : N + setlowerbound(u_Ol[j, i], u_lb_4s[j, i]) + setupperbound(u_Ol[j, i], u_ub_4s[j, i]) + end + end + + # for i=1:4 + # for j=1:N+1 + # setlowerbound(z_Ol[j,i], z_lb_4s[j,i]) + # setupperbound(z_Ol[j,i], z_ub_4s[j,i]) + # end + # end + + @NLparameter(mdl, z_Ref[1 : N + 1, 1 : 4] == 0) + # @NLparameter(mdl, z0[i = 1 : 5] == 0) + @NLparameter(mdl, z0[i = 1 : 4] == 0) + @NLparameter(mdl, uPrev[1 : 10, 1 : 2] == 0) + + # Curvature + # kappa = zeros(N) + # @NLparameter(mdl, c[1 : N] == 0) + if POLYNOMIAL_CURVATURE + @NLparameter(mdl, coeff[i = 1 : 5 + 1] == 0) + @NLexpression(mdl, curvature[i = 1 : N], + sum{coeff[j] * z_Ol[i, 1]^(j - 1), + j = 2 : 6} + coeff[1]) + else + @NLparameter(mdl, curvature[i = 1 : N] == 0) + end + + # System dynamics + setvalue(z0[4], v_ref) + # @NLconstraint(mdl, [i = 1 : 5], z_Ol[1, i] == z0[i]) # initial condition + @NLconstraint(mdl, [i = 1 : 4], z_Ol[1, i] == z0[i]) # initial condition + + for i = 1 : N + if i <= delay_df + @NLexpression(mdl, bta[i], atan(L_a / (L_a + L_b) * tan(uPrev[delay_df + 1 - i, 2]))) + else + @NLexpression(mdl, bta[i], atan(L_a / (L_a + L_b) * tan(u_Ol[i - delay_df, 2]))) + end + + #= + if i <= delay_a + @NLconstraint(mdl, z_Ol[i + 1, 5] == z_Ol[i, 5] + dt * + (uPrev[delay_a + 1 - i, 1] - z_Ol[i, 5]) * acc_f) # v + else + @NLconstraint(mdl, z_Ol[i + 1, 5] == z_Ol[i, 5] + dt * + (u_Ol[i - delay_a, 1] - z_Ol[i, 5]) * acc_f) # v + end + =# + + # s_dot + @NLexpression(mdl, dsdt[i], z_Ol[i, 4] * cos(z_Ol[i, 3] + bta[i]) / (1 - z_Ol[i, 2] * curvature[i])) + # s + @NLconstraint(mdl, z_Ol[i + 1, 1] == z_Ol[i, 1] + dt * dsdt[i]) + @NLconstraint(mdl, z_Ol[i + 1, 2] == z_Ol[i, 2] + dt * z_Ol[i, 4] * sin(z_Ol[i, 3] + bta[i])) # ey + @NLconstraint(mdl, z_Ol[i + 1, 3] == z_Ol[i, 3] + dt * (z_Ol[i, 4] / L_a * sin(bta[i]) - dsdt[i] * curvature[i])) # epsi + # @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * (z_Ol[i, 5] - 0.05 * z_Ol[i, 4])) # v + # v + if i <= delay_a + @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * (uPrev[delay_a + 1 - i, 1])) + # @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * (uPrev[delay_a + 1 - i, 1] - 0.05 * z_Ol[i, 4])) + else + @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * (u_Ol[i - delay_a, 1, 1])) + # @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * (u_Ol[i - delay_a, 1, 1] - 0.05 * z_Ol[i, 4])) + end + end + + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] <= 0.06) + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] >= - 0.06) + + for i = 1 : N - 1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] <= 0.06) + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] >= - 0.06) + end + + # Cost definitions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j] * (sum{(z_Ol[i, j] - z_Ol[i + 1, j])^2, i = 1 : N}), j = 1 : 4} + + QderivU[1] * ((uPrev[1, 1] - u_Ol[1, 1])^2 + sum{(u_Ol[i, 1] - u_Ol[i + 1, 1])^2, i = 1 : N - delay_a - 1})+ + QderivU[2] * ((uPrev[1, 2] - u_Ol[1, 2])^2 + sum{(u_Ol[i, 2] - u_Ol[i + 1, 2])^2, i = 1 : N - delay_df - 1})) + + # Control Input cost + @NLexpression(mdl, controlCost, 0.5 * R[1] * sum{(u_Ol[i, 1])^2, i = 1 : N - delay_a} + + 0.5 * R[2] * sum{(u_Ol[i, 2])^2, i = 1 : N - delay_df}) + + # State cost + @NLexpression(mdl, costZ, 1.0 * sum{Q[i] * sum{(z_Ol[j, i] - z_Ref[j, i])^2, + j = 2 : N + 1}, i = 1 : 4}) # Follow trajectory + + # Objective function + @NLobjective(mdl, Min, costZ + derivCost + controlCost) + + # create first artificial solution (for warm start) + for i = 1 : N + 1 + setvalue(z_Ol[i, :], [(i-1)*dt*v_ref 0 0 v_ref]) + end + for i = 1 : N + setvalue(u_Ol[i, :], [0.15 0]) + end + + # First solve + sol_stat = solve(mdl) + println("Finished solve 1: $sol_stat") + sol_stat = solve(mdl) + println("Finished solve 2: $sol_stat") + + m.mdl = mdl + m.z0 = z0 + m.z_Ref = z_Ref + if POLYNOMIAL_CURVATURE + m.curvature = coeff + else + m.curvature = curvature + end + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.uPrev = uPrev + m.derivCost = derivCost + m.costZ = costZ + m.controlCost = controlCost + return m + end +end + + +type MpcModel_convhull + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + curvature::Array{JuMP.NonlinearParameter, 1} + + eps_lane::Array{JuMP.Variable,1} + #eps_alpha::Array{JuMP.Variable,1} + #eps_vel::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + slackVx::JuMP.NonlinearExpression + slackVy::JuMP.NonlinearExpression + slackPsidot::JuMP.NonlinearExpression + slackEpsi::JuMP.NonlinearExpression + slackEy::JuMP.NonlinearExpression + slackS::JuMP.NonlinearExpression + #slackCost::JuMP.NonlinearExpression + #velocityCost::JuMP.NonlinearExpression + + # function MpcModel_convhull(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates) + function MpcModel_convhull(agent::Agent, track::Track) + + m = new() + + #### Initialize parameters + dt = agent.dt + L_a = agent.l_front + L_b = agent.l_rear + + N = size(agent.optimal_inputs, 1) + + ey_max = track.width / 2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons + # n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation + # v_max = 1.7 # maximum allowed velocity + + delay_a = agent.delay_a + delay_df = agent.delay_df + + #= + # tuning for fast laps + Q = [0.0, 10.0, 1.0, 10.0] # Q (only for path following mode) + R = 0.0 * [1.0, 1.0] # put weights on a and d_f + QderivZ = 1.0 * [1, 1, 1, 1, 1, 1.0] # cost matrix for derivative cost of states + QderivU = 1.0 * 1.0 * 1.0 * [15.0, 0.1] + + vPathFollowing = 1.0 # reference speed for first lap of path following + + Q_term_cost = 1.0 # scaling of Q-function + Q_lane = 16.0 + Q_slack = 1.0 * [100.0, 1.0, 1.0, 1.0, 5.0, 10.0] + =# + + Q = [0.0, 10.0, 1.0, 10.0] # Q (only for path following mode) + R = 0.0 * [1.0, 1.0] # put weights on a and d_f + QderivZ = 1.0 * [1, 1, 1, 1, 1, 1.0] # cost matrix for derivative cost of states + QderivU = 1.0 * 1.0 * 1.0 * [10.0, 0.1] + + vPathFollowing = 1.0 # reference speed for first lap of path following + + Q_term_cost = 1.0 # scaling of Q-function + Q_lane = 16.0 + # Q_slack = 1.1 * 1.0 * [100.0, 1.0, 1.0, 1.0, 4.0 * 5.0, 10.0] + Q_slack = 1.0 * 1.0 * [100.0, 1.0, 1.0, 1.0, 0.5 * 0.5 * 4.0 * 5.0, 10.0] + # Q_slack = 1.1 * 1.0 * [100.0, 1.0, 1.0, 0.1 * 1.0, 0.1 * 4.0 * 5.0, 10.0] + + println("prediction horizon = ", N) + + num_considered_states = size(agent.selected_states_s)[1] + acc_f = 1.0 # if this is on, the agent accelerates and decelerates the whole time + # acc_f = 0.0 + # acc_f = 100 # if this is on, the agent accelerates and decelerates the whole time + # acc_f = 0.1 # if this is on, the agent accelerates and decelerates the whole time + + + mdl = Model(solver = IpoptSolver(print_level=0, max_cpu_time=0.09, + linear_solver="ma27"))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + # @variable( mdl, z_Ol[1 : (N + 1), 1 : 7]) + @variable( mdl, z_Ol[1 : (N + 1), 1 : 6]) + @variable( mdl, u_Ol[1 : N, 1 : 2]) + @variable( mdl, eps_lane[1 : N + 1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1 : num_considered_states] >= 0) # coefficients of the convex hull + # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + # @variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity + + z_lb_6s = ones(N + 1, 1) * [0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(N + 1, 1) * [3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(N, 1) * [-1.3 -0.4] # lower bounds on steering + u_ub_6s = ones(N, 1) * [3.0 0.4] # upper bounds + + for i = 1 : 2 + for j = 1 : N + setlowerbound(u_Ol[j, i], u_lb_6s[j, i]) + setupperbound(u_Ol[j, i], u_ub_6s[j, i]) + end + end + + # for i = 1 : 7 + for i = 1 : 6 + for j = 1 : N + 1 + setlowerbound(z_Ol[j, i], z_lb_6s[j, i]) + setupperbound(z_Ol[j, i], z_ub_6s[j, i]) + end + end + + # @NLparameter(mdl, z0[i = 1 : 7] == 0) + @NLparameter(mdl, z0[i = 1 : 6] == 0) + @NLparameter(mdl, c_Vx[i = 1 : 3] == 0) + @NLparameter(mdl, c_Vy[i = 1 : 4] == 0) + @NLparameter(mdl, c_Psi[i = 1 : 3] == 0) + @NLparameter(mdl, uPrev[1 : 10, 1 : 2] == 0) + @NLparameter(mdl, selStates[1 : num_considered_states, 1 : 6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1 : num_considered_states] == 0) # costs of the states selected in "convhullStates" + + if POLYNOMIAL_CURVATURE + @NLparameter(mdl, coeff[i = 1 : 5 + 1] == 0) + @NLexpression(mdl, curvature[i = 1 : N], + sum{coeff[j] * z_Ol[i, 1]^(j - 1), + j = 2 : 6} + coeff[1]) + else + @NLparameter(mdl, curvature[i = 1 : N] == 0) + end + + # Conditions for first solve: + setvalue(z0[1], 1) + setvalue(c_Vx[3], 0.1) + + # @NLconstraint(mdl, [i = 1 : 7], z_Ol[1, i] == z0[i]) + @NLconstraint(mdl, [i = 1 : 6], z_Ol[1, i] == z0[i]) + + @NLconstraint(mdl, [i = 2 : N + 1], z_Ol[i, 5] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i = 2 : N + 1], z_Ol[i, 5] >= - ey_max - eps_lane[i]) + # @NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,4] <= v_max + eps_vel[i] ) # soft constraint on maximum velocity + @NLconstraint(mdl, sum{alpha[i], i = 1 : num_considered_states} == 1) # constraint on the coefficients of the convex hull + + #= + for i = 1 : 6 + @NLconstraint(mdl, z_Ol[N + 1, i] == sum{alpha[j] * selStates[j, i], + j = 1 : num_considered_states}) + end + =# + + #for n = 1:6 + #@NLconstraint(mdl,z_Ol[N+1,n] == sum{alpha[j]*selStates[j,n],j=1:num_considered_states}) # terminal constraint + #@NLconstraint(mdl,z_Ol[N+1,n] >= sum{alpha[j]*selStates[j,n],j=1:num_considered_states}-eps_alpha[n]) + #@NLconstraint(mdl,z_Ol[N+1,n] <= sum{alpha[j]*selStates[j,n],j=1:num_considered_states}+eps_alpha[n]) + #end + + @NLexpression(mdl, dsdt[i = 1 : N], (z_Ol[i, 1] * cos(z_Ol[i, 4]) - + z_Ol[i, 2] * sin(z_Ol[i, 4])) / + (1 - z_Ol[i, 5] * curvature[i])) + + println("Initializing model...") + + # System dynamics + for i = 1 : N + if i <= delay_df + # yDot + @NLconstraint(mdl, z_Ol[i + 1, 2] == z_Ol[i, 2] + + c_Vy[1] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Vy[2] * z_Ol[i, 1] * z_Ol[i, 3] + + c_Vy[3] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Vy[4] * uPrev[delay_df + 1 - i, 2]) + # psiDot + @NLconstraint(mdl, z_Ol[i + 1, 3] == z_Ol[i, 3] + + c_Psi[1] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Psi[2] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Psi[3] * uPrev[delay_df + 1 - i, 2]) + else + # yDot + @NLconstraint(mdl, z_Ol[i + 1, 2] == z_Ol[i, 2] + + c_Vy[1] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Vy[2] * z_Ol[i, 1] * z_Ol[i, 3] + + c_Vy[3] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Vy[4] * u_Ol[i - delay_df, 2]) + # psiDot + @NLconstraint(mdl, z_Ol[i + 1, 3] == z_Ol[i, 3] + + c_Psi[1] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Psi[2] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Psi[3] * u_Ol[i - delay_df, 2]) + end + # xDot + if i <= delay_a + @NLconstraint(mdl, z_Ol[i + 1, 1] == z_Ol[i, 1] + + c_Vx[1] * z_Ol[i, 2] * z_Ol[i, 3] + + c_Vx[2] * z_Ol[i, 1] + + c_Vx[3] * uPrev[delay_a + 1 - i, 1]) + else + @NLconstraint(mdl, z_Ol[i + 1, 1] == z_Ol[i, 1] + + c_Vx[1] * z_Ol[i, 2] * z_Ol[i, 3] + + c_Vx[2] * z_Ol[i, 1] + + c_Vx[3] * u_Ol[i - delay_a, 1]) + end + + # ePsi + @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * + (z_Ol[i, 3] - dsdt[i] * curvature[i])) + # eY + @NLconstraint(mdl, z_Ol[i + 1, 5] == z_Ol[i, 5] + dt * + (z_Ol[i, 1] * sin(z_Ol[i, 4]) + + z_Ol[i, 2] * cos(z_Ol[i, 4]))) + # s + @NLconstraint(mdl, z_Ol[i + 1, 6] == z_Ol[i, 6] + dt * dsdt[i]) + end + + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] <= 0.12) + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] >= - 0.12) + + for i = 1 : N - 1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] <= 0.12) + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] >= - 0.12) + end + + # Cost functions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j] * (sum{(z_Ol[i, j] - z_Ol[i + 1, j])^2, i = 1 : N}), j = 1 : 6} + + QderivU[1] * ((uPrev[1, 1] - u_Ol[1, 1])^2 + + sum{(u_Ol[i, 1] - u_Ol[i + 1, 1])^2, i = 1 : N - 1}) + + QderivU[2] * ((uPrev[1, 2] - u_Ol[1, 2])^2 + + sum{(u_Ol[i, 2] - u_Ol[i + 1, 2])^2, i = 1 : N - delay_df - 1})) + + # Control Input cost + @NLexpression(mdl, controlCost, R[1] * sum{(u_Ol[i, 1])^2, i = 1 : N - delay_a} + + R[2] * sum{(u_Ol[i, 2])^2, i = 1 : N - delay_df}) + + # Lane cost (soft) + @NLexpression(mdl, laneCost, Q_lane * sum{1.0 * eps_lane[i] + + 20.0 * eps_lane[i]^2, + i = 2 : N + 1}) + # @NLexpression(mdl, laneCost, Q_lane * sum{10.0 * eps_lane[i] + + # 100.0 * eps_lane[i]^2, + # i = 2 : N + 1}) + + # Terminal Cost + @NLexpression(mdl, terminalCost , sum{alpha[i] * statesCost[i], i = 1 : num_considered_states}) + + # Slack cost (soft) + #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + + # Slack cost on vx + @NLexpression(mdl, slackVx, (z_Ol[N + 1, 1] - sum{alpha[j] * selStates[j, 1], j = 1 : num_considered_states})^2) + + # Slack cost on vy + @NLexpression(mdl, slackVy, (z_Ol[N + 1, 2] - sum{alpha[j] * selStates[j, 2], j = 1 : num_considered_states})^2) + + # Slack cost on Psi dot + @NLexpression(mdl, slackPsidot, (z_Ol[N + 1, 3] - sum{alpha[j] * selStates[j, 3], j = 1 : num_considered_states})^2) + + # Slack cost on ePsi + @NLexpression(mdl, slackEpsi, (z_Ol[N + 1, 4] - sum{alpha[j] * selStates[j, 4], j = 1 : num_considered_states})^2) + + # Slack cost on ey + @NLexpression(mdl, slackEy, (z_Ol[N + 1, 5] - sum{alpha[j] * selStates[j, 5], j = 1 : num_considered_states})^2) + + # Slack cost on s + @NLexpression(mdl, slackS, (z_Ol[N + 1, 6] - sum{alpha[j] * selStates[j, 6], j = 1 : num_considered_states})^2) + + # Velocity Cost + #@NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) + + # Overall Cost function (objective of the minimization) + #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + @NLobjective(mdl, Min, derivCost + laneCost + Q_term_cost * terminalCost + controlCost + + Q_slack[1] * slackVx + Q_slack[2] * slackVy + + Q_slack[3] * slackPsidot + Q_slack[4] * slackEpsi + + Q_slack[5] * slackEy + Q_slack[6] * slackS) #+ controlCost + + sol_stat = solve(mdl) + println("Finished solve 1 convhull mpc: $sol_stat") + sol_stat = solve(mdl) + println("Finished solve 2 convhull mpc: $sol_stat") + + m.mdl = mdl + m.z0 = z0 + if POLYNOMIAL_CURVATURE + m.curvature = coeff + else + m.curvature = curvature + end + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + #m.eps_alpha=eps_alpha + + m.derivCost = derivCost + m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + #m.velocityCost= velocityCost #velocity cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + + m.slackVx = slackVx + m.slackVy = slackVy + m.slackPsidot = slackPsidot + m.slackEpsi = slackEpsi + m.slackEy = slackEy + m.slackS = slackS + + return m + end +end + + +type MpcModel_obstacle + + mdl::JuMP.Model + + z0::Array{JuMP.NonlinearParameter,1} + coeff::Array{JuMP.NonlinearParameter,1} + selStates::Array{JuMP.NonlinearParameter,2} + statesCost::Array{JuMP.NonlinearParameter,1} + c_Vx::Array{JuMP.NonlinearParameter,1} + c_Vy::Array{JuMP.NonlinearParameter,1} + c_Psi::Array{JuMP.NonlinearParameter,1} + uPrev::Array{JuMP.NonlinearParameter,2} + curvature::Array{JuMP.NonlinearParameter, 1} + weights_obstacles::Array{JuMP.NonlinearParameter, 1} + weights_obstacle::JuMP.NonlinearParameter + safety_weight::JuMP.NonlinearParameter + adv_states_s::Array{JuMP.NonlinearParameter, 2} + current_lap::JuMP.NonlinearParameter + adv_lap::JuMP.NonlinearParameter + + eps_lane::Array{JuMP.Variable,1} + #eps_alpha::Array{JuMP.Variable,1} + #eps_vel::Array{JuMP.Variable,1} + alpha::Array{JuMP.Variable,1} + z_Ol::Array{JuMP.Variable,2} + u_Ol::Array{JuMP.Variable,2} + + dsdt::Array{JuMP.NonlinearExpression,1} + c::Array{JuMP.NonlinearExpression,1} + + derivCost::JuMP.NonlinearExpression + controlCost::JuMP.NonlinearExpression + laneCost::JuMP.NonlinearExpression + terminalCost::JuMP.NonlinearExpression + obstacle_cost::JuMP.NonlinearExpression + progress_cost::JuMP.NonlinearExpression + slackVx::JuMP.NonlinearExpression + slackVy::JuMP.NonlinearExpression + slackPsidot::JuMP.NonlinearExpression + slackEpsi::JuMP.NonlinearExpression + slackEy::JuMP.NonlinearExpression + slackS::JuMP.NonlinearExpression + #slackCost::JuMP.NonlinearExpression + #velocityCost::JuMP.NonlinearExpression + + # function MpcModel_convhull(mpcParams::MpcParams,mpcCoeff::MpcCoeff,modelParams::ModelParams,trackCoeff::TrackCoeff,selectedStates::SelectedStates) + function MpcModel_obstacle(agent::Agent, track::Track) + + m = new() + + #### Initialize parameters + dt = agent.dt + L_a = agent.l_front + L_b = agent.l_rear + width = agent.width + + N = size(agent.optimal_inputs, 1) + + ey_max = track.width / 2 # bound for the state ey (distance from the center track). It is set as half of the width of the track for obvious reasons + # n_poly_curv = trackCoeff.nPolyCurvature # polynomial degree for curvature approximation + if agent.index == 2 + v_max = 1.5 # maximum allowed velocity + else + v_max = 100.0 + end + + delay_a = agent.delay_a + delay_df = agent.delay_df + + Q = [0.0, 10.0, 1.0, 10.0] # Q (only for path following mode) + R = 0.0 * [1.0, 1.0] # put weights on a and d_f + QderivZ = 1.0 * [1, 1, 1, 1, 1, 1.0] # cost matrix for derivative cost of states + QderivU = 1.0 * 1.0 * 1.0 * [2 * 15.0, 0.1] + + vPathFollowing = 1.0 # reference speed for first lap of path following + + Q_term_cost = 1.0 # scaling of Q-function + Q_lane = 0.5 * 16.0 + Q_slack = 1.1 * 1.0 * [100.0, 1.0, 1.0, 1.0, 1.0 * 5.0, 10.0] + Q_vel = 1.0 + + weights_progress = 0.5 * (- 1.0 * ones(N + 1)) + + println("prediction horizon = ", N) + + num_considered_states = size(agent.selected_states_s)[1] + acc_f = 1.0 # if this is on, the agent accelerates and decelerates the whole time + # acc_f = 0.0 + # acc_f = 100 # if this is on, the agent accelerates and decelerates the whole time + # acc_f = 0.1 # if this is on, the agent accelerates and decelerates the whole time + + + mdl = Model(solver = IpoptSolver(print_level=0, max_cpu_time=0.09, + linear_solver="ma27"))#,check_derivatives_for_naninf="yes"))#,linear_solver="ma57",print_user_options="yes")) + + # @variable( mdl, z_Ol[1 : (N + 1), 1 : 7]) + @variable( mdl, z_Ol[1 : (N + 1), 1 : 6]) + @variable( mdl, u_Ol[1 : N, 1 : 2]) + @variable( mdl, eps_lane[1 : N + 1] >= 0) # eps for soft lane constraints + @variable( mdl, alpha[1 : num_considered_states] >= 0) # coefficients of the convex hull + # @variable( mdl, eps_alpha[1:6] >=0) # eps for soft constraint on alpha + @variable( mdl, eps_vel[1:N+1]>=0) # eps for soft constraint on velocity + + z_lb_6s = ones(N + 1, 1) * [0.1 -Inf -Inf -Inf -Inf -Inf -Inf] # lower bounds on states + z_ub_6s = ones(N + 1, 1) * [3.5 Inf Inf Inf Inf Inf Inf] # upper bounds + u_lb_6s = ones(N, 1) * [-1.3 -0.3] # lower bounds on steering + u_ub_6s = ones(N, 1) * [2.0 0.3] # upper bounds + + for i = 1 : 2 + for j = 1 : N + setlowerbound(u_Ol[j, i], u_lb_6s[j, i]) + setupperbound(u_Ol[j, i], u_ub_6s[j, i]) + end + end + + # for i = 1 : 7 + for i = 1 : 6 + for j = 1 : N + 1 + setlowerbound(z_Ol[j, i], z_lb_6s[j, i]) + setupperbound(z_Ol[j, i], z_ub_6s[j, i]) + end + end + + # @NLparameter(mdl, z0[i = 1 : 7] == 0) + @NLparameter(mdl, z0[i = 1 : 6] == 0) + @NLparameter(mdl, c_Vx[i = 1 : 3] == 0) + @NLparameter(mdl, c_Vy[i = 1 : 4] == 0) + @NLparameter(mdl, c_Psi[i = 1 : 3] == 0) + @NLparameter(mdl, uPrev[1 : 10, 1 : 2] == 0) + @NLparameter(mdl, selStates[1 : num_considered_states, 1 : 6] == 0) # states from the previous trajectories selected in "convhullStates" + @NLparameter(mdl, statesCost[1 : num_considered_states] == 0) # costs of the states selected in "convhullStates" + @NLparameter(mdl, progress[1 : N + 1] == 0) + @NLparameter(mdl, weights_obstacles[1 : num_considered_states] == 1) + @NLparameter(mdl, weights_obstacle == 0) + @NLparameter(mdl, safety_weight == 1) + + @NLparameter(mdl, adv_states_s[1 : (N + 1), 1 : 6] == 0) + @NLparameter(mdl, current_lap == 0) + @NLparameter(mdl, adv_lap == 0) + + + if POLYNOMIAL_CURVATURE + @NLparameter(mdl, coeff[i = 1 : 5 + 1] == 0) + @NLexpression(mdl, curvature[i = 1 : N], + sum{coeff[j] * z_Ol[i, 1]^(j - 1), + j = 2 : 6} + coeff[1]) + else + @NLparameter(mdl, curvature[i = 1 : N] == 0) + end + + # Conditions for first solve: + setvalue(z0[1], 1) + setvalue(c_Vx[3], 0.1) + + # @NLconstraint(mdl, [i = 1 : 7], z_Ol[1, i] == z0[i]) + @NLconstraint(mdl, [i = 1 : 6], z_Ol[1, i] == z0[i]) + + @NLconstraint(mdl, [i = 2 : N + 1], z_Ol[i, 5] <= ey_max + eps_lane[i]) + @NLconstraint(mdl, [i = 2 : N + 1], z_Ol[i, 5] >= - ey_max - eps_lane[i]) + @NLconstraint(mdl,[i = 1:(N+1)], z_Ol[i,1] <= v_max + eps_vel[i] ) # soft constraint on maximum velocity + @NLconstraint(mdl, sum{alpha[i], i = 1 : num_considered_states} == 1) # constraint on the coefficients of the convex hull + + + @NLexpression(mdl, dsdt[i = 1 : N], (z_Ol[i, 1] * cos(z_Ol[i, 4]) - + z_Ol[i, 2] * sin(z_Ol[i, 4])) / + (1 - z_Ol[i, 5] * curvature[i])) + + println("Initializing model...") + + # System dynamics + for i = 1 : N + if i <= delay_df + # yDot + @NLconstraint(mdl, z_Ol[i + 1, 2] == z_Ol[i, 2] + + c_Vy[1] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Vy[2] * z_Ol[i, 1] * z_Ol[i, 3] + + c_Vy[3] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Vy[4] * uPrev[delay_df + 1 - i, 2]) + # psiDot + @NLconstraint(mdl, z_Ol[i + 1, 3] == z_Ol[i, 3] + + c_Psi[1] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Psi[2] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Psi[3] * uPrev[delay_df + 1 - i, 2]) + else + # yDot + @NLconstraint(mdl, z_Ol[i + 1, 2] == z_Ol[i, 2] + + c_Vy[1] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Vy[2] * z_Ol[i, 1] * z_Ol[i, 3] + + c_Vy[3] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Vy[4] * u_Ol[i - delay_df, 2]) + # psiDot + @NLconstraint(mdl, z_Ol[i + 1, 3] == z_Ol[i, 3] + + c_Psi[1] * z_Ol[i, 3] / z_Ol[i, 1] + + c_Psi[2] * z_Ol[i, 2] / z_Ol[i, 1] + + c_Psi[3] * u_Ol[i - delay_df, 2]) + end + # xDot + if i <= delay_a + @NLconstraint(mdl, z_Ol[i + 1, 1] == z_Ol[i, 1] + + c_Vx[1] * z_Ol[i, 2] * z_Ol[i, 3] + + c_Vx[2] * z_Ol[i, 1] + + c_Vx[3] * uPrev[delay_a + 1 - i, 1]) + else + @NLconstraint(mdl, z_Ol[i + 1, 1] == z_Ol[i, 1] + + c_Vx[1] * z_Ol[i, 2] * z_Ol[i, 3] + + c_Vx[2] * z_Ol[i, 1] + + c_Vx[3] * u_Ol[i - delay_a, 1]) + end + + # ePsi + @NLconstraint(mdl, z_Ol[i + 1, 4] == z_Ol[i, 4] + dt * + (z_Ol[i, 3] - dsdt[i] * curvature[i])) + # eY + @NLconstraint(mdl, z_Ol[i + 1, 5] == z_Ol[i, 5] + dt * + (z_Ol[i, 1] * sin(z_Ol[i, 4]) + + z_Ol[i, 2] * cos(z_Ol[i, 4]))) + # s + @NLconstraint(mdl, z_Ol[i + 1, 6] == z_Ol[i, 6] + dt * dsdt[i]) + end + + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] <= 0.12) + @NLconstraint(mdl, u_Ol[1, 2] - uPrev[1, 2] >= - 0.12) + + for i = 1 : N - 1 # Constraints on u: + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] <= 0.12) + @NLconstraint(mdl, u_Ol[i + 1, 2] - u_Ol[i, 2] >= - 0.12) + end + + # Cost functions + # Derivative cost + @NLexpression(mdl, derivCost, sum{QderivZ[j] * (sum{(z_Ol[i, j] - z_Ol[i + 1, j])^2, i = 1 : N}), j = 1 : 6} + + QderivU[1] * ((uPrev[1, 1] - u_Ol[1, 1])^2 + + sum{(u_Ol[i, 1] - u_Ol[i + 1, 1])^2, i = 1 : N - delay_a - 1}) + + QderivU[2] * ((uPrev[1, 2] - u_Ol[1, 2])^2 + + sum{(u_Ol[i, 2] - u_Ol[i + 1, 2])^2, i = 1 : N - delay_df - 1})) + + # Control Input cost + @NLexpression(mdl, controlCost, R[1] * sum{(u_Ol[i, 1])^2, i = 1 : N - delay_a} + + R[2] * sum{(u_Ol[i, 2])^2, i = 1 : N - delay_df}) + + # Lane cost (soft) + @NLexpression(mdl, laneCost, Q_lane * sum{1.0 * eps_lane[i] + + 20.0 * eps_lane[i]^2, + i = 2 : N + 1}) + # @NLexpression(mdl, laneCost, Q_lane * sum{10.0 * eps_lane[i] + + # 100.0 * eps_lane[i]^2, + # i = 2 : N + 1}) + + # Terminal Cost + @NLexpression(mdl, terminalCost , sum{weights_obstacles[i] * alpha[i] * + statesCost[i], i = 1 : num_considered_states}) + + # Slack cost (soft) + #@NLexpression(mdl, slackCost, sum{50*eps_alpha[i]+500*eps_alpha[i]^2,i=1:6}) + + # Slack cost on vx + @NLexpression(mdl, slackVx, (z_Ol[N + 1, 1] - sum{alpha[j] * selStates[j, 1], j = 1 : num_considered_states})^2) + + # Slack cost on vy + @NLexpression(mdl, slackVy, (z_Ol[N + 1, 2] - sum{alpha[j] * selStates[j, 2], j = 1 : num_considered_states})^2) + + # Slack cost on Psi dot + @NLexpression(mdl, slackPsidot, (z_Ol[N + 1, 3] - sum{alpha[j] * selStates[j, 3], j = 1 : num_considered_states})^2) + + # Slack cost on ePsi + @NLexpression(mdl, slackEpsi, (z_Ol[N + 1, 4] - sum{alpha[j] * selStates[j, 4], j = 1 : num_considered_states})^2) + + # Slack cost on ey + @NLexpression(mdl, slackEy, (z_Ol[N + 1, 5] - sum{alpha[j] * selStates[j, 5], j = 1 : num_considered_states})^2) + + # Slack cost on s + @NLexpression(mdl, slackS, (z_Ol[N + 1, 6] - sum{alpha[j] * selStates[j, 6], j = 1 : num_considered_states})^2) + + # Velocity Cost + @NLexpression(mdl, velocityCost , Q_vel*sum{10.0*eps_vel[i]+100.0*eps_vel[i]^2 ,i=2:N+1}) + + radius_s = zeros(NUM_AGENTS - 1) + radius_e_y = zeros(NUM_AGENTS - 1) + + for i = 1 : NUM_AGENTS - 1 + radius_s[i] = 2 * L_a + radius_e_y[i] = 2 * width / 2 + end + + @NLexpression(mdl, obstacle_cost, weights_obstacle * + sum{- log(safety_weight * (((z_Ol[j, 6] - adv_states_s[j, 1]) / radius_s[1])^2 + + ((z_Ol[j, 5] - adv_states_s[j, 2]) / radius_e_y[1])^2 - 1)), + j = 1 : (N + 1)}) + + @NLexpression(mdl, progress_cost, sum{weights_progress[i] * + ((z_Ol[i, 6] + (current_lap - 1) * track.total_length) - + (adv_states_s[i, 1] + (adv_lap - 1) * track.total_length)), i = 2 : N + 1}) + + # Overall Cost function (objective of the minimization) + #@NLobjective(mdl, Min, derivCost + laneCost + controlCost + terminalCost )#+ slackCost)#+ velocityCost) + @NLobjective(mdl, Min, derivCost + laneCost + Q_term_cost * terminalCost + controlCost + velocityCost + + Q_slack[1] * slackVx + Q_slack[2] * slackVy + + Q_slack[3] * slackPsidot + Q_slack[4] * slackEpsi + + Q_slack[5] * slackEy + Q_slack[6] * slackS + + obstacle_cost + progress_cost) #+ controlCost + + sol_stat = solve(mdl) + println("Finished solve 1 convhull mpc: $sol_stat") + sol_stat = solve(mdl) + println("Finished solve 2 convhull mpc: $sol_stat") + + m.mdl = mdl + m.z0 = z0 + if POLYNOMIAL_CURVATURE + m.curvature = coeff + else + m.curvature = curvature + end + m.z_Ol = z_Ol + m.u_Ol = u_Ol + m.c_Vx = c_Vx + m.c_Vy = c_Vy + m.c_Psi = c_Psi + m.uPrev = uPrev + #m.eps_alpha=eps_alpha + + m.derivCost = derivCost + m.controlCost = controlCost + m.laneCost = laneCost + m.terminalCost= terminalCost # terminal cost + #m.velocityCost= velocityCost #velocity cost + m.selStates = selStates # selected states + m.statesCost = statesCost # cost of the selected states + m.alpha = alpha # parameters of the convex hull + m.obstacle_cost = obstacle_cost + + m.adv_states_s = adv_states_s + m.current_lap = current_lap + m.adv_lap = adv_lap + m.obstacle_cost = obstacle_cost + m.progress_cost = progress_cost + m.weights_obstacle = weights_obstacle + m.weights_obstacles = weights_obstacles + m.safety_weight = safety_weight + + m.slackVx = slackVx + m.slackVy = slackVy + m.slackPsidot = slackPsidot + m.slackEpsi = slackEpsi + m.slackEy = slackEy + m.slackS = slackS + + return m + end +end + + +# function solveMpcProblem_pathFollow(mdl::MpcModel_pF,mpcSol::MpcSol,mpcParams_pF::MpcParams,trackCoeff::TrackCoeff,posInfo::PosInfo, +# modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},lapStatus::LapStatus) +function solveMpcProblem_pathFollow(mdl::MpcModel_pF, optimizer::Optimizer, agent::Agent, + track::Track, reference::Array{Float64}) + # Load Parameters + # v_ref = 1.0 + # ey_ref = 0.0 # 0.35 + N = size(agent.optimal_inputs, 1) + kappa = zeros(N) + dt = agent.dt + num_considered_states = size(agent.selected_states_s)[1] + + iteration = agent.current_iteration + current_s = agent.states_s[iteration, :] + s = current_s[1] + e_y = current_s[2] + e_psi = current_s[3] + v = norm(current_s[5 : 6]) + + try + agent.acc = (getvalue(mdl.z_Ol))[5, 2] + catch + println("solution states have wrong size") + agent.acc = 0.0 + end + + acc0 = agent.acc + # zCurr = [s, e_y, e_psi, v, acc0] + zCurr = [s, e_y, e_psi, v] + + u_length = 10 + u_prev = zeros(u_length, 2) + + if iteration <= u_length + u_prev[1 : iteration - 1, :] = agent.inputs[iteration - 1 : - 1 : 1, :] + current_lap = agent.current_lap + if current_lap > 1 + previous_lap = current_lap - 1 + NUM_LOADED_LAPS + needed_iter = agent.iterations_needed[previous_lap] + indeces = needed_iter : - 1 : needed_iter - (u_length - iteration) + u_prev[iteration : end, :] = squeeze(agent.previous_inputs[previous_lap, indeces, :], 1) + end + else + u_prev = agent.inputs[iteration - 1 : - 1 : iteration - u_length, :] + end + #= + lap = agent.current_lap + indeces = num_considered_states + iteration - 1 : - 1 : iteration - u_length + num_considered_states + u_prev = squeeze(agent.previous_inputs[lap, indeces, :], 1) + =# + + uPrev = u_prev + # println(uPrev) + + if POLYNOMIAL_CURVATURE + order = 5 + n_prev = 20 + n_after = 40 + total_num = n_prev + 1 + n_after + num_coords = size(track.s_coord, 1) + # s = 4 + # s = 5.746450616402124 + track_index = findmin(abs(track.s_coord - s))[2] + println("Closest s: ", track.s_coord[track_index]) + s_data = zeros(total_num) + kappa_data = zeros(total_num) + + if track_index - n_prev < 1 + difference = abs(track_index - n_prev) + s_part_1 = track.s_coord[end - difference : end] - track.total_length + s_part_2 = track.s_coord[1 : total_num - difference - 1] + s_data = [s_part_1; s_part_2] + kappa_part_1 = track.curvature[end - difference : end] + kappa_part_2 = track.curvature[1 : total_num - difference - 1] + kappa_data = [kappa_part_1; kappa_part_2] + elseif track_index + n_after > num_coords + difference = track_index + n_after - num_coords + s_part_1 = track.s_coord[track_index - n_prev : end] + s_part_2 = track.s_coord[1 : difference] + track.total_length + s_data = [s_part_1; s_part_2] + kappa_part_1 = track.curvature[track_index - n_prev : end] + kappa_part_2 = track.curvature[1 : difference] + kappa_data = [kappa_part_1; kappa_part_2] + else + s_data = track.s_coord[track_index - n_prev : track_index + n_after] + kappa_data = track.curvature[track_index - n_prev : track_index + n_after] + end + + curvature_approx = polyfit(s_data, kappa_data, order) + indeces = collect(track_index - n_prev : track_index + n_after) + kappa = coeffs(curvature_approx) + # println("coefficents: ", kappa) + # println(curvature_approx) + + #= + app_curvature = zeros(indeces, Float64) + counter = 1 + for i = track_index - n_prev : track_index + n_after + evaluated_poly = polyval(curvature_approx, i * 0.1) + # println("app curvautre: ", typeof(app_curvature[counter])) + # println(typeof(evaluated_poly)) + app_curvature[counter] = evaluated_poly + # println("Error: ", abs(evaluated_poly - track.curvature[i])) + counter += 1 + end + + println("CURVATURE APPRXOIMATION: ", kappa) + figure("test") + println(size(indeces * 0.1)) + println(size(kappa_data)) + plot(indeces * 0.1, kappa_data, "ro") + plot(indeces * 0.1, app_curvature, "bo") + plt[:show]() + =# + else + for i = 1 : N + # println(agent.predicted_s[i + 1, 1]) + s_curve = max(0.0, agent.predicted_s[i + 1, 1]) + kappa[i] = get_curvature(track, s_curve) + end + + #= + for i = 1 : N - 1 + kappa[i] = get_curvature(track, agent.predicted_s[i + 1, 1]) + end + + if size(agent.predicted_s, 2) == 6 + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + dt * agent.predicted_s[end, 5]) + else + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + dt * agent.predicted_s[end, 4]) + end + =# + end + + # z_ref1 = cat(2, zeros(N + 1, 3), v_ref * ones(N + 1, 1)) + # z_ref1 = [0.0 ey_ref 0.0 v_ref] .* ones(N + 1, 4) + z_ref1 = reference .* ones(N + 1, 4) + + # z_ref2 = cat(2, zeros(mpcParams_pF.N + 1, 1), 0.2 * ones(mpcParams_pF.N+1,1),zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) + # z_ref3 = cat(2, zeros(mpcParams_pF.N + 1, 1), - 0.1 * ones(mpcParams_pF.N+1,1),zeros(mpcParams_pF.N+1,1),v_ref*ones(mpcParams_pF.N+1,1)) + + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + # Update current initial condition, curvature and previous input + setvalue(mdl.z0, zCurr) + setvalue(mdl.uPrev, uPrev) + # println("Kappa: ", kappa) + setvalue(mdl.curvature, kappa) # Set approximate curvature + + setvalue(mdl.z_Ref, z_ref1) + + #= + if lapStatus.currentLap == 1 + setvalue(mdl.z_Ref,z_ref1) + elseif lapStatus.currentLap == 2 + setvalue(mdl.z_Ref,z_ref1) + elseif lapStatus.currentLap == 3 + setvalue(mdl.z_Ref,z_ref1) + end + =# + + # Solve Problem and return solution + sol_status = solve(mdl.mdl) + sol_u = getvalue(mdl.u_Ol) + sol_z = getvalue(mdl.z_Ol) + + curvature_values = zeros(N + 1) + if POLYNOMIAL_CURVATURE + println("Coefficents after: ", getvalue(mdl.curvature)) + polynom = Poly(getvalue(mdl.curvature)) + println(polynom) + for i = 1 : N + 1 + curvature_values[i] = polyval(polynom, sol_z[i, 1]) + end + println("Curvature: ", curvature_values) + end + + # mpcSol.a_x = sol_u[1, 1] + # mpcSol.d_f = sol_u[1, 2] + # mpcSol.u = sol_u + # mpcSol.z = sol_z + # mpcSol.solverStatus = sol_status + # mpcSol.cost = zeros(6) + + backward_mapping = [1, 2, 3, 4] + optimizer.solution_inputs = sol_u + optimizer.solution_states_s = sol_z[:, backward_mapping] + + publish_prediction(optimizer) + publish_prediction_for_opponent_pf(mdl, optimizer, agent) + + #mpcSol.cost = [getvalue(mdl.costZ),0,0,getvalue(mdl.derivCost),getvalue(mdl.controlCost),0] + + # Print information + # println("--------------- MPC PF START -----------------------------------------------") + # println("z0 = $(zCurr')") + println("Solved, status = $sol_status") + # println("Predict. to s = $(sol_z[end,1])") + # #println("uPrev = $(uPrev)") + # println("--------------- MPC PF END ------------------------------------------------") + nothing +end + + +# function solveMpcProblem_convhull(m::MpcModel_convhull,mpcSol::MpcSol,mpcCoeff::MpcCoeff,mpcParams::MpcParams,trackCoeff::TrackCoeff,lapStatus::LapStatus, +# posInfo::PosInfo,modelParams::ModelParams,zCurr::Array{Float64},uPrev::Array{Float64},selectedStates::SelectedStates) +function solveMpcProblem_convhull(m::MpcModel_convhull, optimizer::Optimizer, + agent::Agent, track::Track) + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + N = size(agent.optimal_inputs, 1) + kappa = zeros(N) + dt = agent.dt + + # selStates = selectedStates.selStates::Array{Float64,2} + # statesCost = selectedStates.statesCost::Array{Float64,1} + + # [s, e_y, e_psi, psi_dot, v_x, v_y] --> [v_x, v_y, psi_dot, e_psi, e_y, s] + mapping = [5, 6, 4, 3, 2, 1] + selected_states = agent.selected_states_s[:, mapping] + selected_cost = agent.selected_states_cost + + iteration = agent.current_iteration + current_s = agent.states_s[iteration, :] + s = current_s[1] + e_y = current_s[2] + e_psi = current_s[3] + psi_dot = current_s[4] + v_x = current_s[5] + v_y = current_s[6] + + try + agent.acc = (getvalue(m.z_Ol))[2, 7] + catch + println("solution states have wrong size") + end + + # println("INITIAL STATE: ", current_s) + + acc0 = agent.acc + # zCurr = [v_x, v_y, psi_dot, e_psi, e_y, s, acc0] + zCurr = [v_x, v_y, psi_dot, e_psi, e_y, s] + + u_length = 10 + u_prev = zeros(u_length, 2) + + #= + lap = agent.current_lap + indeces = num_considered_states + 1 + iteration - 1 : - 1 : iteration - u_length + num_considered_states + 1 + u_prev = agent.previous_inputs[lap, indeces, :] + =# + + if iteration <= u_length + u_prev[1 : iteration - 1, :] = agent.inputs[iteration - 1 : - 1 : 1, :] + current_lap = agent.current_lap + if current_lap > 1 + previous_lap = current_lap - 1 + NUM_LOADED_LAPS + needed_iter = agent.iterations_needed[previous_lap] + indeces = needed_iter : - 1 : needed_iter - (u_length - iteration) + u_prev[iteration : end, :] = squeeze(agent.previous_inputs[previous_lap, indeces, :], 1) + end + else + u_prev = agent.inputs[iteration - 1 : - 1 : iteration - u_length, :] + end + + uPrev = u_prev + + if POLYNOMIAL_CURVATURE + order = 5 + n_prev = 20 + n_after = 40 + total_num = n_prev + 1 + n_after + num_coords = size(track.s_coord, 1) + track_index = findmin(abs(track.s_coord - s))[2] + println("Closest s: ", track.s_coord[track_index]) + s_data = zeros(total_num) + kappa_data = zeros(total_num) + + if track_index - n_prev < 1 + difference = abs(track_index - n_prev) + s_part_1 = track.s_coord[end - difference : end] - track.total_length + s_part_2 = track.s_coord[1 : total_num - difference - 1] + s_data = [s_part_1; s_part_2] + kappa_part_1 = track.curvature[end - difference : end] + kappa_part_2 = track.curvature[1 : total_num - difference - 1] + kappa_data = [kappa_part_1; kappa_part_2] + elseif track_index + n_after > num_coords + difference = track_index + n_after - num_coords + s_part_1 = track.s_coord[track_index - n_prev : end] + s_part_2 = track.s_coord[1 : difference] + track.total_length + s_data = [s_part_1; s_part_2] + kappa_part_1 = track.curvature[track_index - n_prev : end] + kappa_part_2 = track.curvature[1 : difference] + kappa_data = [kappa_part_1; kappa_part_2] + else + s_data = track.s_coord[track_index - n_prev : track_index + n_after] + kappa_data = track.curvature[track_index - n_prev : track_index + n_after] + end + + curvature_approx = polyfit(s_data, kappa_data, order) + indeces = collect(track_index - n_prev : track_index + n_prev) + kappa = coeffs(curvature_approx) + # println("coefficents: ", kappa) + + #= + app_curvature = zeros(indeces, Float64) + counter = 1 + for i = track_index - n_prev : track_index + n_prev + println("i: ", i) + evaluated_poly = polyval(curvature_approx, track.s_coord[i]) + println("app curvautre: ", typeof(app_curvature[counter])) + println(typeof(evaluated_poly)) + app_curvature[counter] = evaluated_poly + println("Error: ", abs(evaluated_poly - track.curvature[i])) + counter += 1 + end + =# + + #= + println("CURVATURE APPRXOIMATION: ", kappa) + figure("test") + plot(track.s_coord[indeces], track.curvature[indeces], "ro") + plot(track.s_coord[indeces], app_curvature, "bo") + plt[:show]() + =# + else + for i = 1 : N + kappa[i] = get_curvature(track, agent.predicted_s[i + 1, 1]) + end + + #= + if size(agent.predicted_s, 2) == 6 + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + dt * agent.predicted_s[end, 5]) + else + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + dt * agent.predicted_s[end, 4]) + end + =# + + #= + if size(agent.predicted_s, 2) == 6 + kappa[end] = get_curvature(track, agent.predicted_s[N + 1, 1]) + else + kappa[end] = get_curvature(track, agent.predicted_s[N + 1, 1]) + end + =# + + + #= + # println("Prediction before solving: ", agent.predicted_s) + for i = 1 : N - 2 + kappa[i] = get_curvature(track, agent.predicted_s[i + 2, 1]) + end + if size(agent.predicted_s, 2) == 6 + delta_s = dt * agent.predicted_s[end, 5] + kappa[end - 1] = get_curvature(track, agent.predicted_s[end, 1] + + delta_s) + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + 2 * delta_s) + else + delta_s = dt * agent.predicted_s[end, 4] + kappa[end - 1] = get_curvature(track, agent.predicted_s[end, 1] + + delta_s) + kappa[end] = get_curvature(track, agent.predicted_s[end, 1] + + 2 * delta_s) + end + =# + end + + # println("Propagated curvature: ", kappa) + + # Update current initial condition, curvature and System ID coefficients + setvalue(m.z0, zCurr) + setvalue(m.uPrev, uPrev) + setvalue(m.c_Vx, agent.theta_vx) # System ID coefficients + setvalue(m.c_Vy, agent.theta_vy) + setvalue(m.c_Psi, agent.theta_psi_dot) + setvalue(m.curvature, kappa) # Track curvature + setvalue(m.selStates, selected_states) + setvalue(m.statesCost, selected_cost) + + # Solve Problem and return solution + sol_status = solve(m.mdl) + sol_u = getvalue(m.u_Ol) + sol_z = getvalue(m.z_Ol) + + curvature_values = zeros(N + 1) + if POLYNOMIAL_CURVATURE + println("Coefficents after: ", getvalue(m.curvature)) + polynom = Poly(getvalue(m.curvature)) + for i = 1 : N + 1 + curvature_values[i] = polyval(polynom, sol_z[i, 1]) + end + println("Curvature: ", curvature_values) + end + + backward_mapping = [6, 5, 4, 3, 1, 2] + optimizer.solution_inputs = sol_u + optimizer.solution_states_s = sol_z[:, backward_mapping] + + # export data + # mpcSol.a_x = sol_u[1,1] + # mpcSol.d_f = sol_u[1,2] + # mpcSol.u = sol_u + # mpcSol.z = sol_z + #mpcSol.eps_alpha = getvalue(m.eps_alpha) + # mpcSol.solverStatus = sol_status + # mpcSol.cost = zeros(6) + # mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] + + # mpcSol.costSlack = zeros(6) + # mpcSol.costSlack = [getvalue(m.slackVx),getvalue(m.slackVy),getvalue(m.slackPsidot),getvalue(m.slackEpsi),getvalue(m.slackEy),getvalue(m.slackS)] + # println("slack values: ", [getvalue(m.slackVx), getvalue(m.slackVy), + # getvalue(m.slackPsidot), getvalue(m.slackEpsi), + # getvalue(m.slackEy), getvalue(m.slackS)]) + + println(size(getvalue(m.alpha))) + println(size(getvalue(m.selStates))) + terminal_state = sum(getvalue(m.alpha) .* getvalue(m.selStates), 1) + # println("terminal state: ", terminal_state[backward_mapping]) + + println("Solved, status = $sol_status") + + publish_prediction(optimizer) + publish_prediction_for_opponent(m, optimizer, agent) + + nothing +end + + +function solveMpcProblem_obstacle(m::MpcModel_obstacle, optimizer::Optimizer, + agent::Agent, track::Track) + # Load Parameters + sol_status::Symbol + sol_u::Array{Float64,2} + sol_z::Array{Float64,2} + + N = size(agent.optimal_inputs, 1) + kappa = zeros(N) + dt = agent.dt + + # selStates = selectedStates.selStates::Array{Float64,2} + # statesCost = selectedStates.statesCost::Array{Float64,1} + + # [s, e_y, e_psi, psi_dot, v_x, v_y] --> [v_x, v_y, psi_dot, e_psi, e_y, s] + mapping = [5, 6, 4, 3, 2, 1] + selected_states = agent.selected_states_s[:, mapping] + selected_cost = agent.selected_states_cost + + iteration = agent.current_iteration + current_s = agent.states_s[iteration, :] + s = current_s[1] + e_y = current_s[2] + e_psi = current_s[3] + psi_dot = current_s[4] + v_x = current_s[5] + v_y = current_s[6] + + # println("INITIAL STATE: ", current_s) + zCurr = [v_x, v_y, psi_dot, e_psi, e_y, s] + + u_length = 10 + u_prev = zeros(u_length, 2) + + if iteration <= u_length + u_prev[1 : iteration - 1, :] = agent.inputs[iteration - 1 : - 1 : 1, :] + current_lap = agent.current_lap + if current_lap > 1 + previous_lap = current_lap - 1 + NUM_LOADED_LAPS + needed_iter = agent.iterations_needed[previous_lap] + indeces = needed_iter : - 1 : needed_iter - (u_length - iteration) + u_prev[iteration : end, :] = squeeze(agent.previous_inputs[previous_lap, indeces, :], 1) + end + else + u_prev = agent.inputs[iteration - 1 : - 1 : iteration - u_length, :] + end + + uPrev = u_prev + + for i = 1 : N + kappa[i] = get_curvature(track, agent.predicted_s[i + 1, 1]) + end + + # Update current initial condition, curvature and System ID coefficients + setvalue(m.z0, zCurr) + setvalue(m.uPrev, uPrev) + setvalue(m.c_Vx, agent.theta_vx) # System ID coefficients + setvalue(m.c_Vy, agent.theta_vy) + setvalue(m.c_Psi, agent.theta_psi_dot) + setvalue(m.curvature, kappa) # Track curvature + setvalue(m.selStates, selected_states) + setvalue(m.statesCost, selected_cost) + + setvalue(m.weights_obstacles, agent.weights_states) + # println("ADV PREDICTIONS: ", optimizer.adv_predictions_s) + + if !agent.leading && s > optimizer.adv_predictions_s[1, 1] + optimizer.adv_predictions_s[:, 1] += track.total_length + elseif agent.leading && s < optimizer.adv_predictions_s[1, 1] + optimizer.adv_predictions_s[:, 1] -= track.total_length + end + + setvalue(m.adv_states_s, optimizer.adv_predictions_s) + setvalue(m.current_lap, agent.current_lap) + setvalue(m.adv_lap, optimizer.adv_current_lap) + + # Solve Problem and return solution + sol_status = solve(m.mdl) + sol_u = getvalue(m.u_Ol) + sol_z = getvalue(m.z_Ol) + + backward_mapping = [6, 5, 4, 3, 1, 2] + optimizer.solution_inputs = sol_u + optimizer.solution_states_s = sol_z[:, backward_mapping] + + # export data + # mpcSol.a_x = sol_u[1,1] + # mpcSol.d_f = sol_u[1,2] + # mpcSol.u = sol_u + # mpcSol.z = sol_z + #mpcSol.eps_alpha = getvalue(m.eps_alpha) + # mpcSol.solverStatus = sol_status + # mpcSol.cost = zeros(6) + # mpcSol.cost = [0,getvalue(m.terminalCost),getvalue(m.controlCost),getvalue(m.derivCost),0,getvalue(m.laneCost)] + + # mpcSol.costSlack = zeros(6) + # mpcSol.costSlack = [getvalue(m.slackVx),getvalue(m.slackVy),getvalue(m.slackPsidot),getvalue(m.slackEpsi),getvalue(m.slackEy),getvalue(m.slackS)] + # println("slack values: ", [getvalue(m.slackVx), getvalue(m.slackVy), + # getvalue(m.slackPsidot), getvalue(m.slackEpsi), + # getvalue(m.slackEy), getvalue(m.slackS)]) + + println(size(getvalue(m.alpha))) + println(size(getvalue(m.selStates))) + terminal_state = sum(getvalue(m.alpha) .* getvalue(m.selStates), 1) + # println("terminal state: ", terminal_state[backward_mapping]) + + println("Solved, status = $sol_status") + + println("obstacle cost: ", getvalue(m.obstacle_cost)) + + println("test: ", ((optimizer.adv_predictions_s[:, 1] - optimizer.solution_states_s[:, 1]) / 0.25).^2 + + ((optimizer.adv_predictions_s[:, 2] - optimizer.solution_states_s[:, 2]) / 0.25).^2 - 1) + + #= + if sol_status == :Error + exit() + end + =# + + publish_prediction(optimizer) + publish_prediction_for_opponent_obs(m, optimizer, agent) + + nothing +end + + +function solveMpcProblem_obstacle(m::MpcModel_obstacle, optimizer::Optimizer, + agent::Agent, track::Track, leading::Bool) + if leading + obstacle_weight = 1.0 * 0.1 + safety_weight = 1.0 + else + obstacle_weight = 0.5 + safety_weight = 0.5 + end + + setvalue(m.weights_obstacle, obstacle_weight) + setvalue(m.safety_weight, safety_weight) + + solveMpcProblem_obstacle(m, optimizer, agent, track) +end + +function publish_prediction_for_opponent_pf(m::MpcModel_pF, + optimizer::Optimizer, agent::Agent) + propagated_prediction = zeros(optimizer.solution_states_s) + propagated_prediction[1 : end - 1, :] = optimizer.solution_states_s[2 : end, :] + propagated_prediction[end, :] = optimizer.solution_states_s[end, :] + + optimizer.prediction_for_opp.header.stamp = get_rostime() + optimizer.prediction_for_opp.s = propagated_prediction[:, 1] + optimizer.prediction_for_opp.ey = propagated_prediction[:, 2] + optimizer.prediction_for_opp.epsi = propagated_prediction[:, 3] + optimizer.prediction_for_opp.psidot = zeros(propagated_prediction[:, 4]) + optimizer.prediction_for_opp.vx = propagated_prediction[:, 4] + optimizer.prediction_for_opp.vy = zeros(propagated_prediction[:, 4]) + optimizer.prediction_for_opp.current_lap = optimizer.agent.current_lap + + publish(optimizer.prediction_opponent_pub, optimizer.prediction_for_opp) +end + + +function publish_prediction_for_opponent(m::MpcModel_convhull, + optimizer::Optimizer, agent::Agent) + # Publish already propagated prediction for the opponent + selected_laps = agent.selected_laps + indeces = agent.closest_indeces + propagated_states = zeros(agent.selected_states_s) + + index = 1 + for i = 1 : size(selected_laps, 1) + propagated_states[index : index + NUM_HORIZONS * HORIZON - 1, :] = squeeze(agent.trajectories_s[selected_laps[i], indeces[i] : indeces[i] + NUM_HORIZONS * HORIZON - 1, :], 1) + index += NUM_HORIZONS * HORIZON + end + + propagated_state = sum(getvalue(m.alpha) .* propagated_states, 1) + propagated_prediction = zeros(optimizer.solution_states_s) + propagated_prediction[1 : end - 1, :] = optimizer.solution_states_s[2 : end, :] + + threshold = 0.1 + + if abs(propagated_state[1] - optimizer.solution_states_s[end, 1]) < abs(optimizer.solution_states_s[end - 1, 1] - optimizer.solution_states_s[end, 1]) + threshold + propagated_prediction[end, :] = propagated_state + else + propagated_prediction[end, :] = optimizer.solution_states_s[end, :] + end + + optimizer.prediction_for_opp.header.stamp = get_rostime() + optimizer.prediction_for_opp.s = propagated_prediction[:, 1] + optimizer.prediction_for_opp.ey = propagated_prediction[:, 2] + optimizer.prediction_for_opp.epsi = propagated_prediction[:, 3] + optimizer.prediction_for_opp.psidot = propagated_prediction[:, 4] + optimizer.prediction_for_opp.vx = propagated_prediction[:, 5] + optimizer.prediction_for_opp.vy = propagated_prediction[:, 6] + optimizer.prediction_for_opp.current_lap = optimizer.agent.current_lap + + publish(optimizer.prediction_opponent_pub, optimizer.prediction_for_opp) +end + +function publish_prediction_for_opponent_obs(m::MpcModel_obstacle, + optimizer::Optimizer, agent::Agent) + # Publish already propagated prediction for the opponent + selected_laps = agent.selected_laps + indeces = agent.closest_indeces + propagated_states = zeros(agent.selected_states_s) + + index = 1 + for i = 1 : size(selected_laps, 1) + propagated_states[index : index + NUM_HORIZONS * HORIZON - 1, :] = squeeze(agent.trajectories_s[selected_laps[i], indeces[i] : indeces[i] + NUM_HORIZONS * HORIZON - 1, :], 1) + index += NUM_HORIZONS * HORIZON + end + + propagated_state = sum(getvalue(m.alpha) .* propagated_states, 1) + propagated_prediction = zeros(optimizer.solution_states_s) + propagated_prediction[1 : end - 1, :] = optimizer.solution_states_s[2 : end, :] + + threshold = 0.1 + + if abs(propagated_state[1] - optimizer.solution_states_s[end, 1]) < abs(optimizer.solution_states_s[end - 1, 1] - optimizer.solution_states_s[end, 1]) + threshold + propagated_prediction[end, :] = propagated_state + else + propagated_prediction[end, :] = optimizer.solution_states_s[end, :] + end + + optimizer.prediction_for_opp.header.stamp = get_rostime() + optimizer.prediction_for_opp.s = propagated_prediction[:, 1] + optimizer.prediction_for_opp.ey = propagated_prediction[:, 2] + optimizer.prediction_for_opp.epsi = propagated_prediction[:, 3] + optimizer.prediction_for_opp.psidot = propagated_prediction[:, 4] + optimizer.prediction_for_opp.vx = propagated_prediction[:, 5] + optimizer.prediction_for_opp.vy = propagated_prediction[:, 6] + optimizer.prediction_for_opp.current_lap = optimizer.agent.current_lap + + publish(optimizer.prediction_opponent_pub, optimizer.prediction_for_opp) +end \ No newline at end of file diff --git a/workspace/src/barc/src/ekf.py b/workspace/src/barc/src/observers.py old mode 100644 new mode 100755 similarity index 64% rename from workspace/src/barc/src/ekf.py rename to workspace/src/barc/src/observers.py index 0fa7d1e7..ace3bd0f --- a/workspace/src/barc/src/ekf.py +++ b/workspace/src/barc/src/observers.py @@ -18,6 +18,51 @@ from scipy.linalg import inv import rospy +C = array([[1, 0]]) +B = eye(2) +def kinematicLuembergerObserver(vhat_x, vhat_y, w_z, a_x, a_y, v_x_enc, aph, dt): + """ + inputs: + * current longitudinal velocity estimate vhat_x [m/s] + * current lateral velocity estimate vhat_y [m/s] + * yaw rate measurement from gyroscope [rad/s] + * a_x measurement from IMU [m/s^2] + * a_y measurement from IMU [m/s^2] + * v_x estimate from encoder (v_x_enc) [m/s] + output: + * longitudinal velocity estimate vhat_x at next time step [m/s] + * lateral velocity estimate vhat_y at next time step [m/s] + + reference: + Farrelly, Jim, and Peter Wellstead. "Estimation of vehicle lateral velocity." + Control Applications, 1996. Proceedings of the 1996 IEEE International Conference + on IEEE, 1996 + Equations (25) - (31) + """ + + # compute the observer gain + # note, the reshape(-1,1) transforms the array into a n x 1 vector + K = array([ 2*aph*abs(w_z), (aph**2 - 1)*w_z ]).reshape(-1,1) + + # if car is not moving, then acclerations should be zero + if v_x_enc == 0: + a_x = 0 + a_y = 0 + vhat_x = 0 + vhat_y = 0 + + # build system matrices + A = array([[ 0, w_z], \ + [-w_z, 0 ]]) + u = array([a_x, a_y]).reshape(-1,1) + vhat_k = array([vhat_x, vhat_y]).reshape(-1,1) + + # apply observer + vhat_kp1 = vhat_k + dt*( dot( (A - dot(K,C)), vhat_k) + dot(B,u) + K*v_x_enc) + + return vhat_kp1 + + def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): """ EKF Extended Kalman Filter for nonlinear dynamic systems @@ -40,14 +85,13 @@ def ekf(f, mx_k, P_k, h, y_kp1, Q, R, args): Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" """ - - + xDim = mx_k.size # dimension of the state mx_kp1 = f(mx_k, *args) # predict next state A = numerical_jac(f, mx_k, *args) # linearize process model about current state P_kp1 = dot(dot(A,P_k),A.T) + Q # proprogate variance - my_kp1 = h(mx_kp1) # predict future output - H = numerical_jac(h, mx_kp1) # linearize measurement model about predicted next state + my_kp1 = h(mx_kp1, *args) # predict future output + H = numerical_jac(h, mx_kp1, *args) # linearize measurement model about predicted next state P12 = dot(P_kp1, H.T) # cross covariance K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain mx_kp1 = mx_kp1 + dot(K,(y_kp1 - my_kp1)) # state estimate diff --git a/workspace/src/barc/src/optimizer.jl b/workspace/src/barc/src/optimizer.jl new file mode 100755 index 00000000..d38e6a0f --- /dev/null +++ b/workspace/src/barc/src/optimizer.jl @@ -0,0 +1,1256 @@ +#!/usr/bin/env julia + +#= + File name: optimizer.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +using RobotOS +using JuMP +using Ipopt + +include("agent.jl") +include("track.jl") + + +type Optimizer + model::JuMP.Model + + init_state_s::Array{JuMP.NonlinearParameter, 1} + current_input::Array{JuMP.NonlinearParameter, 1} + + states_s::Array{JuMP.Variable, 2} + inputs::Array{JuMP.Variable, 2} + alpha::Array{JuMP.Variable} + + weights_derivative_states::Array{Float64, 1} + weights_derivative_inputs::Array{Float64, 1} + weights_states::Array{Float64, 1} + weights_inputs::Array{Float64, 1} + weights_lane::Float64 + weights_velocity::Float64 + weights_obstacles::Array{JuMP.NonlinearParameter, 1} + weights_progress::Array{Float64, 1} + weights_obstacle::JuMP.NonlinearParameter + + derivative_cost::JuMP.NonlinearExpression + path_cost::JuMP.NonlinearExpression + control_cost::JuMP.NonlinearExpression + lane_cost::JuMP.NonlinearExpression + velocity_cost::JuMP.NonlinearExpression + terminal_cost::JuMP.NonlinearExpression + obstacle_cost::JuMP.NonlinearExpression + progress_cost::JuMP.NonlinearExpression + + curvature::Array{JuMP.NonlinearParameter, 1} + reference_s::Array{JuMP.NonlinearParameter, 2} + progress::Array{JuMP.NonlinearParameter, 1} + + theta_vx::Array{JuMP.NonlinearParameter, 1} + theta_vy::Array{JuMP.NonlinearParameter, 1} + theta_psi_dot::Array{JuMP.NonlinearParameter, 1} + + selected_states::Array{JuMP.NonlinearParameter, 2} + states_cost::Array{JuMP.NonlinearParameter, 1} + + solution_inputs::Array{Float64} + solution_states_s::Array{Float64} + + solver_status::Symbol + + agent::Agent + adv_states_s::Array{JuMP.NonlinearParameter, 2} + + adv_predictions_s::Array{Float64} + adv_current_lap::Int64 + predictions_s::prediction + prediction_for_opp::prediction + first_input::ECU + adversarial::Bool + prediction_sub::RobotOS.Subscriber{barc.msg.prediction} + prediction_pub::RobotOS.Publisher{barc.msg.prediction} + prediction_opponent_pub::RobotOS.Publisher{barc.msg.prediction} + input_pub::RobotOS.Publisher{barc.msg.ECU} + + + Optimizer() = new() +end + +function init!(optimizer::Optimizer, agent::Agent, horizon::Int64) + optimizer.agent = Agent() + optimizer.agent = agent # use the agent from the simulator + + optimizer.solution_inputs = zeros(horizon, 2) + optimizer.solution_states_s = zeros(horizon + 1, 4) + + num_considered_states = size(optimizer.agent.selected_states_s)[1] + + optimizer.weights_derivative_inputs = 0.1 * [1.0, 10.0] + optimizer.weights_inputs = 2.0 * [1.0, 1.0] + optimizer.weights_lane = 10.0 * 1.0 + optimizer.weights_velocity = 1.0 + # optimizer.weights_obstacles = ones(num_considered_states) + # optimizer.weights_progress = - 300.0 * ones(horizon + 1) + optimizer.weights_progress = 0.5 * (- 1.0 * ones(horizon + 1)) + + optimizer.predictions_s = prediction() + optimizer.prediction_for_opp = prediction() + optimizer.first_input = ECU() + optimizer.adv_predictions_s = zeros(horizon + 1, 6) + optimizer.adv_predictions_s[:, 1] = 0.1 * rand(horizon + 1) + 10 * agent.index + optimizer.adv_current_lap = 0 + optimizer.adversarial = false + dummy = 0 + optimizer.prediction_sub = Subscriber("adv_prediction", prediction, + adv_prediction_callback, + (optimizer, dummy), + queue_size=1)::RobotOS.Subscriber{barc.msg.prediction} + optimizer.prediction_pub = Publisher("prediction", prediction, + queue_size=1)::RobotOS.Publisher{barc.msg.prediction} + optimizer.prediction_opponent_pub = Publisher("prediction_for_adv", prediction, + queue_size=1)::RobotOS.Publisher{barc.msg.prediction} + # optimizer.input_pub = Publisher("ecu", ECU, + # queue_size=1)::RobotOS.Publisher{barc.msg.ECU} +end + +function adv_prediction_callback(msg::prediction, optimizer::Optimizer, dummy) + optimizer.adv_predictions_s[:, 1] = msg.s + optimizer.adv_predictions_s[:, 2] = msg.ey + optimizer.adv_predictions_s[:, 3] = msg.epsi + optimizer.adv_predictions_s[:, 4] = msg.psidot + optimizer.adv_predictions_s[:, 5] = msg.vx + optimizer.adv_predictions_s[:, 6] = msg.vy + optimizer.adv_current_lap = msg.current_lap + optimizer.adversarial = true +end + +function publish_prediction(optimizer::Optimizer) + optimizer.predictions_s.header.stamp = get_rostime() + if size(optimizer.solution_states_s, 2) == 6 + optimizer.predictions_s.s = optimizer.solution_states_s[:, 1] + optimizer.predictions_s.ey = optimizer.solution_states_s[:, 2] + optimizer.predictions_s.epsi = optimizer.solution_states_s[:, 3] + optimizer.predictions_s.psidot = optimizer.solution_states_s[:, 4] + optimizer.predictions_s.vx = optimizer.solution_states_s[:, 5] + optimizer.predictions_s.vy = optimizer.solution_states_s[:, 6] + else + optimizer.predictions_s.s = optimizer.solution_states_s[:, 1] + optimizer.predictions_s.ey = optimizer.solution_states_s[:, 2] + optimizer.predictions_s.epsi = optimizer.solution_states_s[:, 3] + optimizer.predictions_s.psidot = zeros(optimizer.solution_states_s[:, 3]) + optimizer.predictions_s.vx = optimizer.solution_states_s[:, 4] + optimizer.predictions_s.vy = zeros(optimizer.solution_states_s[:, 4]) + end + optimizer.predictions_s.current_lap = optimizer.agent.current_lap + + # optimizer.first_input.motor = optimizer.solution_inputs[1, 1] + # optimizer.first_input.servo = optimizer.solution_inputs[1, 2] + + publish(optimizer.prediction_pub, optimizer.predictions_s) + # publish(optimizer.input_pub, optimizer.first_input) +end + +function init_path_following!(optimizer::Optimizer, track::Track) + # TODO: refactor init_path_following and init_learning_mpc + + optimizer.weights_derivative_states = 0.1 * [0.0, 0.0, 0.1, 0.1] + optimizer.weights_states = 1.0 * [0.0, 10.0, 0.1, 1.0] + + weights_derivative_states = optimizer.weights_derivative_states + weights_derivative_inputs = optimizer.weights_derivative_inputs + weights_states = optimizer.weights_states + weights_inputs = optimizer.weights_inputs + + dt = optimizer.agent.dt + # println("optimiziation dt: ", dt) + horizon = size(optimizer.agent.optimal_inputs)[1] + l_front = optimizer.agent.l_front + l_rear = optimizer.agent.l_rear + + iteration = optimizer.agent.current_iteration + initial_s = get_current_s(optimizer.agent) + kappa = zeros(horizon) # curvature + + optimizer.model = Model(solver=IpoptSolver(print_level=0, linear_solver="ma27", + max_cpu_time=0.09)) + + @NLparameter(optimizer.model, reference_s[i = 1 : (horizon + 1), + j = 1 : 4] == 0) + reference_input = zeros(horizon, 2) + + @variable(optimizer.model, optimizer.agent.state_s_lower_bound[j] <= + optimizer.states_s[i = 1 : (horizon + 1), j = 1 : 4] <= + optimizer.agent.state_s_upper_bound[j]) + @variable(optimizer.model, optimizer.agent.input_lower_bound[j] <= + optimizer.inputs[i = 1 : horizon, j = 1 : 2] <= + optimizer.agent.input_upper_bound[j]) + + states_s = optimizer.states_s + inputs = optimizer.inputs + # println(getvalue(states_s[1])) + + @NLparameter(optimizer.model, optimizer.current_input[i = 1 : 2] == 0) + @NLparameter(optimizer.model, optimizer.init_state_s[i = 1 : 4] == + initial_s[i]) + @NLconstraint(optimizer.model, [i = 1 : 4], + states_s[1, i] == optimizer.init_state_s[i]) + + # TODO: What's up with the track curvature in the optimization + # get_curvature does not work in optimizaion. Using different + # approach based on an estimate. Although exact curvature would be + # available + # estimate curvature + + @NLparameter(optimizer.model, curvature[i = 1 : horizon] == kappa[i]) + + @NLexpression(optimizer.model, beta[i = 1 : horizon], atan(l_rear / + (l_front + l_rear) * tan(inputs[i, 2]))) + @NLexpression(optimizer.model, state_s_dot[i = 1 : horizon], + states_s[i, 4] * cos(states_s[i, 3] + + beta[i]) / (1 - states_s[i, 2] * curvature[i])) + + # Kinematic model + #= + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * state_s_dot[i]) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + + dt * states_s[i, 4] * sin(states_s[i, 3] + beta[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + + dt * (states_s[i, 4] / l_rear * + sin(beta[i]) - state_s_dot[i] * curvature[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + dt * inputs[i, 1]) + =# + + # 4-State kinematic model + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * state_s_dot[i]) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + + dt * states_s[i, 4] * sin(states_s[i, 3] + beta[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + + dt * (states_s[i, 4] / l_rear * + sin(beta[i]) - state_s_dot[i] * curvature[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + dt * inputs[i, 1]) + + + current_input = optimizer.current_input + + # create cost function + @NLexpression(optimizer.model, derivative_cost, + sum{weights_derivative_states[j] * sum{states_s[i + 1, j] - + states_s[i, j]^2, i = 1 : horizon}, j = 1 : 4} + + sum{weights_derivative_inputs[j] * + ((current_input[j] - inputs[1, j])^2 + + sum{(inputs[i + 1, j] - inputs[i, j])^2, + i = 1 : horizon - 1}), j = 1 : 2}) + @NLexpression(optimizer.model, control_cost, 0.5 * sum{weights_inputs[j] * + (sum{(inputs[i, j] - reference_input[i, j])^2, + i = 1 : horizon}), j = 1 : 2}) + @NLexpression(optimizer.model, path_cost, 0.5 * + sum{weights_states[j] * sum{(states_s[i, j] - + reference_s[i, j])^2, i = 1 : horizon + 1}, j = 1 : 4}) + + optimizer.derivative_cost = derivative_cost + optimizer.path_cost = path_cost + optimizer.control_cost = control_cost + + optimizer.curvature = curvature + optimizer.reference_s = reference_s + + @NLobjective(optimizer.model, Min, derivative_cost + path_cost + + control_cost) +end + +function init_path_following_regression!(optimizer::Optimizer, track::Track) + # TODO: refactor init_path_following and init_learning_mpc + + # optimizer.weights_derivative_states = 0.1 * [0.0, 0.0, 0.1, 0.1, 0.1, 0.1] + # optimizer.weights_states = 1.0 * [0.0, 10.0, 1.0, 1.0, 1.0, 1.0] + optimizer.weights_derivative_states = 0.1 * [0.0, 0.0, 0.1, 0.1, 0.1] + optimizer.weights_states = 1.0 * [0.0, 10.0, 1.0, 1.0, 1.0] + + weights_derivative_states = optimizer.weights_derivative_states + weights_derivative_inputs = optimizer.weights_derivative_inputs + weights_states = optimizer.weights_states + weights_inputs = optimizer.weights_inputs + + dt = optimizer.agent.dt + # println("optimiziation dt: ", dt) + horizon = size(optimizer.agent.optimal_inputs)[1] + l_front = optimizer.agent.l_front + l_rear = optimizer.agent.l_rear + + iteration = optimizer.agent.current_iteration + # initial_s = get_current_s(optimizer.agent) + initial_s = [0.0; 0.0; 0.0; 1.0; 0.0] + kappa = zeros(horizon) # curvature + + optimizer.model = Model(solver=IpoptSolver(print_level=0, linear_solver="ma27", + max_cpu_time=0.09)) + + @NLparameter(optimizer.model, reference_s[i = 1 : (horizon + 1), j = 1 : 5] == 0) + reference_input = zeros(horizon, 2) + + state_s_lower_bound = [(- Inf) (- Inf) (- Inf) 0.0 0.0] + state_s_upper_bound = [Inf Inf Inf Inf Inf] + + @variable(optimizer.model, state_s_lower_bound[j] <= + optimizer.states_s[i = 1 : (horizon + 1), j = 1 : 5] <= + state_s_upper_bound[j]) + @variable(optimizer.model, optimizer.agent.input_lower_bound[j] <= + optimizer.inputs[i = 1 : horizon, j = 1 : 2] <= + optimizer.agent.input_upper_bound[j]) + + states_s = optimizer.states_s + inputs = optimizer.inputs + # println(getvalue(states_s[1])) + + @NLparameter(optimizer.model, optimizer.current_input[i = 1 : 2] == 0) + @NLparameter(optimizer.model, optimizer.init_state_s[i = 1 : 5] == initial_s[i]) + # @NLparameter(optimizer.model, optimizer.init_state_s[i = 1 : 5] == 0) + @NLconstraint(optimizer.model, [i = 1 : 5], + states_s[1, i] == optimizer.init_state_s[i]) + + # TODO: What's up with the track curvature in the optimization + # get_curvature does not work in optimizaion. Using different + # approach based on an estimate. Although exact curvature would be + # available + # estimate curvature + + @NLparameter(optimizer.model, curvature[i = 1 : horizon] == kappa[i]) + + # Create parameters for linear regression model + @NLparameter(optimizer.model, optimizer.theta_vx[i = 1 : 3] == 0) + @NLparameter(optimizer.model, optimizer.theta_vy[i = 1 : 4] == 0) + @NLparameter(optimizer.model, optimizer.theta_psi_dot[i = 1 : 3] == 0) + + #= + # Expressions for linear regression + @NLexpression(optimizer.model, delta_vx[i = 1 : horizon], + optimizer.theta_vx[1] * states_s[i, 6] * states_s[i, 4] + + optimizer.theta_vx[2] * states_s[i, 5] + + optimizer.theta_vx[3] * inputs[i, 1]) + @NLexpression(optimizer.model, delta_vy[i = 1 : horizon], + optimizer.theta_vy[1] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_vy[2] * states_s[i, 5] * states_s[i, 4] + + optimizer.theta_vy[3] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_vy[4] * inputs[i, 2]) + @NLexpression(optimizer.model, delta_psi_dot[i = 1 : horizon], + optimizer.theta_psi_dot[1] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_psi_dot[2] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_psi_dot[3] * inputs[i, 2 + + + # Expression for s dot + @NLexpression(optimizer.model, state_s_dot[i = 1 : horizon], + (states_s[i, 5] * cos(states_s[i, 3]) - + states_s[i, 6] * sin(states_s[i, 3])) / + (1 - states_s[i, 2] * curvature[i])) + =# + + @NLexpression(optimizer.model, beta[i = 1 : horizon], + atan(l_rear / (l_front + l_rear) * tan(inputs[i, 2]))) + @NLexpression(optimizer.model, state_s_dot[i = 1 : horizon], + (states_s[i, 4] * cos(states_s[i, 3]) - + states_s[i, 5] * sin(states_s[i, 3])) / (1 - states_s[i, 2] * curvature[i])) + + # Model constraints + # s + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * state_s_dot[i]) + # e_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + + dt * (states_s[i, 4] * sin(states_s[i, 3]) + + states_s[i, 5] * cos(states_s[i, 3]))) + # e_psi + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + + dt * (states_s[i, 4] / l_front * + sin(beta[i]) - state_s_dot[i] * curvature[i])) + # v_x + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + + # optimizer.theta_vx[2] * states_s[i, 4] + + optimizer.theta_vx[3] * inputs[i, 1]) + # @NLconstraint(optimizer.model, [i = 1 : horizon + 1], states_s[i, 4] >= 0.1) + # v_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 5] == states_s[i, 5] + + # optimizer.theta_vy[1] * states_s[i, 5] / states_s[i, 4] + + optimizer.theta_vy[4] * inputs[i, 2]) + + #= + # v_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 6] == states_s[i, 6] + optimizer.theta_vy[1] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_vy[2] * states_s[i, 5] * states_s[i, 4] + + optimizer.theta_vy[3] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_vy[4] * inputs[i, 2]) + # psi_dot + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + optimizer.theta_psi_dot[1] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_psi_dot[2] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_psi_dot[3] * inputs[i, 2]) + # s + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * (states_s[i, 5] * cos(states_s[i, 3]) - + states_s[i, 6] * sin(states_s[i, 3])) / + (1 - states_s[i, 2] * curvature[i])) + # e_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + dt * + (states_s[i, 5] * sin(states_s[i, 3]) + + states_s[i, 6] * cos(states_s[i, 3]))) + # e_psi + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + dt * (states_s[i, 4] - + curvature[i] * (states_s[i, 5] * cos(states_s[i, 3]) - + states_s[i, 6] * sin(states_s[i, 3])) / + (1 - states_s[i, 2] * curvature[i]))) + =# + + current_input = optimizer.current_input + + # create cost function + @NLexpression(optimizer.model, derivative_cost, + sum{weights_derivative_states[j] * sum{states_s[i + 1, j] - + states_s[i, j]^2, i = 1 : horizon}, j = 1 : 5} + + sum{weights_derivative_inputs[j] * + ((current_input[j] - inputs[1, j])^2 + + sum{(inputs[i + 1, j] - inputs[i, j])^2, + i = 1 : horizon - 1}), j = 1 : 2}) + @NLexpression(optimizer.model, control_cost, 0.5 * sum{weights_inputs[j] * + (sum{(inputs[i, j] - reference_input[i, j])^2, + i = 1 : horizon}), j = 1 : 2}) + @NLexpression(optimizer.model, path_cost, 0.5 * + sum{weights_states[j] * sum{(states_s[i, j] - + reference_s[i, j])^2, i = 1 : horizon + 1}, j = 1 : 5}) + + optimizer.derivative_cost = derivative_cost + optimizer.path_cost = path_cost + optimizer.control_cost = control_cost + + optimizer.curvature = curvature + optimizer.reference_s = reference_s + + @NLobjective(optimizer.model, Min, derivative_cost + path_cost + + control_cost) +end + +function init_learning_mpc!(optimizer::Optimizer, track::Track) + # TODO: Updated model from system identification + + optimizer.weights_derivative_states = 0.1 * [0.0, 0.0, 0.1, 0.1] + optimizer.weights_states = 1.0 * [0.0, 100.0, 0.1, 10.0] + + target_s = track.total_length + max_e_y = track.width / 2 + # println("MAX EY: ", max_e_y) + + # v_max = optimizer.agent.v_max + + weights_derivative_states = optimizer.weights_derivative_states + weights_derivative_inputs = optimizer.weights_derivative_inputs + weights_states = optimizer.weights_states + weights_inputs = optimizer.weights_inputs + weights_lane = optimizer.weights_lane + weights_velocity = optimizer.weights_velocity + # weights_obstacles = optimizer.weights_obstacles + + dt = optimizer.agent.dt + horizon = size(optimizer.agent.optimal_inputs)[1] + l_front = optimizer.agent.l_front + l_rear = optimizer.agent.l_rear + + iteration = optimizer.agent.current_iteration + initial_s = get_state_s(optimizer.agent, iteration) + kappa = zeros(horizon) # curvature + num_selected_states = size(optimizer.agent.selected_states_s)[1] + reference_input = zeros(horizon, 2) + + optimizer.model = Model(solver = IpoptSolver(print_level=0, linear_solver="ma27", + max_cpu_time=0.09)) + # optimizer.model = Model(solver = IpoptSolver(print_level=0, linear_solver="ma27")) + + # TODO: Bounds for velocity are wrong! + @variable(optimizer.model, optimizer.agent.state_s_lower_bound[j] <= + optimizer.states_s[i = 1 : (horizon + 1), j = 1 : 4] <= + optimizer.agent.state_s_upper_bound[j]) + @variable(optimizer.model, optimizer.agent.input_lower_bound[j] <= + optimizer.inputs[i = 1 : horizon, j = 1 : 2] <= + optimizer.agent.input_upper_bound[j]) + + states_s = optimizer.states_s + inputs = optimizer.inputs + + # @variable(optimizer.model, vel_soft[1 : (horizon + 1)] >= 0) + @variable(optimizer.model, lane_soft[1 : (horizon + 1)] >= 0) + @variable(optimizer.model, alpha[1 : num_selected_states] >= 0) + + @NLparameter(optimizer.model, optimizer.current_input[i = 1 : 2] == 0) + @NLparameter(optimizer.model, optimizer.init_state_s[i = 1 : 4] == + initial_s[i]) + @NLconstraint(optimizer.model, [i = 1 : 4], + states_s[1, i] == optimizer.init_state_s[i]) + @NLparameter(optimizer.model, curvature[i = 1 : horizon] == kappa[i]) + @NLparameter(optimizer.model, selected_states[1 : num_selected_states, + 1 : 4] == 0) + @NLparameter(optimizer.model, states_cost[1 : num_selected_states] == 0) + @NLparameter(optimizer.model, optimizer.progress[1 : horizon + 1] == 0) + @NLparameter(optimizer.model, weights_obstacles[1 : num_selected_states] == 0) + @NLparameter(optimizer.model, weights_obstacle == 0) + + #= + # No slack variables + @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 4] <= + v_max) + @NLconstraint(optimizer.model, [i = 1 : (horizon + 1)], states_s[i, 2] <= + max_e_y) + @NLconstraint(optimizer.model, [i = 1 : (horizon + 1)], states_s[i, 2] >= + - max_e_y) + =# + + # @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 4] <= + # v_max + vel_soft[i]) + @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 2] <= + max_e_y + lane_soft[i]) + @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 2] >= + - max_e_y - lane_soft[i]) + + @NLconstraint(optimizer.model, sum{alpha[i], + i = 1 : num_selected_states} == 1) + for i = 1 : 4 + @NLconstraint(optimizer.model, states_s[horizon + 1, i] == + sum{alpha[j] * selected_states[j, i], + j = 1 : num_selected_states}) + end + + @NLexpression(optimizer.model, beta[i = 1 : horizon], atan(l_rear / + (l_front + l_rear) * tan(inputs[i, 2]))) + @NLexpression(optimizer.model, state_s_dot[i = 1 : horizon], + states_s[i, 4] * cos(states_s[i, 3] + + beta[i]) / (1 - states_s[i, 2] * curvature[i])) + + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * state_s_dot[i]) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + + dt * states_s[i, 4] * sin(states_s[i, 3] + beta[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + + dt * (states_s[i, 4] / l_rear * + sin(beta[i]) - state_s_dot[i] * curvature[i])) + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + dt * inputs[i, 1]) + + current_input = optimizer.current_input + + # create cost function + @NLexpression(optimizer.model, derivative_cost, + sum{weights_derivative_states[j] * sum{states_s[i + 1, j] - + states_s[i, j]^2, i = 1 : horizon}, j = 1 : 4} + + sum{weights_derivative_inputs[j] * + ((current_input[j] - inputs[1, j])^2 + + sum{(inputs[i + 1, j] - inputs[i, j])^2, + i = 1 : horizon - 1}), j = 1 : 2}) + @NLexpression(optimizer.model, control_cost, 0.5 * sum{weights_inputs[j] * + (sum{(inputs[i, j] - reference_input[i, j])^2, + i = 1 : horizon}), j = 1 : 2}) + #= + @NLexpression(optimizer.model, path_cost, 0.5 * + sum{weights_states[j] * sum{(states_s[i, j] - + reference_s[i, j])^2, i = 1 : horizon + 1}, j = 1 : 4}) + =# + # TODO: use variables instead of magic numbers + @NLexpression(optimizer.model, lane_cost, weights_lane * sum{10.0 * + lane_soft[i] + 100.0 * lane_soft[i], i = 2 : (horizon + 1)}) + @NLexpression(optimizer.model, terminal_cost, sum{weights_obstacles[i] * + alpha[i] * states_cost[i], i = 1 : num_selected_states}) + # @NLexpression(optimizer.model, velocity_cost, weights_velocity * sum{10.0 * + # vel_soft[i] + 100 * vel_soft[i], i = 2 : (horizon + 1)}) + + num_adv_agents = 0 + + try + adv_agents = optimizer.adversarial_agents + num_adv_agents = NUM_AGENTS - 1 + println("There are adversarial agents") + catch + num_adv_agents = 0 + println("There are no adversarial agents") + end + + if NUM_AGENTS > 1 + iteration = agent.current_iteration + agent = optimizer.agent + radius_s = zeros(num_adv_agents) + radius_e_y = zeros(num_adv_agents) + + @NLparameter(optimizer.model, + optimizer.adv_states_s[1 : (horizon + 1), 1 : 4] == 0) + + # Asumming that both agents have the same size + for i = 1 : num_adv_agents + radius_s[i] = 2 * agent.l_front + radius_e_y[i] = 2 * agent.width + end + + # TODO: Consider rotation of the opponent? + @NLexpression(optimizer.model, obstacle_cost, weights_obstacle * sum{- log( + ((states_s[j, 1] - optimizer.adv_states_s[j, 1]) / + radius_s[1])^2 + ((states_s[j, 2] - + optimizer.adv_states_s[j, 2]) / radius_e_y[1])^2 - 1), + j = 1 : (horizon + 1)}) + + optimizer.obstacle_cost = obstacle_cost + + @NLexpression(optimizer.model, progress_cost, sum{optimizer.weights_progress[i] * + optimizer.progress[i], i = 1 : horizon + 1}) + + optimizer.progress_cost = progress_cost + # println(optimizer.adv_states_s) + # optimizer.adv_states_s = adv_states_s + else + println("Initialized without adverasarial agents") + end + + optimizer.derivative_cost = derivative_cost + # optimizer.path_cost = path_cost + optimizer.control_cost = control_cost + optimizer.lane_cost = lane_cost + optimizer.terminal_cost = terminal_cost + # optimizer.velocity_cost = velocity_cost + + optimizer.curvature = curvature + # optimizer.reference_s = reference_s + optimizer.selected_states = selected_states + optimizer.states_cost = states_cost + optimizer.weights_obstacle = weights_obstacle + + optimizer.alpha = alpha + + try + optimizer.weights_obstacles = weights_obstacles + obstacle_cost = optimizer.obstacle_cost + @NLobjective(optimizer.model, Min, derivative_cost + # path_cost + + control_cost + lane_cost + terminal_cost + velocity_cost + + obstacle_cost + progress_cost) + # println("applying obstacle_cost") + catch + @NLobjective(optimizer.model, Min, derivative_cost + # path_cost + + control_cost + lane_cost + terminal_cost) # + velocity_cost) + end +end + +function init_lmpc_regression!(optimizer::Optimizer, track::Track) + + optimizer.weights_derivative_states = 0.1 * [0.0, 0.0, 0.1, 0.1, 0.1, 0.1] + optimizer.weights_states = 1.0 * [0.0, 100.0, 0.1, 0.1, 10.0, 10.0] + + target_s = track.total_length + max_e_y = track.width / 2 + # v_max = optimizer.agent.v_max + + weights_derivative_states = optimizer.weights_derivative_states + weights_derivative_inputs = optimizer.weights_derivative_inputs + weights_states = optimizer.weights_states + weights_inputs = optimizer.weights_inputs + weights_lane = optimizer.weights_lane + weights_velocity = optimizer.weights_velocity + # weights_obstacles = optimizer.weights_obstacles + + dt = optimizer.agent.dt + horizon = size(optimizer.agent.optimal_inputs)[1] + l_front = optimizer.agent.l_front + l_rear = optimizer.agent.l_rear + + iteration = optimizer.agent.current_iteration + initial_s = get_state_s(optimizer.agent, iteration) + kappa = zeros(horizon) # curvature + num_selected_states = size(optimizer.agent.selected_states_s)[1] + reference_input = zeros(horizon, 2) + + optimizer.model = Model(solver = IpoptSolver(print_level=0, linear_solver="ma27", + max_cpu_time=0.09)) + # optimizer.model = Model(solver = IpoptSolver(print_level=0, linear_solver="ma27")) + + # TODO: Bounds for velocity are wrong! + @variable(optimizer.model, optimizer.agent.state_s_lower_bound[j] <= + optimizer.states_s[i = 1 : (horizon + 1), j = 1 : 6] <= + optimizer.agent.state_s_upper_bound[j]) + @variable(optimizer.model, optimizer.agent.input_lower_bound[j] <= + optimizer.inputs[i = 1 : horizon, j = 1 : 2] <= + optimizer.agent.input_upper_bound[j]) + + states_s = optimizer.states_s + inputs = optimizer.inputs + + # @variable(optimizer.model, vel_soft[1 : (horizon + 1)] >= 0) + @variable(optimizer.model, lane_soft[1 : (horizon + 1)] >= 0) + @variable(optimizer.model, alpha[1 : num_selected_states] >= 0) + + @NLparameter(optimizer.model, optimizer.current_input[i = 1 : 2] == 0) + @NLparameter(optimizer.model, optimizer.init_state_s[i = 1 : 6] == + initial_s[i]) + @NLconstraint(optimizer.model, [i = 1 : 6], + states_s[1, i] == optimizer.init_state_s[i]) + @NLparameter(optimizer.model, curvature[i = 1 : horizon] == kappa[i]) + @NLparameter(optimizer.model, selected_states[1 : num_selected_states, + 1 : 6] == 0) + @NLparameter(optimizer.model, states_cost[1 : num_selected_states] == 0) + @NLparameter(optimizer.model, optimizer.progress[1 : horizon + 1] == 0) + @NLparameter(optimizer.model, weights_obstacles[1 : num_selected_states] == 0) + @NLparameter(optimizer.model, weights_obstacle == 0) + + # @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 5] <= + # v_max + vel_soft[i]) + @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 2] <= + max_e_y + lane_soft[i]) + @NLconstraint(optimizer.model, [i = 2 : (horizon + 1)], states_s[i, 2] >= + - max_e_y - lane_soft[i]) + + @NLconstraint(optimizer.model, sum{alpha[i], + i = 1 : num_selected_states} == 1) + for i = 1 : 6 + @NLconstraint(optimizer.model, states_s[horizon + 1, i] == + sum{alpha[j] * selected_states[j, i], + j = 1 : num_selected_states}) + end + + # Create parameters for linear regression model + @NLparameter(optimizer.model, optimizer.theta_vx[i = 1 : 3] == 0) + @NLparameter(optimizer.model, optimizer.theta_vy[i = 1 : 4] == 0) + @NLparameter(optimizer.model, optimizer.theta_psi_dot[i = 1 : 3] == 0) + + # Expressions for linear regression + @NLexpression(optimizer.model, delta_vx[i = 1 : horizon], + optimizer.theta_vx[1] * states_s[i, 6] * states_s[i, 4] + + optimizer.theta_vx[2] * states_s[i, 5] + + optimizer.theta_vx[3] * inputs[i, 1]) + @NLexpression(optimizer.model, delta_vy[i = 1 : horizon], + optimizer.theta_vy[1] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_vy[2] * states_s[i, 5] * states_s[i, 4] + + optimizer.theta_vy[3] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_vy[4] * inputs[i, 2]) + @NLexpression(optimizer.model, delta_psi_dot[i = 1 : horizon], + optimizer.theta_psi_dot[1] * states_s[i, 4] / states_s[i, 5] + + optimizer.theta_psi_dot[2] * states_s[i, 6] / states_s[i, 5] + + optimizer.theta_psi_dot[3] * inputs[i, 2]) + + # Expression for s dot + @NLexpression(optimizer.model, state_s_dot[i = 1 : horizon], + (states_s[i, 5] * cos(states_s[i, 3]) - + states_s[i, 6] * sin(states_s[i, 3])) / + (1 - states_s[i, 2] * curvature[i])) + + # Model constraints + # v_x + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 5] == states_s[i, 5] + delta_vx[i]) + # v_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 6] == states_s[i, 6] + delta_vy[i]) + # psi_dot + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 4] == states_s[i, 4] + delta_psi_dot[i]) + # s + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 1] == states_s[i, 1] + dt * state_s_dot[i]) + # e_y + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 2] == states_s[i, 2] + dt * + (states_s[i, 5] * sin(states_s[i, 3]) + + states_s[i, 6] * cos(states_s[i, 3]))) + # e_psi + @NLconstraint(optimizer.model, [i = 1 : horizon], + states_s[i + 1, 3] == states_s[i, 3] + dt * (states_s[i, 4] - + curvature[i] * state_s_dot[i])) + + current_input = optimizer.current_input + + # create cost function + @NLexpression(optimizer.model, derivative_cost, + sum{weights_derivative_states[j] * sum{states_s[i + 1, j] - + states_s[i, j]^2, i = 1 : horizon}, j = 1 : 6} + + sum{weights_derivative_inputs[j] * + ((current_input[j] - inputs[1, j])^2 + + sum{(inputs[i + 1, j] - inputs[i, j])^2, + i = 1 : horizon - 1}), j = 1 : 2}) + @NLexpression(optimizer.model, control_cost, 0.5 * sum{weights_inputs[j] * + (sum{(inputs[i, j] - reference_input[i, j])^2, + i = 1 : horizon}), j = 1 : 2}) + + # TODO: use variables instead of magic numbers + @NLexpression(optimizer.model, lane_cost, weights_lane * sum{10.0 * + lane_soft[i] + 100.0 * lane_soft[i], i = 2 : (horizon + 1)}) + @NLexpression(optimizer.model, terminal_cost, sum{weights_obstacles[i] * + alpha[i] * states_cost[i], i = 1 : num_selected_states}) + # @NLexpression(optimizer.model, velocity_cost, weights_velocity * sum{10.0 * + # vel_soft[i] + 100 * vel_soft[i], i = 2 : (horizon + 1)}) + + num_adv_agents = 0 + + try + adv_agents = optimizer.adversarial_agents + num_adv_agents = NUM_AGENTS - 1 + println("There are adversarial agents") + catch + num_adv_agents = 0 + println("There are no adversarial agents") + end + + if NUM_AGENTS > 1 + iteration = agent.current_iteration + agent = optimizer.agent + radius_s = zeros(num_adv_agents) + radius_e_y = zeros(num_adv_agents) + + @NLparameter(optimizer.model, + optimizer.adv_states_s[1 : (horizon + 1), 1 : 6] == 0) + + # Asumming that both agents have the same size + for i = 1 : num_adv_agents + radius_s[i] = 2 * agent.l_front + radius_e_y[i] = 2 * agent.width + end + + # TODO: Consider rotation of the opponent? + @NLexpression(optimizer.model, obstacle_cost, weights_obstacle * sum{- log( + ((states_s[j, 1] - optimizer.adv_states_s[j, 1]) / + radius_s[1])^2 + ((states_s[j, 2] - + optimizer.adv_states_s[j, 2]) / radius_e_y[1])^2 - 1), + j = 1 : (horizon + 1)}) + + optimizer.obstacle_cost = obstacle_cost + + @NLexpression(optimizer.model, progress_cost, sum{optimizer.weights_progress[i] * + optimizer.progress[i], i = 1 : horizon + 1}) + + optimizer.progress_cost = progress_cost + # println(optimizer.adv_states_s) + # optimizer.adv_states_s = adv_states_s + else + println("Initialized without adverasarial agents") + end + + optimizer.derivative_cost = derivative_cost + # optimizer.path_cost = path_cost + optimizer.control_cost = control_cost + optimizer.lane_cost = lane_cost + optimizer.terminal_cost = terminal_cost + # optimizer.velocity_cost = velocity_cost + + optimizer.curvature = curvature + # optimizer.reference_s = reference_s + optimizer.selected_states = selected_states + optimizer.states_cost = states_cost + optimizer.weights_obstacle = weights_obstacle + + optimizer.alpha = alpha + + try + optimizer.weights_obstacles = weights_obstacles + obstacle_cost = optimizer.obstacle_cost + @NLobjective(optimizer.model, Min, derivative_cost + # path_cost + + control_cost + lane_cost + terminal_cost + velocity_cost + + obstacle_cost + progress_cost) + # println("applying obstacle_cost") + catch + @NLobjective(optimizer.model, Min, derivative_cost + # path_cost + + control_cost + lane_cost + terminal_cost) # + velocity_cost) + end + +end + +function solve_path_following!(optimizer::Optimizer, track::Track, + reference::Array{Float64}) + # TODO: refactor solve_path_following and solve_learning_mpc! + agent = optimizer.agent + # println("STATE INITIALIZED: ", agent.state_initialized) + iteration = agent.current_iteration + horizon = size(optimizer.solution_inputs)[1] + kappa = zeros(horizon) + dt = optimizer.agent.dt + + for i = 1 : horizon - 1 + kappa[i] = get_curvature(track, + optimizer.agent.predicted_s[i + 1, 1]) + end + kappa[end] = get_curvature(track, optimizer.agent.predicted_s[end, 1] + + dt * optimizer.agent.predicted_s[end, 4]) + # println("kappa: ", kappa) + init_state = zeros(4) + init_state[1 : 3] = agent.states_s[iteration, 1 : 3] + init_state[4] = sqrt(agent.states_s[iteration, 5]^2 + agent.states_s[iteration, 6]^2) + println("INITIAL STATE: ", init_state) + + setvalue(optimizer.init_state_s, init_state) + setvalue(optimizer.current_input, agent.inputs[iteration, :]') + setvalue(optimizer.curvature, kappa) + setvalue(optimizer.reference_s, ones(horizon + 1, 1) .* reference) + + solver_status = solve(optimizer.model) + + optimizer.solution_inputs = getvalue(optimizer.inputs) + optimizer.solution_states_s = getvalue(optimizer.states_s) + # optimizer.solution_inputs = repmat([input_upper_bound 0.0], horizon) + + publish_prediction(optimizer) +end + +function pacejka(a) + B = 1.0 # This value determines the steepness of the curve + C = 1.25 + mu = 0.8 # Friction coefficient (responsible for maximum lateral tire force) + m = 1.98 + g = 9.81 + D = mu * m * g/2 + C_alpha_f = D*sin(C*atan(B*a)) + return C_alpha_f +end + +function solve_path_following_regression!(optimizer::Optimizer, track::Track, + reference::Array{Float64}) + # TODO: refactor solve_path_following and solve_learning_mpc! + agent = optimizer.agent + # println("STATE INITIALIZED: ", agent.state_initialized) + iteration = agent.current_iteration + horizon = size(optimizer.solution_inputs)[1] + kappa = zeros(horizon) + dt = optimizer.agent.dt + + for i = 1 : horizon - 1 + kappa[i] = get_curvature(track, + optimizer.agent.predicted_s[i + 1, 1]) + end + if size(optimizer.agent.predicted_s, 2) == 6 + kappa[end] = get_curvature(track, optimizer.agent.predicted_s[end, 1] + + dt * optimizer.agent.predicted_s[end, 5]) + else + kappa[end] = get_curvature(track, optimizer.agent.predicted_s[end, 1] + + dt * optimizer.agent.predicted_s[end, 4]) + end + # println("kappa: ", kappa) + + # x : z[1] + # y : z[2] + # v_x : z[3] + # v_y : z[4] + # psi : z[5] + # psiDot : z[6] + # a : z[7] + # d_f : z[8] + + l_rear = agent.l_rear + l_front = agent.l_front + current_state_s = agent.states_s[iteration, :] + s, e_y, e_psi, psi_dot, v_x, v_y = current_state_s + + # Determine tire forces + a_F = 0.0 + a_R = 0.0 + if abs(v_x) > 0.2 + a_F = atan((v_y + l_front * psi_dot) / abs(v_x)) - agent.current_input[2] + a_R = atan((v_y - l_rear * psi_dot) / abs(v_x)) + end + + FyF = -pacejka(a_F) + FyR = -pacejka(a_R) + + # agent.theta_vx = [-0.05 0.0 0.1]' + # agent.theta_vy = [-0.05 -0.01 0.0 -0.05]' + # agent.theta_psi_dot = [0.0 1.0 0.2]' + # Set parameters for dynamic model + # setvalue(optimizer.theta_vx, [0.1 0.0 0.1]') + # setvalue(optimizer.theta_vy, [-0.4 0.0 0.0 0.0]') + # setvalue(optimizer.theta_psi_dot, [-0.5 0.5 2.0]') + # setvalue(optimizer.theta_vx, agent.theta_vx) + # setvalue(optimizer.theta_vy, agent.theta_vy) + # setvalue(optimizer.theta_psi_dot, agent.theta_psi_dot) + + # init_state = zeros(5) + # init_state[1 : 3] = agent.states_s[iteration, 1 : 3] + # init_state[4 : 5] = agent.states_s[iteration, 5 : 6] + ## init_state[4] = sqrt(agent.states_s[iteration, 5]^2 + agent.states_s[iteration, 6]^2) + # println("INITIAL STATE: ", init_state) + # setvalue(optimizer.init_state_s, init_state) + + println("INITIAL STATE: ", agent.states_s[iteration, :]) + setvalue(optimizer.init_state_s, current_state_s') + + println("CURRENT INPUT: ", agent.inputs[max(iteration - 1, 1), :]) + setvalue(optimizer.current_input, agent.inputs[max(iteration - 1, 1), :]') + setvalue(optimizer.curvature, kappa) + setvalue(optimizer.reference_s, ones(horizon + 1, 1) .* reference) + + solver_status = solve(optimizer.model) + + optimizer.solution_inputs = getvalue(optimizer.inputs) + optimizer.solution_states_s = getvalue(optimizer.states_s) + + # if solver_status == :Infeasible && iteration > 10 + # exit() + # end + + # optimizer.solution_inputs = repmat([input_upper_bound 0.0], horizon) + + # publish_prediction(optimizer) +end + + +function solve_learning_mpc!(optimizer::Optimizer, track::Track) + # determines the optimal inputs and also states in s + agent = optimizer.agent + iteration = agent.current_iteration + horizon = size(optimizer.solution_inputs)[1] + kappa = zeros(horizon) + dt = optimizer.agent.dt + + for i = 1 : horizon - 1 + kappa[i] = get_curvature(track, + optimizer.agent.predicted_s[i + 1, 1]) + end + kappa[end] = get_curvature(track, optimizer.agent.predicted_s[end, 1] + + dt * optimizer.agent.predicted_s[end, 4]) + + init_state = zeros(4) + init_state[1 : 3] = agent.states_s[iteration, 1 : 3] + init_state[4] = sqrt(agent.states_s[iteration, 5]^2 + agent.states_s[iteration, 6]^2) + println("INITIAL STATE: ", init_state) + + setvalue(optimizer.init_state_s, init_state) + + # setvalue(optimizer.init_state_s, agent.states_s[iteration, :]') + setvalue(optimizer.current_input, agent.inputs[iteration, :]') + setvalue(optimizer.curvature, kappa) + setvalue(optimizer.states_cost, agent.selected_states_cost) + + selected_states = zeros(NUM_CONSIDERED_STATES, 4) + selected_states[:, 1 : 3] = agent.selected_states_s[:, 1 : 3] + selected_states[:, 4] = sqrt(agent.selected_states_s[:, 5].^2 + + agent.selected_states_s[:, 6].^2) + setvalue(optimizer.selected_states, selected_states) + + setvalue(optimizer.weights_obstacles, agent.weights_states) + + if NUM_AGENTS > 1 + propagate_adv_prediction!(optimizer) + setvalue(optimizer.adv_states_s, optimizer.adv_predictions_s) + distance = get_total_distance(optimizer, track.total_length) + setvalue(optimizer.progress, vec(distance)) + end + + solver_status = solve(optimizer.model) + + optimizer.solution_inputs = getvalue(optimizer.inputs) + optimizer.solution_states_s = getvalue(optimizer.states_s) + + # Testing maximum acceleration and straight steering + # input_upper_bound = optimizer.agent.input_upper_bound[1] + # optimizer.solution_inputs = repmat([input_upper_bound 0.0], horizon) + + #= + if (solver_status == :Infeasible || solver_status == :UserLimit) && MODE == "learning" + input_lower_bound = optimizer.agent.input_lower_bound[1] + optimizer.solution_inputs = repmat([input_lower_bound 0.0], horizon) + # optimizer.solution_states_s = simulate_s_kin(optimizer.agent, track, + # optimizer.solution_inputs) + else + optimizer.solution_inputs = getvalue(optimizer.inputs) + end + + optimizer.solution_states_s = getvalue(optimizer.states_s) + =# + + alpha = getvalue(optimizer.alpha) + + publish_prediction(optimizer) +end + +function solve_learning_mpc!(optimizer::Optimizer, track::Track, leading::Bool) + if leading + obstacle_weight = 1.0 * 0.01 + else + obstacle_weight = 0.1 + end + + setvalue(optimizer.weights_obstacle, obstacle_weight) + + solve_learning_mpc!(optimizer, track) +end + +function solve_lmpc_regression!(optimizer::Optimizer, track::Track) + # determines the optimal inputs and also states in s + agent = optimizer.agent + iteration = agent.current_iteration + horizon = size(optimizer.solution_inputs)[1] + kappa = zeros(horizon) + dt = optimizer.agent.dt + + for i = 1 : horizon - 1 + kappa[i] = get_curvature(track, + optimizer.agent.predicted_s[i + 1, 1]) + end + kappa[end] = get_curvature(track, optimizer.agent.predicted_s[end, 1] + + dt * optimizer.agent.predicted_s[end, 5]) + + # Set parameters for dynamic model + setvalue(optimizer.theta_vx, agent.theta_vx) + setvalue(optimizer.theta_vy, agent.theta_vy) + setvalue(optimizer.theta_psi_dot, agent.theta_psi_dot) + + println("INITIAL STATE: ", agent.states_s[iteration, :]) + + setvalue(optimizer.init_state_s, agent.states_s[iteration, :]') + setvalue(optimizer.current_input, agent.inputs[iteration, :]') + setvalue(optimizer.curvature, kappa) + setvalue(optimizer.states_cost, agent.selected_states_cost) + setvalue(optimizer.selected_states, agent.selected_states_s) + + # setvalue(optimizer.weights_obstacles, agent.weights_states) + + if NUM_AGENTS > 1 + propagate_adv_prediction!(optimizer) + setvalue(optimizer.adv_states_s, optimizer.adv_predictions_s) + distance = get_total_distance(optimizer, track.total_length) + setvalue(optimizer.progress, vec(distance)) + end + + solver_status = solve(optimizer.model) + + optimizer.solution_inputs = getvalue(optimizer.inputs) + optimizer.solution_states_s = getvalue(optimizer.states_s) + + # Testing maximum acceleration and straight steering + # input_upper_bound = optimizer.agent.input_upper_bound[1] + # optimizer.solution_inputs = repmat([input_upper_bound 0.0], horizon) + + #= + if (solver_status == :Infeasible || solver_status == :UserLimit) && MODE == "learning" + input_lower_bound = optimizer.agent.input_lower_bound[1] + optimizer.solution_inputs = repmat([input_lower_bound 0.0], horizon) + # optimizer.solution_states_s = simulate_s_kin(optimizer.agent, track, + # optimizer.solution_inputs) + else + optimizer.solution_inputs = getvalue(optimizer.inputs) + end + + optimizer.solution_states_s = getvalue(optimizer.states_s) + =# + + alpha = getvalue(optimizer.alpha) + + publish_prediction(optimizer) +end + +function solve_lmpc_regression!(optimizer::Optimizer, track::Track, leading::Bool) + if leading + obstacle_weight = 1.0 * 0.01 + else + obstacle_weight = 0.1 + end + + setvalue(optimizer.weights_obstacle, obstacle_weight) + + solve_lmpc_regression!(optimizer, track) +end + + +function warm_start_solution(optimizer::Optimizer, track_length) + shifted_solution = optimizer.agent.predicted_s + shifted_solution[:, 1] = optimizer.agent.predicted_s[:, 1] - track_length + # shifted_solution[:, 2] = zeros(shifted_solution[:, 2]) + shifted_solution[1, 1] = 0.0 + setvalue(optimizer.states_s, shifted_solution) + setvalue(optimizer.inputs, getvalue(optimizer.inputs)) +end + +function get_total_distance(optimizer::Optimizer, track_length::Int64) + # calculate the distance on the track + num_adv_agents = NUM_AGENTS - 1 + @assert num_adv_agents >= 1 + + horizon = size(optimizer.agent.predicted_s)[1] - 1 + current_laps = zeros(num_adv_agents + 1) + s_coords = zeros(num_adv_agents + 1, horizon + 1) + progress_on_track = zeros(num_adv_agents + 1, horizon + 1) + + for j = 1 : horizon + 1 + s_coords[1, j] = optimizer.agent.predicted_s[j, 1] + current_laps[1] = optimizer.agent.current_lap + progress_on_track[1, j] = (current_laps[1] - 1) * track_length + s_coords[1, j] + + for i = 1 : num_adv_agents + s_coords[i + 1, j] = optimizer.adv_predictions_s[j, 1] + current_laps[i + 1] = optimizer.adv_current_lap + progress_on_track[i + 1, j] = (current_laps[i + 1] - 1) * track_length + s_coords[i + 1, j] + end + end + + # println("Progress on track: ", progress_on_track) + # println(size(progress_on_track)) + + return sum(progress_on_track .* (repmat([1; - 1]', horizon + 1))', 1) +end + +function adjust_convex_hull(agent::Agent, optimizer::Optimizer) + + selected_states_s = agent.selected_states_s + num_selected_states = size(selected_states_s)[1] + horizon = size(optimizer.adv_predictions_s)[1] - 1 + + dummy_weights = ones(size(selected_states_s)[1]) + for i = 1 : NUM_AGENTS - 1 + adv_predictions_s = optimizer.adv_predictions_s + r_s = 2 * agent.l_front + r_e_y = 2 * agent.width / 2 + for j = 1 : num_selected_states + evaluate_ellipse = ((selected_states_s[j, 1] - + adv_predictions_s[end, 1]) / r_s).^2 + + ((selected_states_s[j, 2] - + adv_predictions_s[end, 2]) / r_e_y).^2 + if evaluate_ellipse <= 1 + agent.weights_states[j] = 10 + end + end + end + + sorted_weights = sortperm(agent.weights_states) + max_weight = findmax(agent.weights_states[sorted_weights]) + + if max_weight[1] > 1 + adjusted_weights_indeces = max_weight[2] : num_selected_states + # 5 * 2 * horizon == num_selected_states + for i = 1 : 2 * horizon : num_selected_states + sum_weights = sum(agent.weights_states[i : i + 2 * horizon - 1]) + if sum_weights > 2 * horizon # we know there are adjusted states + max_index = findmax(agent.weights_states[i : i + 2 * horizon - 1])[2] + # println(max_index) + agent.weights_states[i + max_index : i + 2 * horizon - 1] = + 10 * ones(agent.weights_states[i + max_index : i + 2 * horizon - 1]) + end + end + end +end + + +function propagate_adv_prediction!(optimizer::Optimizer, agent::Agent) + # TODO: Change this using alpha!! + try + for i = 1 : NUM_AGENTS - 1 + optimizer.adv_predictions_s[1 : end - 1, :] = optimizer.adv_predictions_s[2 : end, :] + last_pred_s = optimizer.adv_predictions_s[end, :] + optimizer.adv_predictions_s[end, :] = last_pred_s + + optimizer.adv_predictions_s[end, 1] += last_pred_s[end - 1] * agent.dt + end + end +end diff --git a/workspace/src/barc/src/pid.py b/workspace/src/barc/src/pid.py deleted file mode 100644 index 3b8d80ae..00000000 --- a/workspace/src/barc/src/pid.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -class PID: - def __init__(self, P=2.0, I=0.0, D=0, de=0, e_int = 0, Integrator_max=500, Integrator_min=-500): - self.Kp = P # proportional gain - self.Ki = I # integral gain - self.Kd = D # derivative gain - - self.set_point = 0 # reference point - self.e = 0 - - self.e_int = 0 - self.int_e_max = Integrator_max - self.int_e_min = Integrator_min - - self.current_value = 0 - - def update(self,current_value, dt): - self.current_value = current_value - - e_t = self.set_point - current_value - de_t = ( e_t - self.e)/dt - - self.e_int = self.e_int + e_t * dt - if self.e_int > self.int_e_max: - self.e_int = self.int_e_max - elif self.e_int < self.int_e_min: - self.e_int = self.int_e_min - - P_val = self.Kp * e_t - I_val = self.Ki * self.e_int - D_val = self.Kd * de_t - - PID = P_val + I_val + D_val - self.e = e_t - - return PID - - def setPoint(self,set_point): - self.set_point = set_point - # reset error integrator - self.e_int = 0 - # reset error, otherwise de/dt will skyrocket - self.e = set_point - self.current_value - - def setKp(self,P): - self.Kp=P - - def setKi(self,I): - self.Ki=I - - def setKd(self,D): - self.Kd=D - - def getPoint(self): - return self.set_point - - def getError(self): - return self.e - -#%% Example function -def fx(x, u, dt): - x_next = x + (3*x + u) * dt - return x_next - -#%% Test script to ensure program is functioning properly -if __name__ == "__main__": - - from numpy import zeros - import matplotlib.pyplot as plt - - n = 200 - x = zeros(n) - x[0] = 20 - pid = PID(P=3.7, I=5, D=0.5) - dt = 0.1 - - for i in range(n-1): - u = pid.update(x[i], dt) - x[i+1] = fx(x[i],u, dt) - - plt.plot(x) - plt.show() diff --git a/workspace/src/barc/src/plotter.py b/workspace/src/barc/src/plotter.py new file mode 100755 index 00000000..83266e63 --- /dev/null +++ b/workspace/src/barc/src/plotter.py @@ -0,0 +1,527 @@ +#!/usr/bin/env python + +''' + File name: plotter.py + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Python Version: 2.7.12 +''' + +import rospy +from barc.msg import xy_prediction, pos_info, theta, selected_states +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos_ang, hedge_pos +import numpy as np +from numpy import linalg as LA +import matplotlib.pyplot as plt +import matplotlib.patches as patch +import matplotlib.transforms as transforms +import matplotlib.animation as animation +import matplotlib as mpl +import matplotlib.collections as collections +import pdb + +from track import Track +from transformations import rotate_around_z, s_to_xy, xy_to_s + +HORIZON = 10 +NUM_CONSIDERED_STATES = 100 +NUM_LAPS = 10 + + +def set_transformation(axis, angle, translate): + # Try to rotate rectangle using matplotlib's transformations + t1 = mpl.transforms.Affine2D() + t1.rotate(angle) + t1.translate(translate[0], translate[1]) + + # Apparently one also has to transform between data coordinate + # system and display coordinate system + t2 = axis.transData + t3 = t1.__add__(t2) + + return t3 + + +class PlottedAgent: + wheel_length = 0.04 + wheel_width = 0.02 + bumper = 0.01 + l_front = 0.125 + l_rear = 0.125 + width = 0.1 + + x_gps = 0.0 + y_gps = 0.0 + + center_of_mass = np.array([0.0, 0.0]) + psi = np.array(0.0) + psi_and_steering = np.array([0.0]) + front = np.array([0.0, 0.0]) + rear = np.array([0.0, 0.0]) + center_front_wheel = np.array([[0.0, 0.0], [0.0, 0.0]]) + center_rear_wheel = np.array([[0.0, 0.0], [0.0, 0.0]]) + center_front_wheel_left = np.array([0.0, 0.0]) + center_front_wheel_right = np.array([0.0, 0.0]) + center_rear_wheel_left = np.array([0.0, 0.0]) + center_rear_wheel_right = np.array([0.0, 0.0]) + + trajectory_xy = np.zeros((500, 2)) + trajectory_gps_xy = np.zeros((5000, 2)) + traj_counter = 0 + gps_counter = 0 + traj_gps_counter = 0 + prediction_available = False + + num_points = 500 + theta_vx = np.zeros((num_points, 3)) + theta_vy = np.zeros((num_points, 4)) + theta_psi_dot = np.zeros((num_points, 3)) + + theta_counter = 0 + + selection_x = np.zeros((NUM_CONSIDERED_STATES)) + selection_y = np.zeros((NUM_CONSIDERED_STATES)) + + # track = Track(ds=0.1, shape="test", width=1.0) + + steering_angles = np.zeros(500) + s = np.linspace(start=0.0, stop=23.9, num=500) + counter = 0 + + time_start = None + + def __init__(self, color_string, ax, track): + node_name = color_string.split("/")[0] + self.node_name = node_name + sim_string = rospy.get_param(rospy.get_name() + "/" + node_name) + if sim_string == "simulation": + xy_sub = rospy.Subscriber(node_name + "/real_val", pos_info, self.xy_callback) + else: + xy_sub = rospy.Subscriber(node_name + "/pos_info", pos_info, self.xy_callback) + + # xy_gps_sub = rospy.Subscriber(node_name + "/hedge_imu_fusion", hedge_imu_fusion, self.xy_gps_callback) + xy_gps_sub = rospy.Subscriber(node_name + "/hedge_pos", hedge_pos, self.xy_gps_callback) + prediction_sub = rospy.Subscriber(node_name + "/xy_prediction", xy_prediction, self.prediction_callback) + selection_sub = rospy.Subscriber(node_name + "/selected_states", selected_states, self.selection_callback) + + # theta_sub = rospy.Subscriber("theta", theta, self.theta_callback) + + self.track = track + + # self.color = color + self.color = rospy.get_param(color_string) + if self.color == "blue": + self.dot = "bo" + self.star = "b*" + self.line = "b-" + self.dot_extra = "co" + self.line_extra = "c-" + elif self.color == "red": + self.dot = "ro" + self.star = "r*" + self.line = "r-" + self.dot_extra = "mo" + self.line_extra = "m-" + else: + ValueError("Color %s not defined" %self.color) + + print (self.color) + + self.selected_states_plot, = ax.plot(self.selection_x, self.selection_y, + self.dot_extra) + + self.agent_length = self.l_front + self.l_rear + + self.plt_front, = ax.plot([self.center_of_mass[0], self.front[0]], + [self.center_of_mass[1], self.front[1]], "k-", lw=3.0) + self.plt_rear, = ax.plot([self.center_of_mass[0], self.rear[0]], + [self.center_of_mass[1], self.rear[1]], "k-", lw=3.0) + + self.plt_front_axis, = ax.plot(self.center_front_wheel[:, 0], + self.center_front_wheel[:, 1], "k-", lw=3.0) + self.plt_rear_axis, = ax.plot(self.center_rear_wheel[:, 0], + self.center_rear_wheel[:, 1], "k-", lw=3.0) + + # TODO: Add this part + self.prediction_xy, = ax.plot(self.center_of_mass[0] * np.ones(HORIZON + 1), + self.center_of_mass[1] * np.ones(HORIZON + 1), + self.dot) + self.prediction_xy_line, = ax.plot(self.center_of_mass[0] * np.ones(HORIZON + 1), + self.center_of_mass[1] * np.ones(HORIZON + 1), + self.line) + + self.trajectory, = ax.plot(self.trajectory_xy[:, 0], self.trajectory_xy[:, 1], self.line) + + self.current_lap = 0 + + self.rect = patch.Rectangle([- self.l_rear - self.wheel_length / 2 - self.bumper, + - self.width / 2 - self.wheel_width / 2], + self.agent_length + self.wheel_length + 2 * self.bumper, + self.width + self.wheel_width, + color=self.color, alpha=0.5, ec="black") + self.front_wheel_left = patch.Rectangle([- self.wheel_length / 2, + - self.wheel_width / 2], + self.wheel_length, self.wheel_width, + color="gray", alpha=1.0, ec="black") + self.front_wheel_right = patch.Rectangle([- self.wheel_length / 2, + - self.wheel_width / 2], + self.wheel_length, self.wheel_width, + color="gray", alpha=1.0, ec="black") + self.rear_wheel_left = patch.Rectangle([- self.wheel_length / 2, + - self.wheel_width / 2], + self.wheel_length, self.wheel_width, + color="gray", alpha=1.0, ec="black") + self.rear_wheel_right = patch.Rectangle([- self.wheel_length / 2, + - self.wheel_width / 2], + self.wheel_length, self.wheel_width, + color="gray", alpha=1.0, ec="black") + + self.gps_pos, = ax.plot(self.x_gps, self.y_gps, self.star) + self.trajectory_gps, = ax.plot(self.trajectory_gps_xy[:, 0], self.trajectory_gps_xy[:, 1], self.star) + # self.trajectory_gps_dots, = ax.plot(self.trajectory_gps_xy[:, 0], self.trajectory_gps_xy[:, 1], self.dot_extra) + + # self.transform(ax) + + ax.add_artist(self.rect) + ax.add_artist(self.front_wheel_left) + ax.add_artist(self.front_wheel_right) + ax.add_artist(self.rear_wheel_left) + ax.add_artist(self.rear_wheel_right) + + def xy_callback(self, msg): + self.center_of_mass = np.array([msg.x, msg.y]) + + self.psi = msg.psi + self.psi_and_steering = self.psi + msg.u_df + + self.front = self.center_of_mass + \ + rotate_around_z(np.array([self.l_front, 0.0]), self.psi) + self.rear = self.center_of_mass - \ + rotate_around_z(np.array([self.l_rear, 0.0]), self.psi) + + self.center_front_wheel = np.array([[0.0, self.width / 2], + [0.0, - self.width / 2]]) + self.center_front_wheel = rotate_around_z(self.center_front_wheel.T, self.psi) + self.center_front_wheel = np.reshape(np.repeat(self.front, 2), (2, 2)).T + \ + self.center_front_wheel.T + self.center_rear_wheel = np.array([[0.0, self.width / 2], + [0.0, - self.width / 2]]) + self.center_rear_wheel = rotate_around_z(self.center_rear_wheel.T, self.psi) + self.center_rear_wheel = np.reshape(np.repeat(self.rear, 2), (2, 2)).T + \ + self.center_rear_wheel.T + + self.center_front_wheel_left = self.center_front_wheel[0, :] + self.center_front_wheel_right = self.center_front_wheel[1, :] + self.center_rear_wheel_left = self.center_rear_wheel[0, :] + self.center_rear_wheel_right = self.center_rear_wheel[1, :] + + def xy_gps_callback(self, msg): + # if 2 == msg.flags: + self.x_gps = msg.x_m + self.y_gps = msg.y_m + + # print "gps counter: " self.gps_counter + + if self.prediction_available: + # if self.gps_counter % 10 == 0: + self.trajectory_gps_xy[self.traj_gps_counter, :] = np.array([msg.x_m, msg.y_m]) + self.trajectory_gps_xy[self.traj_gps_counter:, :] = np.array([msg.x_m, msg.y_m]) + self.traj_gps_counter += 1 + + self.gps_counter += 1 + + + def transform(self, ax): + self.rect.set_transform((set_transformation(ax, self.psi, + self.center_of_mass))) + self.front_wheel_left.set_transform(set_transformation(ax, self.psi_and_steering, + self.center_front_wheel_left)) + self.front_wheel_right.set_transform(set_transformation(ax, self.psi_and_steering, + self.center_front_wheel_right)) + self.rear_wheel_left.set_transform(set_transformation(ax, self.psi, + self.center_rear_wheel_left)) + self.rear_wheel_right.set_transform(set_transformation(ax, self.psi, + self.center_rear_wheel_right)) + + def prediction_callback(self, msg): + # print(np.array(msg.x)) + # test_s = np.zeros((len(msg.x), 6)) + # test_xy = np.zeros((len(msg.x), 6)) + # test_s[:, 0] = msg.x + # test_s[:, 1] = msg.y + # for i in range(len(msg.x)): + # test_xy[i, :] = s_to_xy(test_s[i, :], self.track) + # self.prediction_xy.set_data(test_xy[:, 0], test_xy[:, 1]) + # self.prediction_xy_line.set_data(test_xy[:, 0], test_xy[:, 1]) + + # self.steering_angles[self.counter] = msg.steering[0] + + # xy_state = np.array([msg.x[0], msg.y[0], 0.0, 0.0, 0.0, 0.0]) + # s_state = xy_to_s(xy_state, self.track) + # self.s[self.counter] = s_state[0] + # self.counter += 1 + + self.prediction_available = True + + try: + self.prediction_xy.set_data(np.array(msg.x), np.array(msg.y)) + self.prediction_xy_line.set_data(np.array(msg.x), np.array(msg.y)) + except RuntimeError: + self.prediction_xy.set_data(np.zeros(HORIZON), np.zeros(HORIZON)) + self.prediction_xy_line.set_data(np.zeros(HORIZON), np.zeros(HORIZON)) + + if msg.current_lap > self.current_lap: + self.trajectory_xy = np.zeros_like(self.trajectory_xy) + self.trajectory_gps_xy = np.zeros_like(self.trajectory_gps_xy) + self.traj_counter = 0 + self.gps_counter = 0 + self.traj_gps_counter = 0 + + self.trajectory_xy[self.traj_counter, :] = np.array([np.array(msg.x)[0], np.array(msg.y)[0]]) + self.trajectory_xy[self.traj_counter:, :] = np.array([np.array(msg.x)[0], np.array(msg.y)[0]]) + self.traj_counter += 1 + + self.current_lap = msg.current_lap + + if self.time_start is None: + self.time_start = rospy.get_time() + + # def theta_callback(self, msg): + # self.theta_vx[self.theta_counter, :] = msg.theta_vx + # self.theta_vy[self.theta_counter, :] = msg.theta_vy + # self.theta_psi_dot[self.theta_counter, :] = msg.theta_psi_dot + # + # self.vx_plot_1.set_data(np.arange(self.num_points), self.theta_vx[:, 0]) + # self.vx_plot_2.set_data(np.arange(self.num_points), self.theta_vx[:, 1]) + # self.vx_plot_3.set_data(np.arange(self.num_points), self.theta_vx[:, 2]) + + # self.theta_counter += 1 + + + def selection_callback(self, msg): + self.selection_x = msg.x + self.selection_y = msg.y + + self.selected_states_plot.set_data(self.selection_x, self.selection_y) + + def update(self, ax): + self.plt_front.set_data([self.center_of_mass[0], self.front[0]], + [self.center_of_mass[1], self.front[1]]) + self.plt_rear.set_data([self.center_of_mass[0], self.rear[0]], + [self.center_of_mass[1], self.rear[1]]) + + self.plt_front_axis.set_data(self.center_front_wheel[:, 0], + self.center_front_wheel[:, 1]) + self.plt_rear_axis.set_data(self.center_rear_wheel[:, 0], + self.center_rear_wheel[:, 1]) + + + self.trajectory.set_data(self.trajectory_xy[:, 0], self.trajectory_xy[:, 1]) + + self.gps_pos.set_data(self.x_gps, self.y_gps) + self.trajectory_gps.set_data(self.trajectory_gps_xy[:, 0], self.trajectory_gps_xy[:, 1]) + # self.trajectory_gps_dots.set_data(self.trajectory_gps_xy[:, 0], self.trajectory_gps_xy[:, 1]) + + self.transform(ax) + + +class Plotter: + """docstring for Plotter""" + plt.ion() # interactive plotting on + + fig = plt.figure("Race") + ax = fig.add_subplot(1, 1, 1) + # ax = fig.add_subplot(2, 1, 1) + # ax_2 = fig.add_subplot(2, 1, 2) + + # ax_2.set_ylim([-1, 1]) + + plotted_agents = None + count = 0 + + time_start = None + + def __init__(self, track, colors=None): + self.track = track + self.draw_track(track) + self.draw_finish_line(track) + + color_strings = [] + """ + try: + colors = [rospy.get_param("agent_1/color"), + rospy.get_param("agent_2/color")] + color_strings = ["agent_1/color", "agent_2/color"] + except: + colors = [rospy.get_param("agent_1/color")] + color_strings = ["agent_1/color"] + """ + node_name = rospy.get_name() + self.num_agents = rospy.get_param(node_name + "/num_agents") + if self.num_agents == 1: + index = rospy.get_param(node_name + "/index") + color_strings = ["agent_" + str(i) + "/color" for i in [index]] + else: + color_strings = ["agent_" + str(i + 1) + "/color" for i in range(self.num_agents)] + # color_strings = ["agent_1/color", "agent_2/color"] + # print("plotter color: ", colors) + + if color_strings is not None: + self.plotted_agents = [PlottedAgent(color_string, self.ax, track) for color_string in color_strings] + + # print(len(np.arange(len(track.curvature)))) + # print(len(self.plotted_agents[0].steering_angles)) + curvature = np.array([track.get_curvature(s) for s in self.plotted_agents[0].s]) + # self.steering_plot, = self.ax_2.plot(np.linspace(0.0, 23.9, 500), + # self.plotted_agents[0].steering_angles, + # "b-") + # self.ax_2.plot(np.linspace(0.0, 23.9, 500), 10 * curvature) + + leading_index = 0 + following_index = 0 + + elapsed_time = 0.0 + self.time_string = "Time : %.2f s" % (round(elapsed_time, 2)) + + # plotter.ax[:annotate](time_string, xy=[0.5, 0.9], + self.time_s = self.ax.annotate("", xy=[0.6, 0.85], xycoords= "axes fraction", + fontsize=20, verticalalignment="top") + # plotter.ax[:annotate](race_string, xy=[0.8, 0.9], xycoords="axes fraction", + self.race_s = self.ax.annotate("", xy=[0.6, 0.75], xycoords="axes fraction", + fontsize=20, verticalalignment="top") + + plt.axis("equal") + plt.grid() + plt.show() + plt.pause(0.0001) + + def draw_track(self, track): + self.ax.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], "k--") + self.ax.plot(track.xy_inner[:, 0], track.xy_inner[:, 1], "k-", lw=2.0) + self.ax.plot(track.xy_outer[:, 0], track.xy_outer[:, 1], "k-", lw=2.0) + + def draw_finish_line(self, track): + self.ax.plot(np.array([0, 0]), track.width / 2 * np.array([1, - 1]), + "k--", lw=5.0) + + def create_race_info_strings(self, num_laps): + if self.num_agents == 2: + agent_1_lap = self.plotted_agents[0].current_lap + agent_2_lap = self.plotted_agents[1].current_lap + if agent_1_lap > agent_2_lap: + leading_index = 1 + following_index = 2 + elif agent_1_lap == agent_2_lap: + agent_1_xy = np.zeros(6) + agent_1_xy[: 2] = self.plotted_agents[0].center_of_mass + agent_1_s = xy_to_s(agent_1_xy, self.track)[0] + agent_2_xy = np.zeros(6) + agent_2_xy[: 2] = self.plotted_agents[1].center_of_mass + agent_2_s = xy_to_s(agent_2_xy, self.track)[0] + if agent_1_s > agent_2_s: + leading_index = 1 + following_index = 2 + else: + leading_index = 2 + following_index = 1 + else: + leading_index = 2 + following_index = 1 + + leading_agent = self.plotted_agents[leading_index - 1].color.capitalize() + leading_lap = self.plotted_agents[leading_index - 1].current_lap + following_agent = self.plotted_agents[following_index - 1].color.capitalize() + following_lap = self.plotted_agents[following_index - 1].current_lap + + self.race_string = "%-13s %-10s\n1. %-10s %d/%d\n2. %-10s %d/%d" %("Race", "Lap", + leading_agent, + leading_lap, num_laps, + following_agent, + following_lap, + num_laps) + else: + leading_agent = self.plotted_agents[0].color.capitalize() + leading_lap = self.plotted_agents[0].current_lap + self.race_string = "%-13s %-10s\n1. %-10s %d/%d" %("Race", "Lap", + leading_agent, + leading_lap, num_laps) + + def update_time_string(self): + time_now = rospy.get_time() + if self.time_start is not None: + elapsed_time = time_now - self.time_start + self.time_string = "Time : %.2f s" % (round(elapsed_time, 2)) + + def update(self, track): + if self.plotted_agents is not None: + for i in range(len(self.plotted_agents)): + self.plotted_agents[i].update(self.ax) + + if self.num_agents == 2: + num_laps = max(self.plotted_agents[0].current_lap, + self.plotted_agents[1].current_lap) + if self.time_start is None: + if self.plotted_agents[0].time_start is not None and \ + self.plotted_agents[1].time_start is not None: + self.time_start = min(self.plotted_agents[0].time_start, + self.plotted_agents[1].time_start) + elif self.plotted_agents[0].time_start is not None: + self.time_start = self.plotted_agents[0].time_start + elif self.plotted_agents[1].time_start is not None: + self.time_start = self.plotted_agents[1].time_start + else: + num_laps = self.plotted_agents[0].current_lap + if self.time_start is None: + if self.plotted_agents[0].time_start is not None: + self.time_start = self.plotted_agents[0].time_start + + self.create_race_info_strings(30) + self.update_time_string() + self.time_s.set_text(self.time_string) + self.race_s.set_text(self.race_string) + + + + # self.steering_plot.set_data(self.plotted_agents[0].s, + # self.plotted_agents[0].steering_angles) + + # self.ax.set_xlim(self.plotted_agents[0].center_of_mass[0] - 1.0, self.plotted_agents[0].center_of_mass[0] + 1.0) + # self.ax.set_ylim(self.plotted_agents[0].center_of_mass[1] - 1.0, self.plotted_agents[0].center_of_mass[1] + 1.0) + + # self.ax_2.set_xlim(0.0, track.total_length) + # self.ax_2.set_ylim(- np.pi / 3, np.pi / 3) + + # plt.savefig("/home/lukas/images/image_" + str(self.count) + ".png") + plt.savefig("/home/mpcubuntu/lukas/images/image_" + str(self.count) + ".png") + + # print("STEERING: ", self.plotted_agents[0].steering_angles[np.max(self.plotted_agents[0].counter - 1, 0)]) + # print("MAX STEERING: ", np.max(self.plotted_agents[0].steering_angles)) + # print("MIN STEERING: ", np.min(self.plotted_agents[0].steering_angles)) + + try: + plt.pause(0.0001) + except: + RuntimeError("Plot not updated.") + + self.count += 1 + + +if __name__ == "__main__": + + try: + rospy.init_node("plotting_stuff") + colors = ["blue"] + # track = Track(ds=0.1, shape="test", width=1.2) + # track = Track(ds=0.1, shape="oval", width=1.2) + track = Track(ds=0.1, shape="l_shape", width=1.2) + plotter = Plotter(track, colors) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + + while not rospy.is_shutdown(): + plotter.update(track) + rate.sleep() + + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/replay.jl b/workspace/src/barc/src/replay.jl new file mode 100755 index 00000000..9ceeac13 --- /dev/null +++ b/workspace/src/barc/src/replay.jl @@ -0,0 +1,818 @@ +#!/usr/bin/env julia + +#= + File name: replay.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + + +using PyCall +using PyPlot +using JLD +using QuickHull + +@pyimport matplotlib.patches as patch +@pyimport matplotlib.transforms as transforms +@pyimport matplotlib.animation as animation +@pyimport matplotlib as mpl +@pyimport matplotlib.collections as collections + +# cv2 = pyimport("cv2") + +include("config_2.jl") +include("filesystem_helpers.jl") +# include("plotter.jl") +include("track.jl") +include("transformations.jl") + + +function plot_selection(ax, x, y, color) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull = patch.Polygon(conv_hull, fill=true, color=color, alpha=0.2) + ax[:add_artist](convex_hull) + + plt_hull_bounds, = ax[:plot](conv_hull[:, 1], conv_hull[:, 2], color * "-", lw=2.0) + plt_states, = ax[:plot](x, y, color * "o") + + counter = 1 + states_per_lap = round(Int64, size(x, 1) / NUM_CONSIDERED_LAPS) + plt_laps = [] + for i = 1 : NUM_CONSIDERED_LAPS + x_lap_i = x[counter : counter + states_per_lap - 1] + y_lap_i = y[counter : counter + states_per_lap - 1] + dummy_lap, = ax[:plot](x_lap_i, y_lap_i, color * "-") + plt_laps = [plt_laps; dummy_lap] + counter += states_per_lap + end + + return convex_hull, plt_hull_bounds, plt_states, plt_laps +end + + +function update_selection(convex_hull, plt_hull_bounds, plt_states, plt_laps, x, y) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull[:set_xy](conv_hull) + plt_hull_bounds[:set_data](conv_hull[:, 1], conv_hull[:, 2]) + plt_states[:set_data](x, y) + + counter = 1 + states_per_lap = round(Int64, size(x, 1) / NUM_CONSIDERED_LAPS) + for i = 1 : NUM_CONSIDERED_LAPS + x_lap_i = x[counter : counter + states_per_lap - 1] + y_lap_i = y[counter : counter + states_per_lap - 1] + plt_laps[i][:set_data](x_lap_i, y_lap_i) + counter += states_per_lap + end +end + + +function plot_prediction(ax, x, y, color) + prediction_dots, = ax[:plot](x[1 : end - 1], y[1 : end - 1], color * "o") + prediction_star, = ax[:plot](x[end], y[end], color * "*", markersize=10) + prediction_line, = ax[:plot](x, y, color * "-") + + return prediction_dots, prediction_star, prediction_line +end + + +function update_prediction(dots, star, line, x, y) + dots[:set_data](x[1 : end - 1], y[1 : end - 1]) + star[:set_data](x[end], y[end]) + line[:set_data](x, y) +end + + +function update_limits(ax, x, y, x_pred, y_pred) + ax[:set_xlim]([min(findmin(x)[1], findmin(x_pred)[1]) - 0.1, + max(findmax(x)[1], findmax(x_pred)[1]) + 0.1]) + # ax[:set_ylim]([min(findmin(y)[1], findmin(y_pred)[1]), + # max(findmax(y)[1], findmax(y_pred)[1])]) +end + +function update_limits(ax, x_pred, y_pred) + ax[:set_xlim]([findmin(x_pred)[1] - 0.1, + findmax(x_pred)[1] + 0.1]) + # ax[:set_ylim]([min(findmin(y)[1], findmin(y_pred)[1]), + # max(findmax(y)[1], findmax(y_pred)[1])]) +end + + +function set_limits(ax, trajectories_s, predictions, index, indeces) + min_val = min(findmin(trajectories_s[:, :, index])[1], + findmin(predictions[indeces, :, index])[1]) + max_val = max(findmax(trajectories_s[:, :, index])[1], + findmax(predictions[indeces, :, index])[1]) + ax[:set_ylim]([min_val - 0.1, max_val + 0.1]) +end + + +function plot_closed_loop(ax, x, y, max_i, color) + closed_loop, = ax[:plot](x[NUM_STATES_BUFFER + (1 : max_i)], + y[NUM_STATES_BUFFER + (1 : max_i)], color * "--", lw=2) + return closed_loop +end + + +function update_closed_loop(closed_loop_plt, x, y, max_i) + closed_loop_plt[:set_data](x[NUM_STATES_BUFFER + (1 : max_i)], + y[NUM_STATES_BUFFER + (1 : max_i)]) +end + +function plot_true_curvature(ax, track::Track, color) + num_points = 3000 + s_coords = linspace(0, track.total_length, num_points) + curvature = zeros(s_coords) + + for i = 1 : num_points + curvature[i] = get_curvature(track, s_coords[i]) + end + + ax[:plot](s_coords, curvature, color * "-") +end + +function plot_curvature(ax, track::Track, predicted_s, color) + curvature = zeros(predicted_s[:, 1]) + + for i = 1 : size(predicted_s, 1) + curvature[i] = get_curvature(track, predicted_s[i, 1]) + end + + curvature_dots, = ax[:plot](predicted_s[:, 1], curvature, color * "o") + curvature_lines, = ax[:plot](predicted_s[:, 1], curvature, color * "-") + + return curvature_dots, curvature_lines +end + +function update_curvature(curvature_dots, curvature_lines, track::Track, predicted_s) + curvature = zeros(predicted_s[:, 1]) + + for i = 1 : size(predicted_s, 1) + curvature[i] = get_curvature(track, predicted_s[i, 1]) + end + + curvature_dots[:set_data](predicted_s[:, 1], curvature) + curvature_lines[:set_data](predicted_s[:, 1], curvature) +end + +function plot_propagated_curvature(ax, track::Track, prev_predicted_s, predicted_s, color) + curvature = propagate_curvature(track, prev_predicted_s) + + curvature_dots, = ax[:plot](prev_predicted_s[2 : end, 1], curvature, color * "o") + curvature_lines, = ax[:plot](prev_predicted_s[2 : end, 1], curvature, color * "-") + + return curvature_dots, curvature_lines +end + +function update_propagated_curvature(curvature_dots, curvature_lines, track::Track, + prev_predicted_s, predicted_s) + curvature = propagate_curvature(track, prev_predicted_s) + + curvature_dots[:set_data](prev_predicted_s[2 : end, 1], curvature) + curvature_lines[:set_data](prev_predicted_s[2 : end, 1], curvature) +end + +function propagate_curvature(track::Track, predicted_s) + shift = 1 + dt = 0.1 + + horizon = size(predicted_s, 1) - 1 + curvature = zeros(horizon) + + for i = 1 : horizon + curvature[i] = get_curvature(track, predicted_s[i + 1, 1]) + end + + # curvature[horizon] = get_curvature(track, predicted_s[horizon, i]) + + #= + for i = 1 : horizon - 2 + curvature[i] = get_curvature(track, predicted_s[i + 2, 1]) + end + + delta_s = dt * predicted_s[end, 5] + curvature[end - 1] = get_curvature(track, predicted_s[end, 1] + delta_s) + curvature[end] = get_curvature(track, predicted_s[end, 1] + 2 * delta_s) + =# + + return curvature +end + + +function replay_prediction(file, track::Track) + plt[:ion]() + + # Create the name of the window + figure_string = create_window_name(file) + + index = get_index(file) + if index == 1 + color = "b" + extra_color = "c" + elseif index == 2 + color = "r" + extra_color = "m" + else + error("Only two colors defined.") + end + + data = load(file) + + # Create figure and subplots + fig = figure("Prediction " * figure_string) + ax_s_ey = fig[:add_subplot](2, 3, 1) + xlabel("s [m]") + ylabel("e_y [m]") + + ax_s_epsi = fig[:add_subplot](2, 3, 2) + xlabel("s [m]") + ylabel("e_psi [rad]") + + ax_s_psidot = fig[:add_subplot](2, 3, 3) + xlabel("s [m]") + ylabel("psi_dot [rad / s]") + + ax_s_vx = fig[:add_subplot](2, 3, 4) + xlabel("s [m]") + ylabel("v_x [m / s]") + + ax_s_vy = fig[:add_subplot](2, 3, 5) + xlabel("s [m]") + ylabel("v_y [m / s]") + + ax_s_c = fig[:add_subplot](2, 3, 6) + xlabel("s [m]") + ylabel("c [1 / m]") + + plot_true_curvature(ax_s_c, track, color) + min_curvature = 0.0 + max_curvature = 0.0 + + for i = 1 : size(track.curvature, 2) + kappa = track.curvature[2, i] + if kappa < min_curvature + min_curvature = kappa + end + if kappa > max_curvature + max_curvature = kappa + end + end + + ax_s_c[:set_ylim]([min_curvature - 0.1, max_curvature + 0.1]) + + ax = [ax_s_ey, ax_s_epsi, ax_s_psidot, ax_s_vx, ax_s_vy] + + trajectories_s = data["trajectories_s"] + all_selected_states = data["all_selected_states"] + all_predictions = data["all_predictions"] + + # Plot track bounds + ax_s_ey[:plot]([- 5; 20], [TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k-", lw=2.0) + ax_s_ey[:plot]([- 5; 20], [- TRACK_WIDTH / 2; - TRACK_WIDTH / 2], "k-", lw=2.0) + ax_s_ey[:plot]([- 5; 20], [0; 0], "k--") + + # Plot finish lines + max_s = ceil(findmax(all_predictions[:, 1, 1])[1] * 10) / 10 + ax_s_ey[:plot]([0; 0], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=5.0) + ax_s_ey[:plot]([max_s; max_s], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=5.0) + + ax_s_ey[:plot]([1.0; 1.0], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=1.0) + ax_s_ey[:plot]([5.5; 5.5], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=1.0) + ax_s_ey[:plot]([7.5; 7.5], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=1.0) + ax_s_ey[:plot]([12.0; 12.0], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=1.0) + ax_s_ey[:plot]([14.0; 14.0], [- TRACK_WIDTH / 2; TRACK_WIDTH / 2], "k--", lw=1.0) + + # Plot reference + ey_init = 0.0 + epsi_init = 0.0 + v_init = V_INIT + if contains(file, "center") + ey_init = EY_CENTER + elseif contains(file, "inner") + ey_init = EY_INNER + elseif contains(file, "outer") + ey_init = EY_OUTER + else + error("No initalization value for ey_init found.") + end + + ey_ref, = ax_s_ey[:plot]([- 5; 20], [ey_init; ey_init], color * "-", lw=2.0) + epsi_ref, = ax_s_epsi[:plot]([- 5; 20], [epsi_init; epsi_init], color * "-", lw=2.0) + vx_ref, = ax_s_vx[:plot]([- 5; 20], [v_init; v_init], color * "-", lw=2.0) + + # Determine the start and end index + start_index = 0 + sum_over_predictions = sumabs(all_predictions[:, :, [4, 6]], (2, 3)) + for i = 1 : 6000 + if sum_over_predictions[i] > 1e-5 + start_index = i + break + end + end + + end_index = 0 + for i = 6000 : - 1 : 1 + if sum_over_predictions[i] > 1e-5 + end_index = i + break + end + end + + for i = 1 : 5 + set_limits(ax[i], trajectories_s, all_predictions, i + 1, start_index : end_index) + end + + iteration = 1 + predicted_s = squeeze(all_predictions[iteration, :, :], 1) + + dots = [] + stars = [] + lines = [] + + for i = 1 : 5 + dot_plt, star, line = plot_prediction(ax[i], predicted_s[:, 1], + predicted_s[:, i + 1], color) + dots = [dots; dot_plt] + stars = [stars; star] + lines = [lines; line] + end + + curvature_dots, curvature_lines = plot_curvature(ax_s_c, track, predicted_s, + extra_color) + prev_curvature_dots, prev_curvature_lines = plot_propagated_curvature(ax_s_c, track, + predicted_s, + predicted_s, color) + #= + for iteration = 2 : start_index + predicted_s = squeeze(all_predictions[iteration, :, :], 1) + + for i = 1 : 5 + update_prediction(dots[i], stars[i], lines[i], predicted_s[:, 1], + predicted_s[:, i + 1]) + update_limits(ax[i], predicted_s[:, 1], predicted_s[:, i + 1]) + end + + plt[:pause](0.001) + end + =# + + test_selection = all_selected_states[start_index] + selected_states = zeros(1, 6) + + for i = 1 : NUM_CONSIDERED_LAPS + dummy = squeeze(trajectories_s[test_selection[i][1], test_selection[i][2], :], 1) + selected_states = [selected_states; dummy] + end + + # Make the references invisible after the path following + ey_ref[:set_alpha](0.0) + epsi_ref[:set_alpha](0.0) + vx_ref[:set_alpha](0.0) + + selected_states = selected_states[2 : end, :] + + conv_hulls = [] + hull_bounds = [] + plt_states = [] + plt_laps = [] + + for i = 1 : 5 + conv_hull, hull_bound, plt_state, plt_lap = plot_selection(ax[i], + selected_states[:, 1], + selected_states[:, i + 1], + extra_color) + conv_hulls = [conv_hulls; conv_hull] + hull_bounds = [hull_bounds; hull_bound] + plt_states = [plt_states; plt_state] + plt_laps = [plt_laps; plt_lap] + end + + lap = 7 # start out with lap 7, since this is the first LMPC lap + prev_s = 0 + max_iteration = findmin(sumabs(trajectories_s[lap, NUM_STATES_BUFFER + 1 : end, 1 : 2], 3))[2] - 1 + closed_loop_plts = [] + + for i = 1 : 5 + closed_loop_plt = plot_closed_loop(ax[i], trajectories_s[lap, :, 1], + trajectories_s[lap, :, i + 1], + max_iteration, color) + closed_loop_plts = [closed_loop_plts; closed_loop_plt] + end + + for iteration = start_index + 1 : end_index + + test_selection = all_selected_states[iteration] + predicted_s = squeeze(all_predictions[iteration, :, :], 1) + prev_predicted_s = squeeze(all_predictions[iteration - 1, :, :], 1) + + if abs(predicted_s[1, 1] - prev_s) > 0.5 * track.total_length + lap += 1 + max_iteration = findmin(sumabs(trajectories_s[lap, NUM_STATES_BUFFER + 1 : end, 1 : 2], 3))[2] - 1 + for i = 1 : 5 + update_closed_loop(closed_loop_plts[i], + trajectories_s[lap, :, 1], + trajectories_s[lap, :, i + 1], + max_iteration) + end + end + + selected_states = zeros(1, 6) + + for i = 1 : NUM_CONSIDERED_LAPS + dummy = squeeze(trajectories_s[test_selection[i][1], + test_selection[i][2], :], 1) + selected_states = [selected_states; dummy] + end + + selected_states = selected_states[2 : end, :] + + for i = 1 : 5 + update_selection(conv_hulls[i], hull_bounds[i], plt_states[i], + plt_laps[1 + (i - 1) * NUM_CONSIDERED_LAPS : i * NUM_CONSIDERED_LAPS], + selected_states[:, 1], + selected_states[:, i + 1]) + + update_prediction(dots[i], stars[i], lines[i], predicted_s[:, 1], + predicted_s[:, i + 1]) + update_limits(ax[i], selected_states[:, 1], + selected_states[:, i + 1], predicted_s[:, 1], + predicted_s[:, i + 1]) + end + + update_curvature(curvature_dots, curvature_lines, track, predicted_s) + update_propagated_curvature(prev_curvature_dots, prev_curvature_lines, + track, prev_predicted_s, predicted_s) + + update_limits(ax_s_c, selected_states[:, 1], selected_states[:, 2], + predicted_s[:, 1], predicted_s[:, 2]) + + prev_s = predicted_s[1, 1] + + fig[:savefig]("/home/mpcubuntu/lukas/prediction/iteration_$(iteration).png", dpi=100) + plt[:pause](0.1) + end +end + + +function replay_data(file) + # Create the name of the window + figure_string = create_window_name(file) + + index = get_index(file) + if index == 1 + color = "b" + extra_color = "c" + elseif index == 2 + color = "r" + extra_color = "m" + else + error("Only two colors defined.") + end + + # Create figure and subplots + fig = figure("States " * figure_string) + + ax_s = fig[:add_subplot](3, 2, 1) + xlabel("iteration") + ylabel("s [m]") + ax_ey = fig[:add_subplot](3, 2, 2) + xlabel("iteration") + ylabel("e_y [m]") + ax_epsi = fig[:add_subplot](3, 2, 3) + xlabel("iteration") + ylabel("e_psi [rad]") + ax_psidot = fig[:add_subplot](3, 2, 4) + xlabel("iteration") + ylabel("psi_dot [rad / s]") + ax_vx = fig[:add_subplot](3, 2, 5) + xlabel("iteration") + ylabel("v_x [m / s]") + ax_vy = fig[:add_subplot](3, 2, 6) + xlabel("iteration") + ylabel("v_y [m / s]") + + data = load(file) + # println(diff(data["time"][1 : 100])) + + trajectories_s = data["trajectories_s"] + trajectories_s = trajectories_s[:, NUM_STATES_BUFFER + 1 : end, :] + trajectories_s[1, :, :] = data["trajectories_s"][1, 1 : end - NUM_STATES_BUFFER, :] + + start_indeces = zeros(Int64, size(trajectories_s, 1)) + end_indeces = zeros(Int64, size(trajectories_s, 1)) + + # Find maximum num iteration for each lap + min_indeces = findmin(sumabs(trajectories_s[:, :, :], (3)), 2)[2] + + for i = 1 : size(trajectories_s, 1) + index = ind2sub(trajectories_s[:, :, 1], min_indeces[i])[2] + if i == size(trajectories_s, 1) + end_indeces[i] = index - 1 + else + end_indeces[i] = index - 1 - NUM_STATES_BUFFER + end + # println(trajectories_s[i, end_indeces[i], 1], " < ", trajectories_s[1, end_indeces[1], 1] - 1) + if trajectories_s[i, end_indeces[i], 1] < trajectories_s[1, end_indeces[1], 1] - 1 + end_indeces[i] += NUM_STATES_BUFFER + end + # println(trajectories_s[i, end_indeces[i], 1]) + end + # println(end_indeces) + + num_iterations = 0 + for i = 1 : size(trajectories_s, 1) + num_iterations += end_indeces[i] + end + + states_s = zeros(num_iterations, 6) + counter = 1 + for i = 1 : size(trajectories_s, 1) + states_s[counter : counter + end_indeces[i] - 1, :] = squeeze(trajectories_s[i, 1 : end_indeces[i], :], 1) + counter += end_indeces[i] + end + + ax = [ax_s; ax_ey; ax_epsi; ax_psidot; ax_vx; ax_vy] + + for i = 1 : 6 + ax[i][:plot](1 : num_iterations, states_s[:, i], color * "-") + plot_lap_indicators(ax[i], states_s[:, i], cumsum(end_indeces)) + end + + #= + ax[5][:plot](sum(end_indeces[1 : 7]) + 1 - NUM_STATES_BUFFER : sum(end_indeces[1 : 8]) + NUM_STATES_BUFFER, + # trajectories_s[7, 1 : end_indeces[7] + NUM_STATES_BUFFER, 5]', "r-") + data["trajectories_s"][8, 1 : end_indeces[8] + 2 * NUM_STATES_BUFFER, 5]', "r-") + =# + + for i = 1 : 6 + ax[i][:set_xlim]([1, sum(end_indeces)]) + end + + fig_s_vx = figure("Velocity " * figure_string) + ax_svx = fig_s_vx[:add_subplot](1, 1, 1) + xlabel("s [m]") + ylabel("v_x [m / s]") + + for i = 1 : size(trajectories_s, 1) + # s = data["trajectories_s"][i, NUM_STATES_BUFFER + 1 : end_indeces[i] + NUM_STATES_BUFFER, 1] + # vx = data["trajectories_s"][i, NUM_STATES_BUFFER + 1 : end_indeces[i] + NUM_STATES_BUFFER, 5] + + s = data["trajectories_s"][i, 1 : end_indeces[i] + 2 * NUM_STATES_BUFFER, 1] + vx = data["trajectories_s"][i, 1 : end_indeces[i] + 2 * NUM_STATES_BUFFER, 5] + + ax_svx[:plot](s', vx', "-", label="lap $(i)") + ax_svx[:legend](loc="bottom right") + end + + theta_vx = data["theta_vx"] + theta_vy = data["theta_vy"] + theta_psidot = data["theta_psi_dot"] + + fig_theta = figure("Theta " * figure_string) + ax_theta_vx = fig_theta[:add_subplot](3, 1, 1) + xlabel("iteration") + ylabel("theta_v_x") + ax_theta_vy = fig_theta[:add_subplot](3, 1, 2) + xlabel("iteration") + ylabel("theta_v_y") + ax_theta_psidot = fig_theta[:add_subplot](3, 1, 3) + xlabel("iteration") + ylabel("theta_psi_dot") + + ax = [ax_theta_vx; ax_theta_vy; ax_theta_psidot] + dict = Dict("a" => 1, "b" => 2, "c" => 3) + theta = Dict(1 => theta_vx, 2 => theta_vy, 3 => theta_psidot) + theta_string = ["theta_v_x_"; "theta_v_y_"; "theta_psi_dot_"] + + for i = 1 : 3 + for j = 1 : size(theta[i], 2) + ax[i][:plot]((1 : num_iterations) + sum(end_indeces[1 : 5]), theta[i][1 : num_iterations, j], + label=theta_string[i] * "$(j)") + ax[i][:legend](loc="upper left", fancybox="true") + end + plot_lap_indicators(ax[i], theta[i], cumsum(end_indeces)) + ax[i][:set_xlim]([1, sum(end_indeces)]) + end + + fig_ang = figure("Inputs " * figure_string) + ax_acc = fig_ang[:add_subplot](2, 1, 1) + xlabel("Iteration") + ylabel("a [m / s^2]") + ax_angle = fig_ang[:add_subplot](2, 1, 2) + xlabel("Iteration") + ylabel("d_f [rad]") + + for i = 1 : size(trajectories_s, 1) + # for i = size(trajectories_s, 1) + acc = data["previous_inputs"][i, 1 : end_indeces[i] + 2 * NUM_STATES_BUFFER, 1] + ax_acc[:plot](1 : size(acc, 2), acc', "-", label="lap $(i)") + + angle = data["previous_inputs"][i, 1 : end_indeces[i] + 2 * NUM_STATES_BUFFER, 2] + ax_angle[:plot](1 : size(angle, 2), angle', "-", label="lap $(i)") + end + + ax_acc[:legend](loc="bottom right") + ax_angle[:legend](loc="bottom right") + + + + plt[:show]() +end + + +function plot_lap_indicators(ax, states, indeces) + max_state = findmax(states)[1] + 0.1 + min_state = findmin(states)[1] - 0.1 + for i in indeces + ax[:plot]([i, i], [min_state, max_state], "k--") + ax[:set_ylim]([min_state, max_state]) + end +end + + +function plot_trajectories(track::Track, laps::Array{Int64}, + trajectories_s::Array{Float64}, + trajectories_xy::Array{Float64}, file) + line = nothing + + figure_string = create_window_name(file) + # Create figure and subplots + fig = figure("Trajectory " * figure_string) + axs = fig[:add_subplot](1, 1, 1) + + # norm = plt[:Normalize](0.6, findmax(V_MAX)[1]) # max should max(V_MAX), min = 0 + + plot(track.xy_coords[:, 1], track.xy_coords[:, 2], "k--") + plot(track.xy_outer[:, 1], track.xy_outer[:, 2], "k-", lw=2.0) + plot(track.xy_inner[:, 1], track.xy_inner[:, 2], "k-", lw=2.0) + + once = true + + v_x_min = findmin(trajectories_xy[laps, :, 3])[1] + v_y_min = findmin(trajectories_xy[laps, :, 4])[1] + v_min = sqrt(v_x_min^2 + v_y_min^2) + v_min = 1.0 + v_x_max = findmax(trajectories_xy[laps, :, 3])[1] + v_y_max = findmax(trajectories_xy[laps, :, 4])[1] + v_max = sqrt(v_x_max^2 + v_y_max^2) + + for lap in reverse(laps) + if sumabs(trajectories_s[lap, :, :], (2, 3))[1] == 0. + # println("No data for lap $(lap) available") + continue + end + trajectory = trajectories_xy[lap, :, 1 : 4] + needed_iters = findmin(sumabs(trajectories_s[lap, :, :], + (1, 3)))[2] - 1 - NUM_STATES_BUFFER + + println("LAP: $(lap), needed_iters: $(needed_iters)") + if needed_iters < 1 + needed_iters = findmin(sumabs(trajectories_s[lap, NUM_STATES_BUFFER + 1 : end, :], + (1, 3)))[2] - 1 - NUM_STATES_BUFFER + end + + x = squeeze(trajectory[1, 1 : needed_iters, 1]', 2) + y = squeeze(trajectory[1, 1 : needed_iters, 2]', 2) + v = sqrt(squeeze(trajectory[1, 1 : needed_iters, 3]', 2).^2 + + squeeze(trajectory[1, 1 : needed_iters, 4]', 2).^2) + + points = reshape([x y], (size(x)[1], 1, 2)) + points_1 = points[1 : end - 1, :, :] + points_2 = points[2 : end, :, :] + segments = cat(2, points_1, points_2) + + # max should max(V_MAX), min = 0 + norm = plt[:Normalize](v_min, v_max) + + lc = collections.LineCollection(segments, cmap="jet", norm=norm) + # Set the values used for colormapping + lc[:set_array](v) + lc[:set_linewidth](2) + line = axs[:add_collection](lc) + + if once + fig[:colorbar](line, ax=axs, label="v [m / s]") + once = false + end + + axis("equal") + # grid("on") + + # plot(trajectory[1, 1 : needed_iters, 1]', trajectory[1, 1 : needed_iters, 2]', "r-") + end + + xlabel("x [m]") + ylabel("y [m]") + + plt[:show]() + plt[:pause](0.5) + + #= + fig[:colorbar](line, ax=axs) + + axis("equal") + # grid("on") + plt[:show]() + plt[:pause](30.0) + =# +end + + + +function replay_recording(file) + data = load(file) + + trajectories_s = data["trajectories_s"] + trajectories_xy = data["trajectories_xy"] + + track = Track() + init!(track) + + # plotter = Plotter() + # init!(plotter, track, simulator.agents) + + num_laps = size(trajectories_s, 1) + if num_laps > 5 + start_lap = 6 + else + start_lap = 1 + end + + # start_lap = 1 + # num_laps = 5 + + start_lap = 96 + num_laps = 100 + + # start_lap = 30 + # num_laps = 35 + + laps = collect(start_lap : num_laps) + plot_trajectories(track, laps, trajectories_s, trajectories_xy, file) + +end + + +function replay() + if length(ARGS) < 1 + error("No argument given. To start the replay specify the argument like this: + + julia replay.jl (optional: ) + + For , try one of: prediction, recording or data. + For , specify the file by giving the date of the recording: + e. g. 0503 (replays most recent file from that folder) or + 0503115854 (replays the file with this timestamp). + ") + elseif length(ARGS) > 2 + error("Too many arguments given. To start the replay specify the argument like this: + + julia replay.jl (optional: ) + + For , try one of: prediction, recording or data. + For , specify the file by giving the date of the recording: + e. g. 0503 (replays most recent file from that folder) or + 0503115854 (replays the file with this timestamp). + ") + end + + if length(ARGS) >= 2 + file = find_file(ARGS[2]) + else + # Load most recently recorded file + file = get_most_recent_file() + end + + if ARGS[1] == "prediction" + track = Track() + init!(track) + replay_prediction(file, track) + elseif ARGS[1] == "recording" + replay_recording(file) + elseif ARGS[1] == "recording-static" + replay_recording_static(file) + elseif ARGS[1] == "data" + replay_data(file) + else + error("Unknown argument $(ARGS[1]). Try one of: prediction, recording + or data.") + end +end + +# run the replay function +replay() \ No newline at end of file diff --git a/workspace/src/barc/src/simple_debugging.jl b/workspace/src/barc/src/simple_debugging.jl new file mode 100644 index 00000000..07af9e6f --- /dev/null +++ b/workspace/src/barc/src/simple_debugging.jl @@ -0,0 +1,537 @@ +# simple debugging +# GP data plotting to check if they are biased +# -> julia simple_debugging.jl GP SYS_ID_KIN_LIN_TI +# plot the newest recorded data +# -> julia simple_debugging.jl record +# plot the newest trajectory data +# -> julia simple_debugging.jl trajectory +# plot the newest recorded data and trajectory +# -> julia simple_debugging.jl both + + +using JLD +using PyPlot +using PyCall +using QuickHull + +@pyimport matplotlib.patches as patch +@pyimport matplotlib.transforms as transforms +@pyimport matplotlib.animation as animation +@pyimport matplotlib as mpl +@pyimport matplotlib.collections as collections + +include("barc_lib/classes.jl") +include("barc_lib/LMPC/functions.jl") + +# FLAG FOR PLOTTING THE SELECTED POINTS +const select_flag = false + +run_time = Dates.format(now(),"yyyy-mm-dd") +# if isfile("$(homedir())/experiments/LMPC-$(run_time)-$(ARGS[2]).jld") +# data = load("$(homedir())/experiments/LMPC-$(run_time)-$(ARGS[2]).jld") +# else + +# find the newest experiment time until now +file_names = readdir("$(homedir())/simulations/") +file_times = zeros(length(file_names)) +for i = 1:length(file_names) + file_times[i] = ctime("$(homedir())/simulations/$(file_names[i])") +end +(~,idx) = findmax(file_times) + +data = load("$(homedir())/simulations/$(file_names[idx])") +oldSS = data["oldSS"] +selectedStates = data["selectedStates"] +solHistory = data["solHistory"] +selectHistory = data["selectHistory"] +GP_vy_History = data["GP_vy_History"] +GP_psidot_History = data["GP_psidot_History"] +selectFeatureHistory = data["selectFeatureHistory"] +statusHistory = data["statusHistory"] +track = data["track"] +modelParams = data["modelParams"] +mpcParams = data["mpcParams"] +log_cvx = data["log_cvx"] +log_cvy = data["log_cvy"] +log_cpsi = data["log_cpsi"] +lapStatus = LapStatus(1,1,false,false,0.3) + + +# end +# figure("lap") +# plot(solHistory.cost) + +# STATE AND SYS_ID PARAMETERS PLOT FOR LAPS DONE +if ARGS[1] == "record" || ARGS[1]=="both" + figure("Record-$(file_names[idx])") # PLOT OF STATE + i = 1; plot_way = [3,2] + while solHistory.cost[i] > 10 + i == 1 ? current_x = 0 : current_x = sum(solHistory.cost[1:i-1]) + x_len = Int(solHistory.cost[i]) + subplot(3,2,1) + plot(current_x+1:current_x+x_len,solHistory.z[1:x_len,i,1,4],color="blue") + plot([current_x,current_x],[0,2.5],color="grey",linestyle="--") + ylabel("vx") + + subplot(3,2,3) + plot(current_x+1:current_x+x_len,solHistory.z[1:x_len,i,1,5],color="blue") + plot([current_x,current_x],[-0.3,0.3],color="grey",linestyle="--") + ylabel("vy") + + subplot(3,2,5) + plot(current_x+1:current_x+x_len,solHistory.z[1:x_len,i,1,6],color="blue") + plot([current_x,current_x],[-3,3],color="grey",linestyle="--") + ylabel("psi_dot") + + # subplot(3,2,5) + # plot(current_x+1:current_x+x_len,solHistory.u[1:x_len,i,1,1],color="blue",label="a") + # plot([current_x,current_x],[-3,3],color="grey",linestyle="--") + # ylabel("psi_dot") + + i += 1 + if i>length(solHistory.cost) + break + end + end + # PLOT OF SYS_ID PARAMETERS + # figure(figsize=(15,10)) + i = 1 + while solHistory.cost[i] > 10 + i == 1 ? current_x = 0 : current_x = sum(solHistory.cost[1:i-1]) + x_len = Int(solHistory.cost[i]) + subplot(3,2,2) + for j=1:size(log_cvx,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="blue",label="cvx_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="red",label="cvx_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="green",label="cvx_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvx[1:x_len,1,j,i],color="green") + end + end + plot([current_x,current_x],[-1,1],color="grey",linestyle="--") + end + legend(); ylabel("cvx") + + subplot(3,2,4) + for j=1:size(log_cvy,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="blue",label="cvy_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="red",label="cvy_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="green",label="cvy_$j") + elseif j == 4 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="orange",label="cvy_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="green") + elseif j == 4 + plot(current_x+1:current_x+x_len,log_cvy[1:x_len,1,j,i],color="orange") + end + end + plot([current_x,current_x],[-1,1],color="grey",linestyle="--") + end + legend(); ylabel("cvy") + + subplot(3,2,6) + for j=1:size(log_cpsi,3) + if i == 1 + if j == 1 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="blue",label="cpsi_$j") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="red",label="cpsi_$j") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="green",label="cpsi_$j") + end + else + if j == 1 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="blue") + elseif j == 2 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="red") + elseif j == 3 + plot(current_x+1:current_x+x_len,log_cpsi[1:x_len,1,j,i],color="green") + end + end + plot([current_x,current_x],[-4,4],color="grey",linestyle="--") + end + legend(); ylabel("cpsi") + i += 1 + if i>length(solHistory.cost) + break + end + end +end + +# TRAJECTORY PLOT FOR ALL THE LAPS DONE +if ARGS[1] == "trajectory" || ARGS[1]=="both" + i = 1 + fig = figure("Trajectory-$(file_names[idx])") + axs = fig[:add_subplot](1, 1, 1); line = nothing + plot(track.xy[:, 1], track.xy[:, 2], color="grey",alpha=0.4) + plot(track.bound1xy[:, 1], track.bound1xy[:, 2], color="red") + plot(track.bound2xy[:, 1], track.bound2xy[:, 2], color="red") + v_min = 3; v_max = 3; + while solHistory.cost[i] > 10 + x_len = Int(solHistory.cost[i]) + v = sqrt(solHistory.z[1:x_len,i,1,4].^2 + solHistory.z[1:x_len,i,1,5].^2) + state = reshape(solHistory.z[1:x_len,i,1,:],x_len,6) + (x,y) = trackFrame_to_xyFrame(state,track) + + points = reshape([x y], (size(x)[1], 1, 2)) + points_1 = points[1 : end - 1, :, :] + points_2 = points[2 : end, :, :] + segments = cat(2, points_1, points_2) + + # v_min = min(findmin(v)[1],v_min) + # v_max = max(findmax(v)[1],v_max) + v_min = findmin(v)[1] + v_max = findmax(v)[1] + # max should max(V_MAX), min = 0 + norm = plt[:Normalize](v_min, v_max) + + lc = collections.LineCollection(segments, cmap="jet", norm=norm) + # Set the values used for colormapping + lc[:set_array](v) + lc[:set_linewidth](2) + line = axs[:add_collection](lc) + i+=1 + if i>length(solHistory.cost) + break + end + end + axis("equal") + # grid("on") + fig[:colorbar](line, ax=axs) + xlabel("x [m]") + ylabel("y [m]") +end +show() + +function set_limits(ax, solHistory::SolHistory,index::Int64) + i = 1; min_val = 0; max_val = 0 + while solHistory.cost[i]>10 + min_val = min(findmin(solHistory.z[:, i, :, index])[1],min_val) + max_val = max(findmax(solHistory.z[:, i, :, index])[1],max_val) + i+=1 + if i>length(solHistory.cost) + break + end + end + ax[:set_ylim]([min_val - 0.1, max_val + 0.1]) +end + +function update_limits(ax, x_pred::Array{Float64}, x_true::Array{Float64,1}, x_select::Array{Float64,1}) + ax[:set_xlim]([min(findmin(x_pred)[1],findmin(x_true)[1],findmin(x_select)[1]) - 0.1, + max(findmax(x_pred)[1],findmax(x_true)[1],findmax(x_select)[1]) + 0.1]) + # println("pre",findmax(x_pred)[1]) + # println(findmax(x_true)[1]) + # println(findmax(x_select)[1]) +end + +function plot_prediction(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + prediction, = ax[:plot](x, y, color=c, marker="*") + return prediction +end + +function update_prediction(plt_h, x::Array{Float64}, y::Array{Float64}, track::Track) + plt_h[:set_data](x, y) +end + +function plot_selection(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull = patch.Polygon(conv_hull, fill=true, color=c, alpha=0.2) + ax[:add_artist](convex_hull) + + plt_hull_bounds, = ax[:plot](conv_hull[:, 1], conv_hull[:, 2], color=c, linestyle="-", lw=2.0, alpha=0.3) + plt_states, = ax[:plot](x, y, color=c, marker="o", alpha=0.3) + + return convex_hull, plt_hull_bounds, plt_states +end + +function update_selection(convex_hull, plt_hull_bounds, plt_states, x::Array{Float64,1}, y::Array{Float64,1}) + # Plot convex hull of the selected_states + conv_hull_temp = qhull(x, y) + conv_hull = zeros(size(conv_hull_temp[1])[1] + 1, 2) + conv_hull[1 : end - 1, 1] = conv_hull_temp[1] + conv_hull[1 : end - 1, 2] = conv_hull_temp[2] + conv_hull[end, 1] = conv_hull[1, 1] + conv_hull[end, 2] = conv_hull[1, 2] + + convex_hull[:set_xy](conv_hull) + plt_hull_bounds[:set_data](conv_hull[:, 1], conv_hull[:, 2]) + plt_states[:set_data](x, y) +end + +function plot_closed_loop(ax, x::Array{Float64,1}, y::Array{Float64,1}, c) + closed_loop, = ax[:plot](x, y, color=c, linestyle="--", lw=2) + return closed_loop +end + +function update_closed_loop(closed_loop_plt, x::Array{Float64,1}, y::Array{Float64,1}, bufferSize::Int64) + x_full = vcat(x,NaN*ones(bufferSize-size(x,1))) + y_full = vcat(y,NaN*ones(bufferSize-size(y,1))) + closed_loop_plt[:set_data](x_full, y_full) +end + +if ARGS[1] == "prediction" + plt[:ion]() + + # Create figure and subplots + fig = figure("Prediction-$(file_names[idx])",figsize=(20,10)) + ax_s_ey = fig[:add_subplot](2, 3, 1) + xlabel("s [m]") + ylabel("e_y [m]") + + ax_s_epsi = fig[:add_subplot](2, 3, 2) + xlabel("s [m]") + ylabel("e_psi [rad]") + + ax_s_psidot = fig[:add_subplot](2, 3, 3) + xlabel("s [m]") + ylabel("psi_dot [rad / s]") + + ax_s_vx = fig[:add_subplot](2, 3, 4) + xlabel("s [m]") + ylabel("v_x [m / s]") + + ax_s_vy = fig[:add_subplot](2, 3, 5) + xlabel("s [m]") + ylabel("v_y [m / s]") + + ax_s_c = fig[:add_subplot](2, 3, 6) + xlabel("s [m]") + ylabel("curvature [1 / m]") + + ax = [ax_s_ey, ax_s_epsi, ax_s_vx, ax_s_vy, ax_s_psidot] + + # PLOT FINISHING LINE + for axe in ax + axe[:plot]([0,0],[-10,10],color="black", linestyle="-") + axe[:plot]([track.s,track.s],[-10,10],color="black", linestyle="-") + end + # PLOT THE BOUNDARY FOR STRAIGHT LINE AND CURVE + for axe in ax + for i = 1:size(track.curvature,1)-1 + if (track.curvature[i] == 0 && track.curvature[i+1] != 0) + axe[:plot]([i-1,i-1]*track.ds,[-10,10],color="grey", linestyle="-") + elseif (track.curvature[i] != 0 && track.curvature[i+1] == 0) + axe[:plot]([i-1,i-1]*track.ds,[-10,10],color="grey", linestyle="-.") + end + end + end + # PLOT THE TRACK BOUNDARY ON e_y + x = (track.idx-1)*track.ds + y = track.w/2*ones(size(x,1)) + ax_s_ey[:plot](x, -y, color="grey", linestyle="--") + ax_s_ey[:plot](x,0*y, color="grey", linestyle="--") + ax_s_ey[:plot](x, y, color="grey", linestyle="--") + + # SET THE Y-LIMITS FOR EACH SUBPLOT + for i = 1 : 5 + set_limits(ax[i], solHistory, i+1) + end + # ax_s_vy[:set_ylim]([-0.15, 0.15]) + ax_s_c[:set_ylim]([-1.2,1.2]) + + # INITIALIZE THE PLOTING FOR MPC PREDICTION + pre_hs = [] + for i = 1 : 5 + pre_h = plot_prediction(ax[i], NaN*ones(size(solHistory.z,3)), NaN*ones(size(solHistory.z,3)), "blue") + pre_hs = [pre_hs; pre_h] + end + + # INITIALIZE THE PLOTTING FOR TRUE PREDICTION WHICH NEEDS TO BE TURNED OFF DURING EXPERIMENT + true_hs = [] + for i = 1 : 5 + true_h = plot_prediction(ax[i], NaN*ones(size(solHistory.z,3)), NaN*ones(size(solHistory.z,3)), "black") + true_hs = [true_hs; true_h] + end + + # INITIALIZE THE PLOTTING FOR SAFESET + conv_hulls = [] + hull_bounds = [] + plt_states = [] + for i = 1 : 5 + conv_hull, hull_bound, plt_state = plot_selection(ax[i], NaN*ones(size(selectedStates.selStates[:, 1])), + NaN*ones(size(selectedStates.selStates[:, 1])), "blue") + conv_hulls = [conv_hulls; conv_hull] + hull_bounds = [hull_bounds; hull_bound] + plt_states = [plt_states; plt_state] + end + + # INITIALIZE THE PLOT FOR THE CLOSE LOOP TRAJECTORY + closed_loop_plts = []; bufferSize = 500 + for i = 1 : 5 + closed_loop_plt = plot_closed_loop(ax[i], NaN*ones(bufferSize), NaN*ones(bufferSize), "grey") + closed_loop_plts = [closed_loop_plts; closed_loop_plt] + end + + # INITIALIZE THE PLOT FOR CURVATURE + curvature_plt, = ax_s_c[:plot](NaN*ones(mpcParams.N+1), NaN*ones(mpcParams.N+1), color="blue", marker="o") + + fig_select = figure("Selected points-$(file_names[idx])",figsize=(20,10)) + ax_track_select = fig_select[:add_subplot](1, 1, 1) + ax_track_select[:plot](track.bound1xy[:,1],track.bound1xy[:,2],color="black") + ax_track_select[:plot](track.bound2xy[:,1],track.bound2xy[:,2],color="black") + select_plt, = ax_track_select[:plot](NaN*ones(size(selectedStates.selStates[:, 1])), + NaN*ones(size(selectedStates.selStates[:, 1])),"go",alpha=0.3) + car_plt, = ax_track_select[:plot](NaN,NaN,"ko",markersize=5,alpha=0.3) + + xlabel("x [m]") + ylabel("y [m]") + axis("equal") + + # UPDATE AND SAVE THE PLOT FOR EVERY LMPC LAPS + GP_flag = false + # if length(ARGS) == 2 + # lap = 1 + # elseif ARGS[3] == "LMPC" + # lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + # elseif ARGS[3] == "GP" + # lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + # GP_vy_plt, = ax_s_vy[:plot](NaN*ones(mpcParams.N),NaN*ones(mpcParams.N),"ro") + # GP_psidot_plt, = ax_s_psidot[:plot](NaN*ones(mpcParams.N),NaN*ones(mpcParams.N),"ro") + # GP_flag = true + # end + lap = 2 + max(selectedStates.Nl,selectedStates.feature_Nl) # starting lap of LMPC + + + while solHistory.cost[lap] > 10 + lapStatus.currentLap = lap + + # UPDATE THE PLOT OF THE CURRENT LAP CLOSE LOOP TRAJECTORY + for i = 1 : 5 + update_closed_loop(closed_loop_plts[i], solHistory.z[1:Int(solHistory.cost[lap]),lap,1,1], + solHistory.z[1:Int(solHistory.cost[lap]),lap,1,i+1], bufferSize) + end + + if lap <= 1 + max(selectedStates.Nl,selectedStates.feature_Nl) + # UPDATE THE PLOT OF THE CURRENT LAP CLOSE LOOP TRAJECTORY + for j = 1:Int(solHistory.cost[lap]) + # SIMULATE THE TRUE MODEL, WHICH SHOULD BE TURNED OFF FOR EXPERIMENTS + true_state, ~, ~ = car_pre_dyn(reshape(solHistory.z[j,lap,1,:],1,size(solHistory.z,4)), + reshape(solHistory.u[j,lap,:,:],size(solHistory.u,3),size(solHistory.u,4)),track,modelParams,6) + + # UPDATE THE PLOTTING DATA + for i = 1 : 5 + update_prediction(pre_hs[i], solHistory.z[j,lap,:,1], solHistory.z[j,lap,:,1+i], track) + update_prediction(true_hs[i], true_state[:,1], true_state[:,i+1], track) + update_limits(ax[i],solHistory.z[j,lap,:,1],true_state[:,1],NaN*ones(mpcParams.N+1)) + end + + # UPDATE THE CURVATURE PLOT + curvature=curvature_prediction(reshape(solHistory.z[j,lap,:,:],size(solHistory.z[j,lap,:,:],3),size(solHistory.z[j,lap,:,:],4)),track) + update_prediction(curvature_plt,solHistory.z[j,lap,:,1],curvature,track) + ax_s_c[:set_xlim]([findmin(solHistory.z[j,lap,:,1])[1]-0.1, findmax(solHistory.z[j,lap,:,1])[1] + 0.1]) + + fig[:savefig]("$(homedir())/simulations/animations/lap$(lap)_iteration$(j).png", dpi=100) + println("lap$(lap)_iteration$(j)") + end + else + for j = 1:Int(solHistory.cost[lap]) + # SIMULATE THE TRUE MODEL, WHICH SHOULD BE TURNED OFF FOR EXPERIMENTS + true_state, ~, ~ = car_pre_dyn(reshape(solHistory.z[j,lap,1,:],1,size(solHistory.z,4)), + reshape(solHistory.u[j,lap,:,:],size(solHistory.u,3),size(solHistory.u,4)),track,modelParams,6) + + # UPDATE THE PLOTTING DATA + for i = 1 : 5 + update_selection(conv_hulls[i], hull_bounds[i], plt_states[i], + reshape(selectHistory[j,lap,:,1],size(selectHistory[j,lap,:,1],3)), + reshape(selectHistory[j,lap,:,1+i],size(selectHistory[j,lap,:,1],3))) + update_prediction(pre_hs[i], solHistory.z[j,lap,:,1], solHistory.z[j,lap,:,1+i], track) + update_prediction(true_hs[i], true_state[:,1], true_state[:,i+1], track) + update_limits(ax[i],solHistory.z[j,lap,:,1],true_state[:,1],reshape(selectHistory[j,lap,:,1],size(selectHistory,3))) + + if select_flag + # FEATURE DATA PLOTTING UPDATE + (z_iden_x, z_iden_y) = trackFrame_to_xyFrame(reshape(selectFeatureHistory[j,lap,:,:],size(selectFeatureHistory,3),size(selectFeatureHistory,4)),track) + update_prediction(select_plt,z_iden_x,z_iden_y,track) + + # CAR POSITION PLOT UPDATE + (car_x,car_y)=trackFrame_to_xyFrame(reshape(solHistory.z[j,lap,1,:],1,6),track) + update_prediction(car_plt,car_x,car_y,track) + end + end + + if GP_flag + GP_vy = solHistory.z[j,lap,2:end,5]+GP_vy_History[j,lap,:] + GP_psidot = solHistory.z[j,lap,2:end,6]+GP_psidot_History[j,lap,:] + update_prediction(GP_vy_plt, solHistory.z[j,lap,2:end,1], GP_vy, track) + update_prediction(GP_psidot_plt, solHistory.z[j,lap,2:end,1], GP_psidot, track) + end + + # UPDATE THE CURVATURE PLOT + curvature=curvature_prediction(reshape(solHistory.z[j,lap,:,:],size(solHistory.z[j,lap,:,:],3),size(solHistory.z[j,lap,:,:],4)),track) + update_prediction(curvature_plt,solHistory.z[j,lap,:,1],curvature,track) + ax_s_c[:set_xlim]([findmin(solHistory.z[j,lap,:,1])[1]-0.1, findmax(solHistory.z[j,lap,:,1])[1] + 0.1]) + ax_s_c[:set_title](statusHistory[j,lap]) + + fig[:savefig]("$(homedir())/simulations/animations/lap$(lap)_iteration$(j).png", dpi=100) + if select_flag + fig_select[:savefig]("$(homedir())/simulations/animations/lap$(lap)_iteration$(j)_select.png", dpi=100) + end + println("lap$(lap)_iteration$(j)") + end + end + lap += 1 + if lap>length(solHistory.cost) + break + end + end +end + +if ARGS[1] == "GP" + data = load("$(homedir())/simulations/Feature_Data/FeatureData_GP-$(ARGS[2]).jld") + feature_GP_z = data["feature_GP_z"] + feature_GP_vy_e = data["feature_GP_vy_e"] + feature_GP_psidot_e = data["feature_GP_psidot_e"] + + fig = figure("GPR-$(file_names[idx])",figsize=(20,10)) + min_val = minimum(feature_GP_vy_e) + max_val = maximum(feature_GP_vy_e) + ax_s_eVy = fig[:add_subplot](2, 1, 1) + ax_s_eVy[:plot](feature_GP_vy_e,".") + for i = 1:length(feature_GP_vy_e)-1 + if feature_GP_z[i,1] > feature_GP_z[i+1,1] + ax_s_eVy[:plot]([i,i],[min_val,max_val],color="grey",linestyle="--") + end + end + xlabel("s [m]") + ylabel("e_vy [m/s]") + + min_val = minimum(feature_GP_psidot_e) + max_val = maximum(feature_GP_psidot_e) + ax_s_ePsidot = fig[:add_subplot](2, 1, 2) + ax_s_ePsidot[:plot](feature_GP_psidot_e,".") + for i = 1:length(feature_GP_vy_e)-1 + if feature_GP_z[i,1] > feature_GP_z[i+1,1] + ax_s_ePsidot[:plot]([i,i],[min_val,max_val],color="grey",linestyle="--") + end + end + xlabel("s [m]") + ylabel("e_Psi_dot [rad/s]") +end +show() \ No newline at end of file diff --git a/workspace/src/barc/src/simple_debugging.py b/workspace/src/barc/src/simple_debugging.py new file mode 100644 index 00000000..ae8d02f4 --- /dev/null +++ b/workspace/src/barc/src/simple_debugging.py @@ -0,0 +1,167 @@ +import numpy as np +import os +import sys +import pdb +import matplotlib.pyplot as plt +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/Library")) +from Localization_helpers import Track +# l = Track(0.01,0.8) +# l.createRaceTrack() + +# FIGURE 1 plotting of estimator output data +homedir = os.path.expanduser("~") +pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") +npz_output = np.load(pathSave) +x_est_his = npz_output["x_est_his"] +y_est_his = npz_output["y_est_his"] +vx_est_his =npz_output["vx_est_his"] +vy_est_his =npz_output["vy_est_his"] +ax_est_his =npz_output["ax_est_his"] +ay_est_his =npz_output["ay_est_his"] +psiDot_est_his =npz_output["psiDot_est_his"] +yaw_est_his =npz_output["yaw_est_his"] +estimator_time =npz_output["estimator_time"] + +pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") +npz_imu = np.load(pathSave) +psiDot_his = npz_imu["psiDot_his"] +roll_his = npz_imu["roll_his"] +pitch_his = npz_imu["pitch_his"] +yaw_his = npz_imu["yaw_his"] +ax_his = npz_imu["ax_his"] +ay_his = npz_imu["ay_his"] +imu_time = npz_imu["imu_time"] +imu_time = estimator_time + +pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") +npz_gps = np.load(pathSave) +x_his = npz_gps["x_his"] +y_his = npz_gps["y_his"] +gps_time = npz_gps["gps_time"] +gps_time = estimator_time + +pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") +npz_enc = np.load(pathSave) +v_fl_his = npz_enc["v_fl_his"] +v_fr_his = npz_enc["v_fr_his"] +v_rl_his = npz_enc["v_rl_his"] +v_rr_his = npz_enc["v_rr_his"] +enc_time = npz_enc["enc_time"] +enc_time = estimator_time + +pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") +npz_ecu = np.load(pathSave) +a_his = npz_ecu["a_his"] +df_his = npz_ecu["df_his"] +ecu_time = npz_ecu["ecu_time"] +ecu_time = estimator_time + + +# pdb.set_trace() +# FIGURE 1 plotting of estimator data +num_col_plt = 3 +num_row_plt = 1 +fig = plt.figure("Estimator") +ax1 = fig.add_subplot(num_col_plt,num_row_plt,1,ylabel="yaw_estimation") +ax1.plot(estimator_time,yaw_est_his,label="yaw_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_col_plt,num_row_plt,2,ylabel="v estimation") +ax2.plot(estimator_time,vx_est_his,label="vx_est") +ax2.plot(estimator_time,vy_est_his,label="vy_est") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_col_plt,num_row_plt,3,ylabel="acc & psidot estimation") +ax3.plot(estimator_time,ax_est_his,label="ax") +ax3.plot(estimator_time,ay_est_his,label="ay") +ax3.plot(estimator_time,psiDot_est_his,label="psiDot") +ax3.legend() +ax3.grid() +""" +# FIGURE 2 plotting of IMU data +num_plot = 3 +fig = plt.figure("Imu") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="IMU yaw") +# ax1.plot(imu_time,yaw_his, label="yaw") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="IMU acc & psidot") +ax2.plot(imu_time,psiDot_his,label="psiDot") +ax2.plot(imu_time,ax_his,label="ax") +ax2.plot(imu_time,ay_his,label="ay") +print np.mean(ax_his), np.mean(ay_his) +ax2.legend() +ax2.grid() +""" +ax3 = fig.add_subplot(num_plot,1,3,ylabel="pitch & roll angle") +# ax3.plot(imu_time,roll_his,label="roll angle") +# ax3.plot(imu_time,pitch_his,label="pitch angle") +ax3.legend() +ax3.grid() +# GPS comparison +num_plot = 2 +fig = plt.figure("GPS") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="x") +ax1.plot(gps_time, x_his, label="x") +ax1.plot(estimator_time, x_est_his, label="x_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="y") +ax2.plot(gps_time, y_his, label="y") +ax2.plot(estimator_time, y_est_his, label="y_est") +ax2.legend() +ax2.grid() +# ecu plot +fig = plt.figure("input") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(ecu_time, df_his, "-", label="cmd.df") +ax4.plot(ecu_time, a_his, "--", label="cmd.a") +ax4.legend() +ax4.grid() +# enc plot +fig = plt.figure("encoder") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(enc_time, v_fl_his, "--", label="fl") +ax4.plot(enc_time, v_fr_his, "--", label="fr") +ax4.plot(enc_time, v_rl_his, "-", label="rl") +ax4.plot(enc_time, v_rr_his, "-", label="rr") +ax4.legend() +ax4.grid() + +# trajectory +fig = plt.figure("track x-y plot") +ax1 = fig.add_subplot(1,1,1,ylabel="track x-y plot") +# ax1.plot(l.nodes[0],l.nodes[1],color="grey",linestyle="--", alpha=0.3) +# ax1.plot(l.nodes_bound1[0],l.nodes_bound1[1],color="red",alpha=0.3) +# ax1.plot(l.nodes_bound2[0],l.nodes_bound2[1],color="red",alpha=0.3) +ax1.axis("equal") +ax1.plot(x_est_his,y_est_his,color="green") +ax1.plot(x_his,y_his,color="blue") +ax1.legend() +""" +# pdb.set_trace() +# raw data and estimation data comparison +num_plot = 3 +fig = plt.figure("raw data and est data comparison") +ax2 = fig.add_subplot(num_plot,1,1,ylabel="ax") +ax2.plot(imu_time, ax_his, ".", label="ax_meas") +ax2.plot(estimator_time, ax_est_his, label="ax_est") +# ax2.plot(estimator_time, a_his, "--", label="cmd.acc") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,2,ylabel="ay") +ax3.plot(imu_time, ay_his, ".", label="ay_meas") +ax3.plot(estimator_time, ay_est_his, label="ay_est") +# ax3.plot(estimator_time, df_his, "--", label="cmd.df") +ax3.legend() +ax3.grid() +ax4 = fig.add_subplot(num_plot,1,3,ylabel="psidot") +ax4.plot(imu_time, psiDot_his, ".", label="psidot_meas") +ax4.plot(estimator_time,psiDot_est_his,label="psidot_est") +# ax4.plot(estimator_time, df_his, "--", label="cmd.df") +ax4.legend() +ax4.grid() +""" + +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/simple_debugging2.py b/workspace/src/barc/src/simple_debugging2.py new file mode 100644 index 00000000..11008fb1 --- /dev/null +++ b/workspace/src/barc/src/simple_debugging2.py @@ -0,0 +1,165 @@ +import numpy as np +import os +import sys +import pdb +import matplotlib.pyplot as plt +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/Library")) +from Localization_helpers import Track +# l = Track(0.01,0.8) +# l.createRaceTrack() + +# FIGURE 1 plotting of estimator output data +homedir = os.path.expanduser("~") +pathSave = os.path.join(homedir,"barc_debugging2/estimator_output.npz") +npz_output = np.load(pathSave) +x_est_his = npz_output["x_est_his"] +y_est_his = npz_output["y_est_his"] +vx_est_his =npz_output["vx_est_his"] +vy_est_his =npz_output["vy_est_his"] +ax_est_his =npz_output["ax_est_his"] +ay_est_his =npz_output["ay_est_his"] +psiDot_est_his =npz_output["psiDot_est_his"] +yaw_est_his =npz_output["yaw_est_his"] + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_imu.npz") +npz_imu = np.load(pathSave) +psiDot_his = npz_imu["psiDot_his"] +roll_his = npz_imu["roll_his"] +pitch_his = npz_imu["pitch_his"] +yaw_his = npz_imu["yaw_his"] +ax_his = npz_imu["ax_his"] +ay_his = npz_imu["ay_his"] + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_gps.npz") +npz_gps = np.load(pathSave) +x_his = npz_gps["x_his"] +y_his = npz_gps["y_his"] + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_enc.npz") +npz_enc = np.load(pathSave) +v_fl_his = npz_enc["v_fl_his"] +v_fr_his = npz_enc["v_fr_his"] +v_rl_his = npz_enc["v_rl_his"] +v_rr_his = npz_enc["v_rr_his"] + +pathSave = os.path.join(homedir,"barc_debugging2/estimator_ecu.npz") +npz_ecu = np.load(pathSave) +a_his = npz_ecu["a_his"] +df_his = npz_ecu["df_his"] + +""" +# pdb.set_trace() +# FIGURE 1 plotting of estimator data +num_col_plt = 3 +num_row_plt = 1 +fig = plt.figure("Estimator") +ax1 = fig.add_subplot(num_col_plt,num_row_plt,1,ylabel="yaw_estimation") +ax1.plot(yaw_est_his,label="yaw_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_col_plt,num_row_plt,2,ylabel="v estimation") +ax2.plot(vx_est_his,label="vx_est") +ax2.plot(vy_est_his,label="vy_est") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_col_plt,num_row_plt,3,ylabel="acc & psidot estimation") +ax3.plot(ax_est_his,label="ax") +ax3.plot(ay_est_his,label="ay") +ax3.plot(psiDot_est_his,label="psiDot") +ax3.legend() +ax3.grid() +# FIGURE 2 plotting of IMU data +num_plot = 3 +fig = plt.figure("Imu") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="IMU yaw") +ax1.plot(yaw_his, label="yaw") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="IMU acc & psidot") +ax2.plot(psiDot_his,label="psiDot") +ax2.plot(ax_his,label="ax") +ax2.plot(ay_his,label="ay") +print np.mean(ax_his), np.mean(ay_his) +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,3,ylabel="pitch & roll angle") +ax3.plot(roll_his,label="roll angle") +ax3.plot(pitch_his,label="pitch angle") +ax3.legend() +ax3.grid() +""" +# GPS comparison +num_plot = 2 +fig = plt.figure("GPS") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="x") +ax1.plot( x_his, label="x") +ax1.plot( x_est_his, label="x_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="y") +ax2.plot( y_his, label="y") +ax2.plot( y_est_his, label="y_est") +ax2.legend() +ax2.grid() +""" +# ecu plot +fig = plt.figure("input") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot( df_his, "-", label="cmd.df") +ax4.plot( a_his, "--", label="cmd.a") +ax4.legend() +ax4.grid() +# enc plot +fig = plt.figure("encoder") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot( v_rl_his, "-", label="rl") +ax4.plot( v_rr_his, "-", label="rr") +ax4.legend() +ax4.grid() +""" +# trajectory +fig = plt.figure("track x-y plot") +ax1 = fig.add_subplot(1,1,1,ylabel="track x-y plot") +# ax1.plot(l.nodes[0],l.nodes[1],color="grey",linestyle="--", alpha=0.3) +# ax1.plot(l.nodes_bound1[0],l.nodes_bound1[1],color="red",alpha=0.3) +# ax1.plot(l.nodes_bound2[0],l.nodes_bound2[1],color="red",alpha=0.3) +# num = 2000 +ax1.axis("equal") +# ax1.plot(x_est_his[-num:],y_est_his[-num:],color="green") +# ax1.plot(x_his[-num:],y_his[-num:],color="blue") +ax1.plot(x_est_his[:],y_est_his[:],color="green") +ax1.plot(x_his[:],y_his[:],color="blue") +# ax1.legend() +fig = plt.figure("encoder") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot( v_rl_his, "-", label="rl") +ax4.plot( v_rr_his, "-", label="rr") +ax4.plot( vx_est_his, "-", label="vx_est") +ax4.legend() +ax4.grid() + +""" +# raw data and estimation data comparison +num_plot = 3 +fig = plt.figure("raw data and est data comparison") +ax2 = fig.add_subplot(num_plot,1,1,ylabel="ax") +ax2.plot( ax_his, ".", label="ax_meas") +ax2.plot( ax_est_his, label="ax_est") +# ax2.plot(estimator_time, a_his, "--", label="cmd.acc") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,2,ylabel="ay") +ax3.plot( ay_his, ".", label="ay_meas") +ax3.plot( ay_est_his, label="ay_est") +# ax3.plot(estimator_time, df_his, "--", label="cmd.df") +ax3.legend() +ax3.grid() +ax4 = fig.add_subplot(num_plot,1,3,ylabel="psidot") +ax4.plot( psiDot_his, ".", label="psidot_meas") +ax4.plot(psiDot_est_his,label="psidot_est") +# ax4.plot(estimator_time, df_his, "--", label="cmd.df") +ax4.legend() +ax4.grid() +""" +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/simple_debugging_lukas.py b/workspace/src/barc/src/simple_debugging_lukas.py new file mode 100755 index 00000000..97a15e88 --- /dev/null +++ b/workspace/src/barc/src/simple_debugging_lukas.py @@ -0,0 +1,260 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +import pdb + +PLOT = False + +l_f = 0.125 +l_r = 0.125 + +homedir = os.path.expanduser("~") + +steering_str = homedir + "/barc_debugging/steering/" +acc_str = homedir + "/barc_debugging/acceleration/" +steering_dir = os.listdir(steering_str) +acc_dir = os.listdir(acc_str) + +X = 0. +d_f = 0. +pwm_entries = 0. + +first = True + +for file in steering_dir: + print file + pathSave = steering_str + file + "/estimator_imu.npz" + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + + pathSave = steering_str + file + "/estimator_enc.npz" + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + v_meas_his = npz_enc["v_meas_his"] + enc_time = npz_enc["enc_time"] + + v_his = 0.5 * (v_rl_his + v_rr_his) + # v_his = v_rr_his + + """ + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + npz_ecu = np.load(pathSave) + a_his = npz_ecu["a_his"] + df_his = npz_ecu["df_his"] + ecu_time = npz_ecu["ecu_time"] + """ + + # Calculation of steering angle + steering_pwm = float(file.split("_")[0]) + + mean_vel = np.mean(v_his) + first_index = np.argmax(v_his > mean_vel) + enc_time = enc_time[first_index:-25] + imu_time = imu_time[first_index:-25] + psi_dot = psiDot_his[first_index:-25] + v_x = v_his[first_index:-25] + + v_x = np.interp(imu_time, enc_time, v_x) + + d_f_ = np.arctan(psi_dot * (l_f + l_r) / v_x) + + pwm_entries_ = steering_pwm * np.ones_like(d_f_) + X_ = np.zeros((len(pwm_entries_), 2)) + X_[:, 0] = pwm_entries_ + X_[:, 1] = np.ones_like(d_f_) + + if first: + X = X_ + d_f = d_f_ + pwm_entries = pwm_entries_ + first = False + else: + X = np.vstack((X, X_)) + d_f = np.hstack((d_f, d_f_)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + if PLOT: + # FIGURE 2 plotting of IMU data + fig = plt.figure("Imu") + ax2 = fig.add_subplot(1,1,1,ylabel="IMU acc & psidot") + ax2.plot(imu_time, psi_dot, label="psiDot") + ax2.legend() + ax2.grid() + + # enc plot + fig = plt.figure("encoder") + ax4 = fig.add_subplot(1,1,1, ylabel="ax") + # ax4.plot(enc_time, v_fl_his, "--", label="fl") + # ax4.plot(enc_time, v_fr_his, "--", label="fr") + # ax4.plot(enc_time, v_rl_his, "-", label="rl") + ax4.plot(enc_time, v_x, "-", label="rr") + # ax4.plot(enc_time, v_meas_his, "-", label="meas") + ax4.legend() + ax4.grid() + + plt.show() + +theta = np.matmul(np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)), d_f) + +print theta + +theta_2 = np.zeros_like(theta) +theta_2[0] = 1.0 / theta[0] +theta_2[1] = - theta[1] / theta[0] + +print theta_2 + +pwms = np.array([50, 140]) +calc_steering = theta[0] * pwms + theta[1] + +# enc plot +fig = plt.figure("Steering Angle") +ax_df = fig.add_subplot(1,1,1, ylabel="ax") +ax_df.plot(pwm_entries, d_f, "*", label="d_f") +ax_df.plot(pwms, calc_steering, "-", label="calc_df") +ax_df.legend() +ax_df.grid() + +plt.show() + +v_dot = 0. +V = 0. +pwm_entries = 0. +C1_pos = 0. +C1_neg = 0. +C2_pos = 0. +C2_neg = 0. +CM = 0. +X = 0. + +first = True +positive = True + +for file in acc_dir: + print file + pathSave = acc_str + file + "/estimator_imu.npz" + npz_imu = np.load(pathSave) + psiDot_his = npz_imu["psiDot_his"] + roll_his = npz_imu["roll_his"] + pitch_his = npz_imu["pitch_his"] + yaw_his = npz_imu["yaw_his"] + ax_his = npz_imu["ax_his"] + ay_his = npz_imu["ay_his"] + imu_time = npz_imu["imu_time"] + + pathSave = acc_str + file + "/estimator_enc.npz" + npz_enc = np.load(pathSave) + v_fl_his = npz_enc["v_fl_his"] + v_fr_his = npz_enc["v_fr_his"] + v_rl_his = npz_enc["v_rl_his"] + v_rr_his = npz_enc["v_rr_his"] + v_meas_his = npz_enc["v_meas_his"] + enc_time = npz_enc["enc_time"] + + acc_pwm = float(file.split("_")[1]) + breaking_pwm = float(file.split("_")[2]) + + acc_start = np.argmax(v_rr_his > 0.5) + acc_max = np.argmax(v_rr_his) + acc_end = len(v_rr_his) - np.argmax(v_rr_his[:: -1] > 0.6) + + dummy_vrr = v_rr_his[acc_max : acc_end] + dummy_vrr2 = v_rr_his[acc_start : acc_max] + + for i in range(len(dummy_vrr2 - 1))[::-1]: + if dummy_vrr2[i - 1] == dummy_vrr2[i]: + dummy_vrr2 = np.delete(dummy_vrr2, i) + + for i in range(len(dummy_vrr - 1))[::-1]: + if dummy_vrr[i - 1] == dummy_vrr[i]: + dummy_vrr = np.delete(dummy_vrr, i) + + acc_pos = np.diff(dummy_vrr2) + acc_neg = np.diff(dummy_vrr) + + v_pos = dummy_vrr2[:-1] + v_neg = dummy_vrr[:-1] + + pwm_entries_ = acc_pwm * np.ones_like(acc_pos) + c1_pos = np.ones_like(acc_pos) + + X_ = np.transpose(np.vstack((pwm_entries_, c1_pos))) + + print X_.shape + + if positive: + if first: + v_dot = acc_pos + X = X_ + pwm_entries = pwm_entries_ + first = False + else: + print X_.shape + print X.shape + X = np.vstack((X, X_)) + v_dot = np.hstack((v_dot, acc_pos)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + c1_neg = np.ones_like(acc_neg) + pwm_entries_ = breaking_pwm * np.ones_like(acc_neg) + X_ = np.transpose(np.vstack((pwm_entries_, c1_neg))) + + if not positive: + if first: + v_dot = acc_neg + X = X_ + pwm_entries = pwm_entries_ + first = False + else: + print X_.shape + print X.shape + X = np.vstack((X, X_)) + v_dot = np.hstack((v_dot, acc_neg)) + pwm_entries = np.hstack((pwm_entries, pwm_entries_)) + + PLOT = True + if PLOT: + + # enc plot + fig = plt.figure("encoder") + ax4 = fig.add_subplot(1,1,1,ylabel="ax") + # ax4.plot(enc_time, v_fl_his, "--", label="fl") + # ax4.plot(enc_time, v_fr_his, "--", label="fr") + # ax4.plot(enc_time, v_rl_his, "-", label="rl") + ax4.plot(enc_time, v_rr_his, "-", label="rr") + ax4.plot(enc_time[acc_start], v_rr_his[acc_start], "ro") + ax4.plot(enc_time[acc_max], v_rr_his[acc_max], "ko") + ax4.plot(enc_time[acc_end], v_rr_his[acc_end], "mo") + + # ax4.plot(enc_time, v_meas_his, "-", label="meas") + ax4.legend() + ax4.grid() + + plt.show() + +theta = np.matmul(np.matmul(np.linalg.inv(np.matmul(np.transpose(X), X)), np.transpose(X)), v_dot) + +print theta + +pwms = np.array([40, 120]) +# Assuming velocity of 1.0 +calc_acc = theta[0] * pwms + theta[1] * 1 + +# enc plot +fig = plt.figure("Acceleration") +ax_df = fig.add_subplot(1,1,1, ylabel="ax") +ax_df.plot(pwm_entries, v_dot, "*", label="d_f") +ax_df.plot(pwms, calc_acc, "-", label="calc_df") +ax_df.legend() +ax_df.grid() + +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/simple_debugging_old.py b/workspace/src/barc/src/simple_debugging_old.py new file mode 100644 index 00000000..f24900b4 --- /dev/null +++ b/workspace/src/barc/src/simple_debugging_old.py @@ -0,0 +1,156 @@ +import numpy as np +import os +import matplotlib.pyplot as plt +from Localization_helpers import Track +l = Track(0.01,0.8) +l.createRaceTrack() + +# FIGURE 1 plotting of estimator output data +homedir = os.path.expanduser("~") +pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") +npz_output = np.load(pathSave) +x_est_his = npz_output["x_est_his"] +y_est_his = npz_output["y_est_his"] +vx_est_his =npz_output["vx_est_his"] +vy_est_his =npz_output["vy_est_his"] +ax_est_his =npz_output["ax_est_his"] +ay_est_his =npz_output["ay_est_his"] +psi_dot_est_his =npz_output["psi_dot_est_his"] +# yaw_est_his =npz_output["yaw_est_his"] +estimator_time =npz_output["estimator_time"] + +pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") +npz_imu = np.load(pathSave) +psidot_raw_his = npz_imu["psidot_raw_his"] +# roll_his = npz_imu["roll_his"] +# pitch_his = npz_imu["pitch_his"] +yaw_his = npz_imu["yaw_his"] +a_x_meas_his = npz_imu["a_x_meas_his"] +a_y_meas_his = npz_imu["a_y_meas_his"] +imu_time = npz_imu["imu_time"] + +pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") +npz_gps = np.load(pathSave) +x_his = npz_gps["x_his"] +y_his = npz_gps["y_his"] +gps_time = npz_gps["gps_time"] + +# pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") +# npz_enc = np.load(pathSave) +# v_fl_his = npz_enc["v_fl_his"] +# v_fr_his = npz_enc["v_fr_his"] +# v_rl_his = npz_enc["v_rl_his"] +# v_rr_his = npz_enc["v_rr_his"] +# enc_time = npz_enc["enc_time"] + +pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu_mess.npz") +npz_ecu = np.load(pathSave) +a_his = npz_ecu["a_his"] +df_his = npz_ecu["df_his"] +ecu_time = npz_ecu["ecu_time"] + +# # FIGURE 1 plotting of estimator data +# num_col_plt = 3 +# num_row_plt = 1 +# fig = plt.figure("Estimator") +# ax1 = fig.add_subplot(num_col_plt,num_row_plt,1,ylabel="yaw_estimation") +# ax1.plot(estimator_time,yaw_est_his,label="yaw_est") +# ax1.legend() +# ax1.grid() +# ax2 = fig.add_subplot(num_col_plt,num_row_plt,2,ylabel="v estimation") +# ax2.plot(estimator_time,vx_est_his,label="vx_est") +# ax2.plot(estimator_time,vy_est_his,label="vy_est") +# ax2.legend() +# ax2.grid() +# ax3 = fig.add_subplot(num_col_plt,num_row_plt,3,ylabel="acc & psidot estimation") +# ax3.plot(estimator_time,ax_est_his,label="ax") +# ax3.plot(estimator_time,ay_est_his,label="ay") +# ax3.plot(estimator_time,psiDot_est_his,label="psiDot") +# ax3.legend() +# ax3.grid() + +# # FIGURE 2 plotting of IMU data +# num_plot = 3 +# fig = plt.figure("Imu") +# ax1 = fig.add_subplot(num_plot,1,1,ylabel="IMU yaw") +# ax1.plot(imu_time,yaw_his, label="yaw") +# ax1.legend() +# ax1.grid() +# ax2 = fig.add_subplot(num_plot,1,2,ylabel="IMU acc & psidot") +# ax2.plot(imu_time,psiDot_his,label="psiDot") +# ax2.plot(imu_time,ax_his,label="ax") +# ax2.plot(imu_time,ay_his,label="ay") +# print np.mean(ax_his), np.mean(ay_his) +# ax2.legend() +# ax2.grid() +# ax3 = fig.add_subplot(num_plot,1,3,ylabel="pitch & roll angle") +# ax3.plot(imu_time,roll_his,label="roll angle") +# ax3.plot(imu_time,pitch_his,label="pitch angle") +# ax3.legend() +# ax3.grid() + +# GPS comparison +num_plot = 2 +fig = plt.figure("GPS") +ax1 = fig.add_subplot(num_plot,1,1,ylabel="x") +ax1.plot(gps_time, x_his, label="x") +ax1.plot(estimator_time, x_est_his, label="x_est") +ax1.legend() +ax1.grid() +ax2 = fig.add_subplot(num_plot,1,2,ylabel="y") +ax2.plot(gps_time, y_his, label="y") +ax2.plot(estimator_time, y_est_his, label="y_est") +ax2.legend() +ax2.grid() + +# ecu plot +fig = plt.figure("input") +ax4 = fig.add_subplot(1,1,1,ylabel="ax") +ax4.plot(ecu_time, df_his, "-", label="cmd.df") +ax4.plot(ecu_time, a_his, "--", label="cmd.a") +ax4.legend() +ax4.grid() + +# # enc plot +# fig = plt.figure("encoder") +# ax4 = fig.add_subplot(1,1,1,ylabel="ax") +# ax4.plot(enc_time, v_fl_his, "--", label="fl") +# ax4.plot(enc_time, v_fr_his, "--", label="fr") +# ax4.plot(enc_time, v_rl_his, "-", label="rl") +# ax4.plot(enc_time, v_rr_his, "-", label="rr") +# ax4.legend() +# ax4.grid() + +# # trajectory +# fig = plt.figure("track x-y plot") +# ax1 = fig.add_subplot(1,1,1,ylabel="track x-y plot") +# ax1.plot(l.nodes[0],l.nodes[1],color="grey",linestyle="--", alpha=0.3) +# ax1.plot(l.nodes_bound1[0],l.nodes_bound1[1],color="red",alpha=0.3) +# ax1.plot(l.nodes_bound2[0],l.nodes_bound2[1],color="red",alpha=0.3) +# ax1.axis("equal") +# ax1.plot(x_est_his,y_est_his,color="green") +# ax1.legend() + +# raw data and estimation data comparison +num_plot = 3 +fig = plt.figure("raw data and est data comparison") +ax2 = fig.add_subplot(num_plot,1,1,ylabel="ax") +ax2.plot(imu_time, a_x_meas_his, ".", label="ax_meas") +ax2.plot(estimator_time, ax_est_his, label="ax_est") +# ax2.plot(estimator_time, a_his, "--", label="cmd.acc") +ax2.legend() +ax2.grid() +ax3 = fig.add_subplot(num_plot,1,2,ylabel="ay") +ax3.plot(imu_time, a_y_meas_his, ".", label="ay_meas") +ax3.plot(estimator_time, ay_est_his, label="ay_est") +# ax3.plot(estimator_time, df_his, "--", label="cmd.df") +ax3.legend() +ax3.grid() +ax4 = fig.add_subplot(num_plot,1,3,ylabel="psidot") +ax4.plot(imu_time, psidot_raw_his, ".", label="psidot_meas") +ax4.plot(estimator_time,psi_dot_est_his,label="psidot_est") +# ax4.plot(estimator_time, df_his, "--", label="cmd.df") +ax4.legend() +ax4.grid() + +plt.show() \ No newline at end of file diff --git a/workspace/src/barc/src/state_estimation_DynBkMdl.py b/workspace/src/barc/src/state_estimation_DynBkMdl.py deleted file mode 100755 index c3e0b57b..00000000 --- a/workspace/src/barc/src/state_estimation_DynBkMdl.py +++ /dev/null @@ -1,187 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -import time -import os -from barc.msg import ECU, Encoder -from sensor_msgs.msg import Imu -from geometry_msgs.msg import Vector3 -from numpy import pi, cos, sin, eye, array, zeros -from ekf import ekf -from system_models import f_3s, h_3s -from tf import transformations -from numpy import unwrap - -# input variables -d_f = 0 -FxR = 0 - -# raw measurement variables -yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) - -# from encoder -v_x_enc = 0 -t0 = time.time() -n_FL = 0 # counts in the front left tire -n_FR = 0 # counts in the front right tire -n_FL_prev = 0 -n_FR_prev = 0 -r_tire = 0.04 # radius from tire center to perimeter along magnets [m] -dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge - -# ecu command update -def ecu_callback(data): - global FxR, d_f - FxR = data.motor # input motor force [N] - d_f = data.servo # input steering angle [rad] - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z - global yaw_prev - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw - - # extract angular velocity and linear acceleration data - w_x = data.angular_velocity.x - w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z - -# encoder measurement update -def enc_callback(data): - global v_x_enc, d_f, t0 - global n_FL, n_FR, n_FL_prev, n_FR_prev - - n_FL = data.FL - n_FR = data.FR - - # compute time elapsed - tf = time.time() - dt = tf - t0 - - # if enough time elapse has elapsed, estimate v_x - dt_min = 0.20 - if dt >= dt_min: - # compute speed : speed = distance / time - v_FL = float(n_FL- n_FL_prev)*dx_qrt/dt - v_FR = float(n_FR- n_FR_prev)*dx_qrt/dt - - # update encoder v_x, v_y measurements - # only valid for small slip angles, still valid for drift? - v_x_enc = (v_FL + v_FR)/2.0*cos(d_f) - - # update old data - n_FL_prev = n_FL - n_FR_prev = n_FR - t0 = time.time() - - -# state estimation node -def state_estimation(): - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('encoder', Encoder, enc_callback) - rospy.Subscriber('ecu', ECU, ecu_callback) - state_pub = rospy.Publisher('state_estimate', Vector3, queue_size = 10) - - # get vehicle dimension parameters - L_a = rospy.get_param("L_a") # distance from CoG to front axel - L_b = rospy.get_param("L_b") # distance from CoG to rear axel - m = rospy.get_param("m") # mass of vehicle - I_z = rospy.get_param("I_z") # moment of inertia about z-axis - vhMdl = (L_a, L_b, m, I_z) - - # get encoder parameters - dt_vx = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x - - # get tire model - B = rospy.get_param("tire_model/B") - C = rospy.get_param("tire_model/C") - mu = rospy.get_param("tire_model/mu") - TrMdl = ([B,C,mu],[B,C,mu]) - - # get external force model - a0 = rospy.get_param("air_drag_coeff") - Ff = rospy.get_param("friction") - - # get EKF observer properties - q_std = rospy.get_param("state_estimation/q_std") # std of process noise - r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise - v_x_min = rospy.get_param("state_estimation/v_x_min") # minimum velociy before using EKF - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = time.time() - - # estimation variables for Luemberger observer - z_EKF = array([1.0, 0.0, 0.0]) - - # estimation variables for EKF - P = eye(3) # initial dynamics coveriance matrix - Q = (q_std**2)*eye(3) # process noise coveriance matrix - R = (r_std**2)*eye(2) # measurement noise coveriance matrix - - while not rospy.is_shutdown(): - - # publish state estimate - (v_x, v_y, r) = z_EKF # note, r = EKF estimate yaw rate - - # publish information - state_pub.publish( Vector3(v_x, v_y, r) ) - - # apply EKF - if v_x_enc > v_x_min: - # get measurement - y = array([v_x_enc, w_z]) - - # define input - u = array([ d_f, FxR ]) - - # build extra arguments for non-linear function - F_ext = array([ a0, Ff ]) - args = (u, vhMdl, TrMdl, F_ext, dt) - - # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_3s, z_EKF, P, h_3s, y, Q, R, args ) - - else: - z_EKF[0] = float(v_x_enc) - z_EKF[2] = float(w_z) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_KinBkMdl.py b/workspace/src/barc/src/state_estimation_KinBkMdl.py deleted file mode 100755 index 46aba05f..00000000 --- a/workspace/src/barc/src/state_estimation_KinBkMdl.py +++ /dev/null @@ -1,194 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import rospy -import time -import os -from sensor_msgs.msg import Imu -from barc.msg import ECU, Encoder, Z_KinBkMdl -from numpy import pi, cos, sin, eye, array, zeros, unwrap -from ekf import ekf -from system_models import f_KinBkMdl, h_KinBkMdl -from tf import transformations -from numpy import unwrap - -# input variables [default values] -d_f = 0 # steering angle [deg] -acc = 0 # acceleration [m/s] - -# raw measurement variables -yaw_prev = 0 -(roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z) = zeros(9) -yaw_prev = 0 -yaw_local = 0 -read_yaw0 = False -psi = 0 -psi_meas = 0 - -# from encoder -v = 0 -v_meas = 0 -t0 = time.time() -n_FL = 0 # counts in the front left tire -n_FR = 0 # counts in the front right tire -n_BL = 0 # counts in the back left tire -n_BR = 0 # counts in the back right tire -n_FL_prev = 0 -n_FR_prev = 0 -n_BL_prev = 0 -n_BR_prev = 0 -r_tire = 0.036 # radius from tire center to perimeter along magnets [m] -dx_qrt = 2.0*pi*r_tire/4.0 # distance along quarter tire edge [m] - -# ecu command update -def ecu_callback(data): - global acc, d_f - acc = data.motor # input acceleration - d_f = data.servo # input steering angle - -# imu measurement update -def imu_callback(data): - # units: [rad] and [rad/s] - global roll, pitch, yaw, a_x, a_y, a_z, w_x, w_y, w_z - global yaw_prev, yaw0, read_yaw0, yaw_local, psi_meas - - # get orientation from quaternion data, and convert to roll, pitch, yaw - # extract angular velocity and linear acceleration data - ori = data.orientation - quaternion = (ori.x, ori.y, ori.z, ori.w) - (roll, pitch, yaw) = transformations.euler_from_quaternion(quaternion) - - # save initial measurements - if not read_yaw0: - read_yaw0 = True - yaw_prev = yaw - yaw0 = yaw - - # unwrap measurement - yaw = unwrap(array([yaw_prev, yaw]), discont = pi)[1] - yaw_prev = yaw - yaw_local = yaw - yaw0 - psi_meas = yaw_local - - # extract angular velocity and linear acceleration data - w_x = data.angular_velocity.x - w_y = data.angular_velocity.y - w_z = data.angular_velocity.z - a_x = data.linear_acceleration.x - a_y = data.linear_acceleration.y - a_z = data.linear_acceleration.z - -# encoder measurement update -def enc_callback(data): - global v, t0, dt_v_enc, v_meas - global n_FL, n_FR, n_FL_prev, n_FR_prev - global n_BL, n_BR, n_BL_prev, n_BR_prev - - n_FL = data.FL - n_FR = data.FR - n_BL = data.BL - n_BR = data.BR - - # compute time elapsed - tf = time.time() - dt = tf - t0 - - # if enough time elapse has elapsed, estimate v_x - if dt >= dt_v_enc: - # compute speed : speed = distance / time - v_FL = float(n_FL - n_FL_prev)*dx_qrt/dt - v_FR = float(n_FR - n_FR_prev)*dx_qrt/dt - v_BL = float(n_BL - n_BL_prev)*dx_qrt/dt - v_BR = float(n_BR - n_BR_prev)*dx_qrt/dt - - # Uncomment/modify according to your encoder setup - # v_meas = (v_FL + v_FR)/2.0 - # Modification for 3 working encoders - v_meas = (v_FL + v_BL + v_BR)/3.0 - # Modification for bench testing (driven wheels only) - # v = (v_BL + v_BR)/2.0 - - # update old data - n_FL_prev = n_FL - n_FR_prev = n_FR - n_BL_prev = n_BL - n_BR_prev = n_BR - t0 = time.time() - - -# state estimation node -def state_estimation(): - global dt_v_enc - global v_meas, psi_meas - # initialize node - rospy.init_node('state_estimation', anonymous=True) - - # topic subscriptions / publications - rospy.Subscriber('imu/data', Imu, imu_callback) - rospy.Subscriber('encoder', Encoder, enc_callback) - rospy.Subscriber('ecu', ECU, ecu_callback) - state_pub = rospy.Publisher('state_estimate', Z_KinBkMdl, queue_size = 10) - - # get vehicle dimension parameters - L_a = rospy.get_param("L_a") # distance from CoG to front axel - L_b = rospy.get_param("L_b") # distance from CoG to rear axel - vhMdl = (L_a, L_b) - - # get encoder parameters - dt_v_enc = rospy.get_param("state_estimation/dt_v_enc") # time interval to compute v_x from encoders - - # get EKF observer properties - q_std = rospy.get_param("state_estimation/q_std") # std of process noise - r_std = rospy.get_param("state_estimation/r_std") # std of measurementnoise - - # set node rate - loop_rate = 50 - dt = 1.0 / loop_rate - rate = rospy.Rate(loop_rate) - t0 = time.time() - - # estimation variables for Luemberger observer - z_EKF = zeros(4) - - # estimation variables for EKF - P = eye(4) # initial dynamics coveriance matrix - Q = (q_std**2)*eye(4) # process noise coveriance matrix - R = (r_std**2)*eye(2) # measurement noise coveriance matrix - - while not rospy.is_shutdown(): - - # publish state estimate - (x, y, psi, v) = z_EKF - - # publish information - state_pub.publish( Z_KinBkMdl(x, y, psi, v) ) - - # collect measurements, inputs, system properties - # collect inputs - y = array([psi_meas, v_meas]) - u = array([ d_f, acc ]) - args = (u,vhMdl,dt) - - # apply EKF and get each state estimate - (z_EKF,P) = ekf(f_KinBkMdl, z_EKF, P, h_KinBkMdl, y, Q, R, args ) - - # wait - rate.sleep() - -if __name__ == '__main__': - try: - state_estimation() - except rospy.ROSInterruptException: - pass diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas.py new file mode 100755 index 00000000..8cf99aac --- /dev/null +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas.py @@ -0,0 +1,645 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton + +# This estimator was developed by Shuqi Xu (shuqixu@berkeley.edu) +# --------------------------------------------------------------------------- + +import rospy +# from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np +import os + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = 0.0 + df_delay = 0.0 + loop_rate = 50.0 + + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0., 1., 0.0]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.2 * 10., 0.2 * 10., 0.1, 10., 1., 0.1, 100 * 0.01]) + + """ + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0., 1.]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.1 * 10., 0.1 * 10., 0.1, 10., 10., 0.1, 100.0 * 0.01]) + """ + """ + + # x, y, vx, vy, ax, ay, psi, psidot + Q = diag([0., 0., 0., 0., 1., 1., 0.1, 1.]) + # x_meas, y_meas, vel_meas, a_x_meas, a_y_meas, psiDot_meas, vy_meas + R = diag([0.5 * 10., 0.5 * 10., 0.1, 10., 10., 0.1, 0.01]) + """ + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + # track = Track(0.01,0.8) + # track.createRaceTrack() + estMsg = pos_info() + + while not rospy.is_shutdown(): + if ecu.a != 0: + est.estimateState(imu,gps,enc,ecu,est.ukf) + else: + est.x_est = 0.1 + + + # estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + + imu.saveHistory() + gps.saveHistory() + enc.saveHistory() + ecu.saveHistory() + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + estimator_time = est.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + gps_time = gps.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDrift_est = 0.0 + self.curr_time = 0.0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + # u = [self.a_his.pop(0), self.df_his.pop(0)] + u = [ecu.a, self.df_his.pop(0)] + + bta = 0.5 * u[1] + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, sin(bta)*enc.v_meas]) + # y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + + KF(y,u) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est, _) = self.z + + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est, _ ) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*9 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # psidot_drift + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7]+x[8] # psiDot + y[6] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [0.0] + self.psiDot_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.roll_his = [0.0] + self.pitch_his = [0.0] + + # time stamp + self.t0 = t0 + self.time_his = [0.0] + + # Time for yawDot integration + self.curr_time = 0.0 + self.prev_time = 0.0 + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.prev_time = self.curr_time + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + + # GPS measurement history + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([0.0]) + self.curr_time = 0.0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # n_intplt = 20 + # if size(self.x_his,0) > n_intplt: # do interpolation when there is enough points + # x_intplt = self.x_his[-n_intplt:] + # y_intplt = self.y_his[-n_intplt:] + # t_intplt = self.time_his[-n_intplt:] + # t_matrix = vstack([t_intplt**2, t_intplt, ones(sz)]).T + # self.c_X = linalg.lstsq(t_matrix, x_intplt)[0] + # self.c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + # self.x = polyval(self.c_X, self.curr_time) + # self.y = polyval(self.c_Y, self.curr_time) + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.use_both_encoders = False + else: + self.use_both_encoders = True + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [0.0] + self.v_fr_his = [0.0] + self.v_rl_his = [0.0] + self.v_rr_his = [0.0] + self.v_meas_his = [0.0] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [0.0] + self.curr_time = 0.0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + + if self.use_both_encoders: + v_est = (self.v_rl + self.v_rr)/2 + else: + v_est = self.v_rr + + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = 0.0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas_messy.py b/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas_messy.py new file mode 100755 index 00000000..80de04f3 --- /dev/null +++ b/workspace/src/barc/src/state_estimation_SensorKinematicModel_lukas_messy.py @@ -0,0 +1,685 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +# from Localization_helpers import Localization +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_pos, hedge_imu_fusion, hedge_imu_raw, hedge_pos_ang +from std_msgs.msg import Header +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append +from numpy import ones, polyval, delete, size +from observers import ekf +from system_models import f_SensorKinematicModel, h_SensorKinematicModel +from tf import transformations +import math +import numpy as np +import os + +# ***_meas are values that are used by the Kalman filters +# ***_raw are raw values coming from the sensors + +class StateEst(object): + """This class contains all variables that are read from the sensors and then passed to + the Kalman filter.""" + # input variables + cmd_servo = 0.0 + cmd_motor = 0.0 + cmd_t = 0.0 + + # IMU + yaw_prev = 0.0 + yaw0 = 0.0 # yaw at t = 0 + yaw_meas = 0.0 + psiDot_meas = 0.0 + a_x_meas = 0.0 + a_y_meas = 0.0 + imu_updated = False + att = (0.0,0.0,0.0) # attitude + # history data storage for debugging + roll_raw_his = [0.0] + pitch_raw_his = [0.0] + yaw_raw_his = [0.0] + yaw_his = [0.0] # yaw after unwrap + yaw0_his = [0.0] + yaw_meas_his = [0.0] + psidot_raw_his = [0.0] + a_x_raw_his = [0.0] + a_y_raw_his = [0.0] + a_x_meas_his = [0.0] + a_y_meas_his = [0.0] + imu_time = [0.0] + imu_prev_time = 0.0 + + # Velocity + vel_meas = 0.0 + vel_updated = False + vel_prev = 0.0 + vel_count = 0.0 # this counts how often the same vel measurement has been received + + # GPS + x_meas = 0.0 + y_meas = 0.0 + x2_meas = 0.0 + y2_meas = 0.0 + gps_updated = False + x_hist = zeros(15) + y_hist = zeros(15) + t_gps = zeros(15) + c_X = array([0,0,0]) + c_Y = array([0,0,0]) + yaw_gps_meas_his = [0.0] + yaw_gps_meas = 0.0 + # GPS IMU FUSION + gps_x_his = [0.0] + gps_y_his = [0.0] + gps_time = [0.0] + + qx_his = [0.0] + qy_his = [0.0] + qz_his = [0.0] + qw_his = [0.0] + ax_his = [0.0] + ay_his = [0.0] + az_his = [0.0] + vx_his = [0.0] + vy_his = [0.0] + vz_his = [0.0] + gps_imu_fusion_time = [0.0] + + # GPS IMU RAW + acc_x_his = [0.0] + acc_y_his = [0.0] + acc_z_his = [0.0] + gyro_x_his = [0.0] + gyro_y_his = [0.0] + gyro_z_his = [0.0] + compass_x_his = [0.0] + compass_y_his = [0.0] + compass_z_his = [0.0] + gps_imu_raw_time = [0.0] + + # real_val: true data + x_true = 0.0 + y_true = 0.0 + vx_true = 0.0 + vy_true = 0.0 + yaw_true = 0.0 + psidot_true = 0.0 + yaw_true_his = [0.0] + psiDot_true_his = [0.0] + + # Estimator data + x_est = 0.0 + y_est = 0.0 + + # General variables + t0 = 0.0 # Time when the estimator was started + running = False # bool if the car is driving + + def __init__(self): + self.x_meas = 0 + + # ecu command update + def ecu_callback(self, data): + self.cmd_motor = data.motor # input motor force [N] + self.cmd_servo = data.servo # input steering angle [rad] + if not self.running: # set 'running' to True once the first command is received -> here yaw is going to be set to zero + self.running = True + + # TRUE CALL BACK: only for simulator + def true_callback(self, data): + self.x_true = data.x + self.y_true = data.y + self.vx_true = data.v_x + self.vy_true = data.v_y + self.yaw_true = data.psi + self.psidot_true = data.psiDot + # FOR DEBUGGING purpose + self.yaw_true_his.append(data.psi) + self.psiDot_true_his.append(data.psiDot) + + # ultrasound gps data + def gps_callback(self, data): + """This function is called when a new GPS signal is received.""" + # units: [rad] and [rad/s] + # get current time stamp + t_now = rospy.get_rostime().to_sec()-self.t0 + + #t_msg = data.timestamp_ms/1000.0 - self.t0 + + t_msg = t_now + + # if abs(t_now - t_msg) > 0.1: + # print "GPS: Bad synchronization - dt = %f"%(t_now-t_msg) + + # get current gps measurement + # self.x_meas = (data.x_m + self.x2_meas)/2 + # self.y_meas = (data.y_m + self.y2_meas)/2 + self.x_meas = data.x_m + self.y_meas = data.y_m + #print "Received coordinates : (%f, %f)" % (self.x_meas, self.y_meas) + + # check if we have good measurement + # compute distance we have travelled from previous estimate to current measurement + # if we've travelled more than 1 m, the GPS measurement is probably junk, so ignore it + # otherwise, store measurement, and then perform interpolation + dist = (self.x_est-data.x_m)**2 + (self.y_est-data.y_m)**2 + if dist < 1.0: + self.x_hist = append(self.x_hist, data.x_m) + self.y_hist = append(self.y_hist, data.y_m) + self.t_gps = append(self.t_gps, t_msg) + + # self.x_hist = delete(self.x_hist,0) + # self.y_hist = delete(self.y_hist,0) + # self.t_gps = delete(self.t_gps,0) + + # Keep only the last second worth of coordinate data in the x_hist and y_hist buffer + # These buffers are used for interpolation + # without overwriting old data, the arrays would grow unbounded + self.x_hist = self.x_hist[self.t_gps > t_now-1.0] + self.y_hist = self.y_hist[self.t_gps > t_now-1.0] + self.t_gps = self.t_gps[self.t_gps > t_now-1.0] + sz = size(self.t_gps, 0) + + # perform interpolation for (x,y) as a function of time t + # getting two function approximations x(t) and y(t) + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + # use least squares to get the coefficients for this function approximation + # using (x,y) coordinate data from the past second (time) + if sz > 4: + t_matrix = vstack([self.t_gps**2, self.t_gps, ones(sz)]).T # input matrix: [ t^2 t 1 ] + self.c_X = linalg.lstsq(t_matrix, self.x_hist)[0] + self.c_Y = linalg.lstsq(t_matrix, self.y_hist)[0] + self.gps_updated = True + + self.gps_x_his.append(data.x_m) + self.gps_y_his.append(data.y_m) + # self.gps_time.append(rospy.get_rostime().to_sec()-self.t0) + + # def gps2_callback(self, data): + # self.x2_meas = data.x_m + # self.y2_meas = data.y_m + + def gps_ang_callback(self, data): + yaw = math.radians(data.angle+90) + if not self.running: + self.yaw_prev = 0 + else: + self.yaw_gps_meas = np.unwrap([self.yaw_prev,yaw])[1] + self.yaw_prev = self.yaw_gps_meas + self.gps_time.append(rospy.get_rostime().to_sec()-self.t0) + self.yaw_gps_meas_his.append(self.yaw_gps_meas) + + + def gps_imu_fusion_callback(self,data): + # GPS IMU FUSION + self.gps_imu_fusion_time.append(rospy.get_rostime().to_sec()-self.t0) + self.qx_his.append(data.qx) + self.qy_his.append(data.qy) + self.qz_his.append(data.qz) + self.qw_his.append(data.qw) + self.ax_his.append(data.ax) + self.ay_his.append(data.ay) + self.az_his.append(data.az) + self.vx_his.append(data.vx) + self.vy_his.append(data.vy) + self.vz_his.append(data.vz) + + def gps_imu_raw_callback(self,data): + # GPS IMU RAW + self.gps_imu_raw_time.append(rospy.get_rostime().to_sec()-self.t0) + self.acc_x_his.append(data.acc_x) + self.acc_y_his.append(data.acc_y) + self.acc_z_his.append(data.acc_z) + self.gyro_x_his.append(data.gyro_x) + self.gyro_y_his.append(data.gyro_y) + self.gyro_z_his.append(data.gyro_z) + self.compass_x_his.append(data.compass_x) + self.compass_y_his.append(data.compass_y) + self.compass_z_his.append(data.compass_z) + + # imu measurement update + def imu_callback(self, data): + # units: [rad] and [rad/s] + current_t = rospy.get_rostime().to_sec() + + # get orientation from quaternion data, and convert to roll, pitch, yaw + # extract angular velocity and linear acceleration data + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, yaw_raw) = transformations.euler_from_quaternion(quaternion) + # yaw_meas is element of [-pi,pi] + + # yaw = unwrap([self.yaw_prev, yaw_raw])[1] # get smooth yaw (from beginning) + # self.yaw_prev = self.yaw_meas # and always use raw measured yaw for unwrapping + # if not self.running: + # self.yaw0 = yaw # set yaw0 to current yaw + # self.yaw_meas = 0 # and current yaw to zero + # else: + # self.yaw_meas = yaw - self.yaw0 + + # self.yaw_meas = yaw - self.yaw0 + + if self.imu_prev_time > 0: + self.yaw_meas += self.psiDot_meas * (current_t-self.imu_prev_time) + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot_meas = w_z + self.a_x_meas = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.a_y_meas = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.att = (roll_raw,pitch_raw,yaw_raw) + self.imu_updated = True + + # FOR DEBUGGING PURPOSE + self.imu_time.append(current_t-self.t0) + self.psidot_raw_his.append(w_z) + self.a_x_raw_his.append(a_x) + self.a_y_raw_his.append(a_y) + self.a_x_meas_his.append(self.a_x_meas) + self.a_y_meas_his.append(self.a_y_meas) + self.roll_raw_his.append(roll_raw) + self.pitch_raw_his.append(pitch_raw) + self.yaw_raw_his.append(yaw_raw) + # self.yaw_his.append(yaw) # yaw after unwrap() + # self.yaw0_his.append(self.yaw0) # yaw after unwrap() + self.yaw_meas_his.append(self.yaw_meas) + + self.imu_prev_time = current_t + + + def encoder_vel_callback(self, data): + # if data.vel_est != self.vel_prev: + # self.vel_meas = data.vel_est + # self.vel_updated = True + # self.vel_prev = data.vel_est + # self.vel_count = 0 + # else: + # self.vel_count = self.vel_count + 1 + # if self.vel_count > 10: # if 10 times in a row the same measurement + # self.vel_meas = 0 # set velocity measurement to zero + # self.vel_updated = True + # self.vel_meas = data.vel_est + # self.vel_updated = True + + # Average both rear speed + v_est = (data.vel_bl + data.vel_br)/2 + if v_est != self.vel_prev: + self.vel_meas = v_est + self.vel_updated = True + self.vel_prev = v_est + self.vel_count = 0 + else: + self.vel_count = self.vel_count + 1 + if self.vel_count > 10: # if 10 times in a row the same measurement + self.vel_meas = 0 # set velocity measurement to zero + self.vel_updated = True + + + +# state estimation node +def state_estimation(): + se = StateEst() + # initialize node + rospy.init_node('state_estimation', anonymous=True) + + # topic subscriptions / publications + rospy.Subscriber('imu/data', Imu, se.imu_callback, queue_size=1) + rospy.Subscriber('vel_est', Vel_est, se.encoder_vel_callback, queue_size=1) + rospy.Subscriber('ecu', ECU, se.ecu_callback, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + + # Simulations and Experiment will subscribe to different GPS topis + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, se.gps_callback, queue_size=1) + # rospy.Subscriber('hedge_imu_fusion_2', hedge_imu_fusion, se.gps2_callback, queue_size=1) + rospy.Subscriber('hedge_pos_ang', hedge_pos_ang, se.gps_ang_callback, queue_size=1) + + + # rospy.Subscriber('real_val', pos_info, se.true_callback, queue_size=1) + # rospy.Subscriber('hedge_pos', hedge_pos, se.gps_callback, queue_size=1) + + # for debugging to collect the data + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, se.gps_imu_fusion_callback, queue_size=1) + rospy.Subscriber('hedge_imu_raw', hedge_imu_raw, se.gps_imu_raw_callback, queue_size=1) + + + + # get vehicle dimension parameters + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + vhMdl = (L_f, L_r) + + # set node rate + loop_rate = 50 + dt = 1.0 / loop_rate + rate = rospy.Rate(loop_rate) + se.t0 = rospy.get_rostime().to_sec() # set initial time + + # z_EKF = zeros(14) # x, y, psi, v, psi_drift + # P = eye(14) # initial dynamics coveriance matrix + z_EKF = zeros(8) # x, y, psi, v, psi_drift + P = eye(8) # initial dynamics coveriance matrix + + qa = 1000.0 + qp = 1000.0 + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, x, y, psi, v + #Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp,0.01, 0.01,0.01,1.0,1.0,0.1]) + #R = diag([0.5,0.5,0.5,0.1,10.0,1.0,1.0, 5.0,5.0,0.1,0.5, 1.0, 1.0]) + + # experiemnt + # drift_1 + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 0.001, 0.2,0.2,1.0,1.0,0.1]) + # R = 0.1*diag([100.0,100.0,1.0,1.0,1.0,100.0,100.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + # # without yaw_meas + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 1, 0.2,0.2,1.0,1.0,0.1]) + # R = 0.1*diag([5.0,5.0,1.0, 1.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + + # experiemnt: single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa, 1/3*dt**3*qp, dt*qp, 0.001]) + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 1.0, 100.0, 100.0, 10.0]) + + # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = 0.1*diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa, 1/3*dt**3*qp, dt*qp]) + # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 100.0, 100.0, 10.0]) + + Q = diag([0.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0]) + R = diag([10.0, 10.0, 0.1, 0.1, 10.0, 10.0, 0.01]) + + # R = 0.1*diag([100.0, 100.0, 1.0, 1.0, 1.0, 100.0, 100.0, 10.0, 10.0]) + + # experiemnt: single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = diag([1e-2, 1e-2, 1e-2, 1e-2, 10, 10, 1e-3, 10, 1e-3 ])**2 + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = diag([1e-2, 1e-2, 1e-2, 1e-2, 1e-2, 1.0, 1.0])**2 + + # # simulation + # Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp,dt*qp, 0.1, 0.2,0.2,1.0,1.0,0.1]) + # R = diag([5.0,5.0,1.0,10.0,100.0,1000.0,1000.0, 5.0,5.0,10.0,1.0, 10.0,10.0]) + + # simulation single model + # # x, y, vx, vy, ax, ay, psi, psidot, psidrift, + # Q = diag([1/20*dt**5*qa,1/20*dt**5*qa,1/3*dt**3*qa,1/3*dt**3*qa,dt*qa,dt*qa,1/3*dt**3*qp, dt*qp, 0.1]) + # # x_meas, y_meas, vel_meas, yaw_meas, psiDot_meas, a_x_meas, a_y_meas + # R = diag([1.0, 1.0, 1.0, 10.0, 1.0, 10.0, 10.0]) + + + + # R = diag([4*5.0,4*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 4*5.0,4*5.0,10.0,1.0, 10.0,10.0]) + # R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,1000.0,1000.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) + # x,y,v,psi,psiDot,a_x,a_y, x, y, psi, v + # R = diag([1*5.0,1*5.0,1.0,2*10.0,2*100.0,50.0,1.0, 1*5.0,1*5.0,10.0,1.0, 10.0,10.0]) + + # Set up track parameters + # l = Localization() + # l.create_track() + # l.create_feature_track() + # l.create_race_track() + + d_f_hist = [0.0]*10 # assuming that we are running at 50Hz, array of 10 means 0.2s lag + d_a_hist = [0.0]*5 + d_f_lp = 0.0 + a_lp = 0.0 + + t_now = 0.0 + + # Estimation variables + (x_est, y_est, a_x_est, a_y_est, v_est_2) = [0]*5 + bta = 0.0 + v_est = 0.0 + psi_est = 0.0 + # psi_est=3.1415926 + # z_EKF[11]=psi_est + + est_counter = 0 + acc_f = 0.0 + vel_meas_est = 0.0 + + psi_drift_est_his = [0.0] + psi_drift_est_2_his = [0.0] + psi_est_his = [0.0] + psi_est_2_his = [0.0] + vx_est_his = [0.0] + vy_est_his = [0.0] + ax_est_his = [0.0] + ay_est_his = [0.0] + psi_dot_est_his = [0.0] + v2_est_his = [0.0] + vel_meas_his = [0.0] + a_his = [0.0] + df_his = [0.0] + a_lp_his = [0.0] + df_lp_his = [0.0] + estimator_time = [0.0] + x_est_his = [0.0] + y_est_his = [0.0] + x_est_2_his = [0.0] + y_est_2_his = [0.0] + + msg = pos_info() + + while not rospy.is_shutdown(): + t_now = rospy.get_rostime().to_sec()-se.t0 + + se.x_meas = polyval(se.c_X, t_now) + se.y_meas = polyval(se.c_Y, t_now) + se.gps_updated = False + se.imu_updated = False + se.vel_updated = False + + # define input + d_f_hist.append(se.cmd_servo) # this is for a 0.2 seconds delay of steering + # d_a_hist.append(se.cmd_motor) # this is for a 0.2 seconds delay of steering + + # d_f_lp = d_f_lp + 0.5*(se.cmd_servo-d_f_lp) # low pass filter on steering + # a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + # u = [a_lp, d_f_hist.pop(0)] + + + d_f = d_f_hist.pop(0) + # d_f_lp = d_f_lp + 0.3*(d_f-d_f_lp) # low pass filter on steering + # # a_lp = a_lp + 1.0*(se.cmd_motor-a_lp) # low pass filter on acceleration + # u = [se.cmd_motor, d_f_lp] + + u = [se.cmd_motor, d_f] # this is with 0.2s delay for steering without low pass filter + # u = [se.cmd_motor, se.cmd_servo] + # u = [d_a_hist.pop(0), d_f_hist.pop(0)] # this is with 0.2s delay for steering without low pass filter + + # u = [se.cmd_motor, se.cmd_servo] + + # u = [ 0, 0] + + bta = 0.5 * u[1] + + # print "V, V_x and V_y : (%f, %f, %f)" % (se.vel_meas,cos(bta)*se.vel_meas, sin(bta)*se.vel_meas) + + # get measurement + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.yaw_meas, se.vel_meas, cos(bta)*se.vel_meas, sin(bta)*se.vel_meas]) + + # measurement without yaw_meas from imu + y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, + se.x_meas, se.y_meas, se.vel_meas, sin(bta)*se.vel_meas]) + + # y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + # y = array([se.x_meas, se.y_meas, se.vel_meas, se.yaw_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + y = array([se.x_meas, se.y_meas, se.vel_meas, se.psiDot_meas, se.a_x_meas, se.a_y_meas, sin(bta)*se.vel_meas]) + + + + # build extra arguments for non-linear function + args = (u, vhMdl, dt, 0) + + # apply EKF and get each state estimate + (z_EKF, P) = ekf(f_SensorKinematicModel, z_EKF, P, h_SensorKinematicModel, y, Q, R, args) + # Read values + # (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est, psi_drift_est, + # x_est_2, y_est_2, psi_est_2, v_est_2, psi_drift_est_2) = z_EKF # note, r = EKF estimate yaw rate + + (x_est, y_est, v_x_est, v_y_est, a_x_est, a_y_est, psi_est, psi_dot_est) = z_EKF # note, r = EKF estimate yaw rate + psi_drift_est = 0 + # dummy + x_est_2=0 + y_est_2=0 + psi_est_2=0 + v_est_2=0 + psi_drift_est_2=0 + + + # sections to save the data and for debugging + estimator_time.append(t_now) + # psi_drift_est_his.append(psi_drift_est) + psi_drift_est_2_his.append(psi_drift_est_2) + psi_est_his.append(psi_est) + psi_est_2_his.append(psi_est_2) + vx_est_his.append(v_x_est) + vy_est_his.append(v_y_est) + ax_est_his.append(a_x_est) + ay_est_his.append(a_y_est) + psi_dot_est_his.append(psi_dot_est) + v2_est_his.append(v_est_2) + vel_meas_his.append(se.vel_meas) # measurment data from encoder + a_his.append(u[0]) + df_his.append(u[1]) + a_lp_his.append(se.cmd_motor) + df_lp_his.append(d_f) + + x_est_his.append(x_est) + y_est_his.append(y_est) + x_est_2_his.append(x_est_2) + y_est_2_his.append(y_est_2) + + + se.x_est = x_est + se.y_est = y_est + #print "V_x and V_y : (%f, %f)" % (v_x_est, v_y_est) + + # Update track position + # l.set_pos(x_est_2, y_est_2, psi_est_2, v_x_est, v_y_est, psi_dot_est) + # l.set_pos(x_est, y_est, psi_est, v_x_est, v_y_est, psi_dot_est) + # l.set_pos(x_est, y_est, se.yaw_meas, se.vel_meas, v_y_est, psi_dot_est) + + + # Calculate new s, ey, epsi (only 12.5 Hz, enough for controller that runs at 10 Hz) + # if est_counter%4 == 0: + # l.find_s() + #l.s = 0 + #l.epsi = 0 + #l.s_start = 0 + + # and then publish position info + ros_t = rospy.get_rostime() + msg.x = x_est + msg.y = y_est + msg.v_x = v_x_est + msg.v_y = v_y_est + msg.psi = psi_est + msg.psiDot = psi_dot_est + msg.u_a = se.cmd_motor + msg.u_df = se.cmd_servo + + state_pub_pos.publish(msg) + + # wait + est_counter += 1 + rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_true_his = se.yaw_true_his, + psiDot_true_his = se.psiDot_true_his, + x_est_his = x_est_his, + y_est_his = y_est_his, + x_est_2_his = x_est_2_his, + y_est_2_his = y_est_2_his, + psi_drift_est_his = psi_drift_est_his, + psi_drift_est_2_his = psi_drift_est_2_his, + psi_est_his = psi_est_his, + psi_est_2_his = psi_est_2_his, + vx_est_his = vx_est_his, + vy_est_his = vy_est_his, + ax_est_his = ax_est_his, + ay_est_his = ay_est_his, + psi_dot_est_his = psi_dot_est_his, + v2_est_his = v2_est_his, + vel_meas_his = vel_meas_his, + a_his = a_his, + df_his = df_his, + a_lp_his = a_lp_his, + df_lp_his = df_lp_his, + estimator_time = estimator_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psidot_raw_his = se.psidot_raw_his, + a_x_raw_his = se.a_x_raw_his, + a_y_raw_his = se.a_y_raw_his, + a_x_meas_his = se.a_x_meas_his, + a_y_meas_his = se.a_y_meas_his, + roll_raw_his = se.roll_raw_his, + pitch_raw_his = se.pitch_raw_his, + yaw_raw_his = se.yaw_raw_his, + yaw_his = se.yaw_his, + yaw0_his = se.yaw0_his, + yaw_meas_his = se.yaw_meas_his, + imu_time = se.imu_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_hedge_imu_fusion.npz") + np.savez(pathSave,gps_x_his = se.gps_x_his, + gps_y_his = se.gps_y_his, + qx_his = se.qx_his, + qy_his = se.qy_his, + qz_his = se.qz_his, + qw_his = se.qw_his, + ax_his = se.ax_his, + ay_his = se.ay_his, + az_his = se.az_his, + vx_his = se.vx_his, + vy_his = se.vy_his, + vz_his = se.vz_his, + gps_time = se.gps_time, + yaw_gps_meas_his = se.yaw_gps_meas_his, + gps_imu_fusion_time = se.gps_imu_fusion_time) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_hedge_imu_raw.npz") + np.savez(pathSave,acc_x_his = se.acc_x_his, + acc_y_his = se.acc_y_his, + acc_z_his = se.acc_z_his, + gyro_x_his = se.gyro_x_his, + gyro_y_his = se.gyro_y_his, + gyro_z_his = se.gyro_z_his, + compass_x_his = se.compass_x_his, + compass_y_his = se.compass_y_his, + compass_z_his = se.compass_z_his, + gps_imu_raw_time = se.gps_imu_raw_time) + + print "finishing saveing data" + +if __name__ == '__main__': + try: + state_estimation() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimator.py b/workspace/src/barc/src/state_estimator.py new file mode 100755 index 00000000..004f4779 --- /dev/null +++ b/workspace/src/barc/src/state_estimator.py @@ -0,0 +1,737 @@ +#!/usr/bin/env python +""" + File name: stateEstimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +import os +import sys +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/library")) +from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator/delay_a") + df_delay = rospy.get_param("state_estimator/delay_df") + loop_rate = 50.0 + + Q = eye(8) + Q[0,0] = rospy.get_param("state_estimator/Q_x") + Q[1,1] = rospy.get_param("state_estimator/Q_y") + Q[2,2] = rospy.get_param("state_estimator/Q_vx") + Q[3,3] = rospy.get_param("state_estimator/Q_vy") + Q[4,4] = rospy.get_param("state_estimator/Q_ax") + Q[5,5] = rospy.get_param("state_estimator/Q_ay") + Q[6,6] = rospy.get_param("state_estimator/Q_psi") + Q[7,7] = rospy.get_param("state_estimator/Q_psiDot") + R = eye(7) + R[0,0] = rospy.get_param("state_estimator/R_x") + R[1,1] = rospy.get_param("state_estimator/R_y") + R[2,2] = rospy.get_param("state_estimator/R_vx") + R[3,3] = rospy.get_param("state_estimator/R_ax") + R[4,4] = rospy.get_param("state_estimator/R_ay") + R[5,5] = rospy.get_param("state_estimator/R_psiDot") + R[6,6] = rospy.get_param("state_estimator/R_vy") + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + track = Track(rospy.get_param("ds"),rospy.get_param("ey")) + if rospy.get_param("feature_flag"): + track.createFeatureTrack() + else: + track.createRaceTrack() + + estMsg = pos_info() + + while not rospy.is_shutdown(): + # if ecu.a != 0: + est.estimateState(imu,gps,enc,ecu,est.ekf) + + estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + KF_x_his = est.x_his, + KF_y_his = est.y_his, + KF_v_meas_his = est.v_meas_his, + KF_ax_his = est.ax_his, + KF_ay_his = est.ay_his, + KF_psiDot_his = est.psiDot_his, + KF_a_his = est.a_his, + KF_df_his = est.df_his, + estimator_time = est.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + x_ply_his = gps.x_ply_his, + y_ply_his = gps.y_ply_his, + gps_time = gps.time_his, + gps_ply_time = gps.time_ply_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + if rospy.get_param("feature_flag"): self.z[6] = pi/4 + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDot_drift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [0.0] + self.y_his = [0.0] + self.v_meas_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.psiDot_his = [0.0] + self.a_his = [0.0] + self.df_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, 0.5*u[1]*enc.v_meas]) + # y = np.array([gps.x_ply, gps.y_ply, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + KF(y,u) + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(y[0]) + self.y_his.append(y[1]) + self.v_meas_his.append(y[2]) + self.ax_his.append(y[3]) + self.ay_his.append(y[4]) + self.psiDot_his.append(y[5]) + self.a_his.append(u[0]) + self.df_his.append(u[1]) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ekfMultiRate(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + + idx = [] + if self.x_his[-1] == y[0]: + idx.append(0) + if self.y_his[-1] == y[1]: + idx.append(1) + if self.v_meas_his[-1] == y[2]: + idx.append(2) + if self.ax_his[-1] == y[3]: + idx.append(3) + if self.ay_his[-1] == y[4]: + idx.append(4) + if self.psiDot_his[-1] == y[5]: + idx.append(5) + + if len(idx) == 6: + print "No measurement data received!" + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = mx_kp1 + else: + H = np.delete(H,(idx),axis=0) + R = np.delete(self.R,(idx),axis=0) + R = np.delete(R,(idx),axis=1) + y = np.delete(y,(idx),axis=0) + my_kp1 = np.delete(my_kp1,(idx),axis=0) + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + self.prev_time = self.curr_time + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.saveHistory() + + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + self.x_ply = 0.0 + self.y_ply = 0.0 + + # GPS measurement history + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + self.x_ply_his = np.array([0.0]) + self.y_ply_his = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([0.0]) + self.time_ply_his = np.array([0.0]) + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + dist = np.sqrt((data.x_m-self.x_his[-1])**2+(data.y_m-self.y_his[-1])**2) + # if dist < 0.5: + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + n_intplt = 50 # 50*0.01=0.5s data + if size(self.x_ply_his,0) > n_intplt: + x_intplt = self.x_ply_his[-n_intplt:] + y_intplt = self.y_ply_his[-n_intplt:] + t_intplt = self.time_ply_his[-n_intplt:]-self.time_ply_his[-n_intplt] + t_matrix = vstack([t_intplt**2, t_intplt, ones(n_intplt)]).T + c_X = linalg.lstsq(t_matrix, x_intplt)[0] + c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + self.x_ply = polyval(c_X, self.curr_time-self.time_ply_his[-n_intplt]) + self.y_ply = polyval(c_Y, self.curr_time-self.time_ply_his[-n_intplt]) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + # if self.x_ply_his[-1] != self.x_ply: + self.x_ply_his = np.append(self.x_ply_his,self.x_ply) + self.y_ply_his = np.append(self.y_ply_his,self.y_ply) + self.time_ply_his = np.append(self.time_ply_his,self.curr_time) + + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + v_est = (self.v_rl + self.v_rr)/2 + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimator_lukas.py b/workspace/src/barc/src/state_estimator_lukas.py new file mode 100755 index 00000000..74500c5f --- /dev/null +++ b/workspace/src/barc/src/state_estimator_lukas.py @@ -0,0 +1,739 @@ +#!/usr/bin/env python +""" + File name: stateEstimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +import os +import sys +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/library")) +from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator/delay_a") + df_delay = rospy.get_param("state_estimator/delay_df") + loop_rate = 50.0 + + Q = eye(8) + Q[0,0] = rospy.get_param("state_estimator/Q_x") + Q[1,1] = rospy.get_param("state_estimator/Q_y") + Q[2,2] = rospy.get_param("state_estimator/Q_vx") + Q[3,3] = rospy.get_param("state_estimator/Q_vy") + Q[4,4] = rospy.get_param("state_estimator/Q_ax") + Q[5,5] = rospy.get_param("state_estimator/Q_ay") + Q[6,6] = rospy.get_param("state_estimator/Q_psi") + Q[7,7] = rospy.get_param("state_estimator/Q_psiDot") + R = eye(7) + R[0,0] = rospy.get_param("state_estimator/R_x") + R[1,1] = rospy.get_param("state_estimator/R_y") + R[2,2] = rospy.get_param("state_estimator/R_vx") + R[3,3] = rospy.get_param("state_estimator/R_ax") + R[4,4] = rospy.get_param("state_estimator/R_ay") + R[5,5] = rospy.get_param("state_estimator/R_psiDot") + R[6,6] = 0.1 + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + """ + track = Track(rospy.get_param("ds"),rospy.get_param("ey")) + if rospy.get_param("feature_flag"): + track.createFeatureTrack() + else: + track.createRaceTrack() + """ + + estMsg = pos_info() + + while not rospy.is_shutdown(): + # if ecu.a != 0: + est.estimateState(imu,gps,enc,ecu,est.ekf) + + # estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + KF_x_his = est.x_his, + KF_y_his = est.y_his, + KF_v_meas_his = est.v_meas_his, + KF_ax_his = est.ax_his, + KF_ay_his = est.ay_his, + KF_psiDot_his = est.psiDot_his, + KF_a_his = est.a_his, + KF_df_his = est.df_his, + estimator_time = est.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + x_ply_his = gps.x_ply_his, + y_ply_his = gps.y_ply_his, + gps_time = gps.time_his, + gps_ply_time = gps.time_ply_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + if rospy.get_param("feature_flag"): self.z[6] = pi/4 + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDot_drift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [0.0] + self.y_his = [0.0] + self.v_meas_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.psiDot_his = [0.0] + self.a_his = [0.0] + self.df_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, 0.5*u[1]*enc.v_meas]) + # y = np.array([gps.x_ply, gps.y_ply, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + KF(y,u) + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(y[0]) + self.y_his.append(y[1]) + self.v_meas_his.append(y[2]) + self.ax_his.append(y[3]) + self.ay_his.append(y[4]) + self.psiDot_his.append(y[5]) + self.a_his.append(u[0]) + self.df_his.append(u[1]) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ekfMultiRate(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + + idx = [] + if self.x_his[-1] == y[0]: + idx.append(0) + if self.y_his[-1] == y[1]: + idx.append(1) + if self.v_meas_his[-1] == y[2]: + idx.append(2) + if self.ax_his[-1] == y[3]: + idx.append(3) + if self.ay_his[-1] == y[4]: + idx.append(4) + if self.psiDot_his[-1] == y[5]: + idx.append(5) + + if len(idx) == 6: + print "No measurement data received!" + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = mx_kp1 + else: + H = np.delete(H,(idx),axis=0) + R = np.delete(self.R,(idx),axis=0) + R = np.delete(R,(idx),axis=1) + y = np.delete(y,(idx),axis=0) + my_kp1 = np.delete(my_kp1,(idx),axis=0) + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*7 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + self.prev_time = self.curr_time + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.saveHistory() + + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + + # GPS measurement + self.x = 0.0 + self.y = 0.0 + self.x_ply = 0.0 + self.y_ply = 0.0 + + # GPS measurement history + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + self.x_ply_his = np.array([0.0]) + self.y_ply_his = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([0.0]) + self.time_ply_his = np.array([0.0]) + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + dist = np.sqrt((data.x_m-self.x_his[-1])**2+(data.y_m-self.y_his[-1])**2) + # if dist < 0.5: + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + n_intplt = 50 # 50*0.01=0.5s data + if size(self.x_ply_his,0) > n_intplt: + x_intplt = self.x_ply_his[-n_intplt:] + y_intplt = self.y_ply_his[-n_intplt:] + t_intplt = self.time_ply_his[-n_intplt:]-self.time_ply_his[-n_intplt] + t_matrix = vstack([t_intplt**2, t_intplt, ones(n_intplt)]).T + c_X = linalg.lstsq(t_matrix, x_intplt)[0] + c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + self.x_ply = polyval(c_X, self.curr_time-self.time_ply_his[-n_intplt]) + self.y_ply = polyval(c_Y, self.curr_time-self.time_ply_his[-n_intplt]) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + # if self.x_ply_his[-1] != self.x_ply: + self.x_ply_his = np.append(self.x_ply_his,self.x_ply) + self.y_ply_his = np.append(self.y_ply_his,self.y_ply) + self.time_ply_his = np.append(self.time_ply_his,self.curr_time) + + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + v_est = (self.v_rl + self.v_rr)/2 + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/state_estimator_lukas_yaw.py b/workspace/src/barc/src/state_estimator_lukas_yaw.py new file mode 100755 index 00000000..1ee6b915 --- /dev/null +++ b/workspace/src/barc/src/state_estimator_lukas_yaw.py @@ -0,0 +1,780 @@ +#!/usr/bin/env python +""" + File name: stateEstimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +import os +import sys +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/library")) +# from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator_2/delay_a") + df_delay = rospy.get_param("state_estimator_2/delay_df") + loop_rate = 50.0 + + Q = eye(8) + Q[0,0] = rospy.get_param("state_estimator_2/Q_x") + Q[1,1] = rospy.get_param("state_estimator_2/Q_y") + Q[2,2] = rospy.get_param("state_estimator_2/Q_vx") + Q[3,3] = rospy.get_param("state_estimator_2/Q_vy") + Q[4,4] = rospy.get_param("state_estimator_2/Q_ax") + Q[5,5] = rospy.get_param("state_estimator_2/Q_ay") + Q[6,6] = rospy.get_param("state_estimator_2/Q_psi") + Q[7,7] = rospy.get_param("state_estimator_2/Q_psiDot") + R = eye(8) + R[0,0] = rospy.get_param("state_estimator_2/R_x") + R[1,1] = rospy.get_param("state_estimator_2/R_y") + R[2,2] = rospy.get_param("state_estimator_2/R_vx") + R[3,3] = rospy.get_param("state_estimator_2/R_ax") + R[4,4] = rospy.get_param("state_estimator_2/R_ay") + R[5,5] = rospy.get_param("state_estimator_2/R_psiDot") + R[6,6] = rospy.get_param("state_estimator_2/R_vy") + R[7,7] = 20.0 + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + """ + track = Track(rospy.get_param("ds"),rospy.get_param("ey")) + if rospy.get_param("feature_flag"): + track.createFeatureTrack() + else: + track.createRaceTrack() + """ + + estMsg = pos_info() + + while not rospy.is_shutdown(): + # if ecu.a != 0: + est.estimateState(imu,gps,enc,ecu,est.ekf) + + # estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + KF_x_his = est.x_his, + KF_y_his = est.y_his, + KF_v_meas_his = est.v_meas_his, + KF_ax_his = est.ax_his, + KF_ay_his = est.ay_his, + KF_psiDot_his = est.psiDot_his, + KF_a_his = est.a_his, + KF_df_his = est.df_his, + estimator_time = est.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + x_ply_his = gps.x_ply_his, + y_ply_his = gps.y_ply_his, + gps_time = gps.time_his, + gps_ply_time = gps.time_ply_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDot_drift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [0.0] + self.y_his = [0.0] + self.v_meas_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.psiDot_his = [0.0] + self.a_his = [0.0] + self.df_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + + # y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, 0.5 * u[1] * enc.v_meas, gps.angle]) + # y = np.array([gps.x_ply, gps.y_ply, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + KF(y,u) + + # if abs(y[5])<0.3: + # self.z[3] = 0.0 + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(y[0]) + self.y_his.append(y[1]) + self.v_meas_his.append(y[2]) + self.ax_his.append(y[3]) + self.ay_his.append(y[4]) + self.psiDot_his.append(y[5]) + self.a_his.append(u[0]) + self.df_his.append(u[1]) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ekfMultiRate(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + + idx = [] + if self.x_his[-1] == y[0]: + idx.append(0) + if self.y_his[-1] == y[1]: + idx.append(1) + if self.v_meas_his[-1] == y[2]: + idx.append(2) + if self.ax_his[-1] == y[3]: + idx.append(3) + if self.ay_his[-1] == y[4]: + idx.append(4) + if self.psiDot_his[-1] == y[5]: + idx.append(5) + + if len(idx) == 6: + print "No measurement data received!" + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = mx_kp1 + else: + H = np.delete(H,(idx),axis=0) + R = np.delete(self.R,(idx),axis=0) + R = np.delete(R,(idx),axis=1) + y = np.delete(y,(idx),axis=0) + my_kp1 = np.delete(my_kp1,(idx),axis=0) + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*8 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + y[7] = x[6] # psi + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + self.prev_time = self.curr_time + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.saveHistory() + + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + # rospy.Subscriber('hedge_imu_fusion', hedge_imu_fusion, self.gps_callback, queue_size=1) + rospy.Subscriber('hedge_pos', hedge_pos, self.gps_callback, queue_size=1) + + # GPS measurement + self.angle = 0.0 + self.x = 0.0 + self.y = 0.0 + self.x_ply = 0.0 + self.y_ply = 0.0 + + # GPS measurement history + self.angle_his = np.array([0.0]) + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + self.x_ply_his = np.array([0.0]) + self.y_ply_his = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([0.0]) + self.time_ply_his = np.array([0.0]) + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + dist = np.sqrt((data.x_m-self.x_his[-1])**2+(data.y_m-self.y_his[-1])**2) + # if dist < 0.5: + if self.x_his[-1] != data.x_m: + self.x = data.x_m + self.y = data.y_m + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + n_intplt = 50 # 50*0.01=0.5s data + if size(self.x_ply_his,0) > n_intplt: + x_intplt = self.x_ply_his[-n_intplt:] + y_intplt = self.y_ply_his[-n_intplt:] + t_intplt = self.time_ply_his[-n_intplt:]-self.time_ply_his[-n_intplt] + t_matrix = vstack([t_intplt**2, t_intplt, ones(n_intplt)]).T + c_X = linalg.lstsq(t_matrix, x_intplt)[0] + c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + self.x_ply = polyval(c_X, self.curr_time-self.time_ply_his[-n_intplt]) + self.y_ply = polyval(c_Y, self.curr_time-self.time_ply_his[-n_intplt]) + + # Estimate yaw angle from previous 2 time steps + x_0 = self.x_his[-1] + y_0 = self.y_his[-1] + x_1 = self.x + y_1 = self.y + argument_y = y_1 - y_0 + argument_x = x_1 - x_0 + angle = 0.0 + if argument_x > 0.0: + angle = np.arctan(argument_y / argument_x) + if argument_y >= 0.0 and argument_x < 0.0: + angle = np.pi + np.arctan(argument_y / argument_x) + if argument_y < 0.0 and argument_x < 0.0: + angle = - np.pi + np.arctan(argument_y / argument_x) + if argument_y > 0.0 and argument_x == 0.0: + angle = np.pi / 2.0 + if argument_y < 0.0 and argument_x == 0.0: + angle = - np.pi / 2.0 + if angle < 0.0: + angle += 2.0 * np.pi + self.angle = np.unwrap([self.angle_his[-1],angle])[1] + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + self.angle_his= np.append(self.angle_his,self.angle) + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + # if self.x_ply_his[-1] != self.x_ply: + self.x_ply_his = np.append(self.x_ply_his,self.x_ply) + self.y_ply_his = np.append(self.y_ply_his,self.y_ply) + self.time_ply_his = np.append(self.time_ply_his,self.curr_time) + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.use_both_encoders = False + else: + self.use_both_encoders = True + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + + if self.use_both_encoders: + v_est = (self.v_rl + self.v_rr)/2 + else: + v_est = self.v_rr + + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass \ No newline at end of file diff --git a/workspace/src/barc/src/state_estimator_multiple_beacons.py b/workspace/src/barc/src/state_estimator_multiple_beacons.py new file mode 100755 index 00000000..70a3ae7d --- /dev/null +++ b/workspace/src/barc/src/state_estimator_multiple_beacons.py @@ -0,0 +1,780 @@ +#!/usr/bin/env python +""" + File name: stateEstimator.py + Author: Shuqi Xu + Email: shuqixu@kth.se + Python Version: 2.7.12 +""" +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +import rospy +import os +import sys +homedir = os.path.expanduser("~") +sys.path.append(os.path.join(homedir,"barc/workspace/src/barc/src/library")) +# from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion, hedge_pos_ang +from std_msgs.msg import Header +from numpy import eye, zeros, diag, tan, cos, sin, vstack, linalg, pi +from numpy import ones, polyval, size, dot, add +from scipy.linalg import inv, cholesky +from tf import transformations +import math +import numpy as np + +def main(): + # node initialization + rospy.init_node("state_estimation") + a_delay = rospy.get_param("state_estimator/delay_a") + df_delay = rospy.get_param("state_estimator/delay_df") + loop_rate = 50.0 + + Q = eye(8) + Q[0,0] = rospy.get_param("state_estimator/Q_x") + Q[1,1] = rospy.get_param("state_estimator/Q_y") + Q[2,2] = rospy.get_param("state_estimator/Q_vx") + Q[3,3] = rospy.get_param("state_estimator/Q_vy") + Q[4,4] = rospy.get_param("state_estimator/Q_ax") + Q[5,5] = rospy.get_param("state_estimator/Q_ay") + Q[6,6] = rospy.get_param("state_estimator/Q_psi") + Q[7,7] = rospy.get_param("state_estimator/Q_psiDot") + R = eye(8) + R[0,0] = rospy.get_param("state_estimator/R_x") + R[1,1] = rospy.get_param("state_estimator/R_y") + R[2,2] = rospy.get_param("state_estimator/R_vx") + R[3,3] = rospy.get_param("state_estimator/R_ax") + R[4,4] = rospy.get_param("state_estimator/R_ay") + R[5,5] = rospy.get_param("state_estimator/R_psiDot") + R[6,6] = rospy.get_param("state_estimator/R_vy") + R[7,7] = 1.0 + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + gps = GpsClass(t0) + enc = EncClass(t0) + ecu = EcuClass(t0) + est = Estimator(t0,loop_rate,a_delay,df_delay,Q,R) + + """ + track = Track(rospy.get_param("ds"),rospy.get_param("ey")) + if rospy.get_param("feature_flag"): + track.createFeatureTrack() + else: + track.createRaceTrack() + """ + + estMsg = pos_info() + + while not rospy.is_shutdown(): + + est.estimateState(imu,gps,enc,ecu,est.ukf) + + # estMsg.s, estMsg.ey, estMsg.epsi = track.Localize(est.x_est, est.y_est, est.yaw_est) + + estMsg.header.stamp = rospy.get_rostime() + estMsg.v = np.sqrt(est.vx_est**2 + est.vy_est**2) + estMsg.x = est.x_est + estMsg.y = est.y_est + estMsg.v_x = est.vx_est + estMsg.v_y = est.vy_est + estMsg.psi = est.yaw_est + estMsg.psiDot = est.psiDot_est + estMsg.a_x = est.ax_est + estMsg.a_y = est.ay_est + estMsg.u_a = ecu.a + estMsg.u_df = ecu.df + + est.state_pub_pos.publish(estMsg) + est.saveHistory() + est.rate.sleep() + + homedir = os.path.expanduser("~") + pathSave = os.path.join(homedir,"barc_debugging/estimator_output.npz") + np.savez(pathSave,yaw_est_his = est.yaw_est_his, + psiDot_est_his = est.psiDot_est_his, + x_est_his = est.x_est_his, + y_est_his = est.y_est_his, + vx_est_his = est.vx_est_his, + vy_est_his = est.vy_est_his, + ax_est_his = est.ax_est_his, + ay_est_his = est.ay_est_his, + KF_x_his = est.x_his, + KF_y_his = est.y_his, + KF_v_meas_his = est.v_meas_his, + KF_ax_his = est.ax_his, + KF_ay_his = est.ay_his, + KF_psiDot_his = est.psiDot_his, + KF_a_his = est.a_his, + KF_df_his = est.df_his, + estimator_time = est.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_imu.npz") + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_gps.npz") + np.savez(pathSave,x_his = gps.x_his, + y_his = gps.y_his, + x_ply_his = gps.x_ply_his, + y_ply_his = gps.y_ply_his, + angle_his = gps.angle_his, + gps_time = gps.time_his, + gps_ply_time = gps.time_ply_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_enc.npz") + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + pathSave = os.path.join(homedir,"barc_debugging/estimator_ecu.npz") + np.savez(pathSave,a_his = ecu.a_his, + df_his = ecu.df_his, + ecu_time = ecu.time_his) + + print "Finishing saveing state estimation data" + + +class Estimator(object): + """ Object collecting estimated state data + Attributes: + Estimated states: + 1.x_est 2.y_est + 3.vx_est 4.vy_est 5.v_est + 6.ax_est 7.ay_est + 8.yaw_est 9.psiDot_est 10.psiDrift_est + Estimated states history: + 1.x_est_his 2.y_est_his + 3.vx_est_his 4.vy_est_his 5.v_est_his + 6.ax_est_his 7.ay_est_his + 8.yaw_est_his 9.psiDot_est_his 10.psiDrift_est_his + Time stamp + 1.t0 2.time_his 3.curr_time + Methods: + stateEstimate(imu,gps,enc,ecu): + Estimate current state from sensor data + ekf(y,u): + Extended Kalman filter + ukf(y,u): + Unscented Kalman filter + numerical_jac(func,x,u): + Calculate jacobian numerically + f(x,u): + System prediction model + h(x,u): + System measurement model + """ + + def __init__(self,t0,loop_rate,a_delay,df_delay,Q,R): + """ Initialization + Arguments: + t0: starting measurement time + """ + dt = 1.0 / loop_rate + self.rate = rospy.Rate(loop_rate) + L_f = rospy.get_param("L_a") # distance from CoG to front axel + L_r = rospy.get_param("L_b") # distance from CoG to rear axel + self.vhMdl = (L_f, L_r) + self.Q = Q + self.R = R + self.P = np.eye(np.size(Q,0)) # initializationtial covariance matrix + self.z = np.zeros(np.size(Q,0)) # initial state mean + self.dt = dt + self.a_delay = a_delay + self.df_delay = df_delay + self.a_his = [0.0]*int(a_delay/dt) + self.df_his = [0.0]*int(df_delay/dt) + + self.state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + self.t0 = t0 + + self.x_est = 0.0 + self.y_est = 0.0 + self.vx_est = 0.0 + self.vy_est = 0.0 + self.v_est = 0.0 + self.ax_est = 0.0 + self.ay_est = 0.0 + self.yaw_est = 0.0 + self.psiDot_est = 0.0 + self.psiDot_drift_est = 0.0 + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x_est_his = [0.0] + self.y_est_his = [0.0] + self.vx_est_his = [0.0] + self.vy_est_his = [0.0] + self.v_est_his = [0.0] + self.ax_est_his = [0.0] + self.ay_est_his = [0.0] + self.yaw_est_his = [0.0] + self.psiDot_est_his = [0.0] + self.time_his = [0.0] + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his = [0.0] + self.y_his = [0.0] + self.v_meas_his = [0.0] + self.ax_his = [0.0] + self.ay_his = [0.0] + self.psiDot_his = [0.0] + self.a_his = [0.0] + self.df_his = [0.0] + + # ecu command update + def estimateState(self,imu,gps,enc,ecu,KF): + """Do extended Kalman filter to estimate states""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a_his.append(ecu.a) + self.df_his.append(ecu.df) + u = [self.a_his.pop(0), self.df_his.pop(0)] + + # y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + y = np.array([gps.x, gps.y, enc.v_meas, imu.ax, imu.ay, imu.psiDot, 0.5*u[1]*enc.v_meas, gps.angle]) + # y = np.array([gps.x_ply, gps.y_ply, enc.v_meas, imu.ax, imu.ay, imu.psiDot]) + KF(y,u) + + # SAVE THE measurement/input SEQUENCE USED BY KF + self.x_his.append(y[0]) + self.y_his.append(y[1]) + self.v_meas_his.append(y[2]) + self.ax_his.append(y[3]) + self.ay_his.append(y[4]) + self.psiDot_his.append(y[5]) + self.a_his.append(u[0]) + self.df_his.append(u[1]) + + def ekf(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + self.R)) # Kalman filter gain + + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,self.R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ekfMultiRate(self, y, u): + """ + EKF Extended Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x_k+1 = f(x_k) + w_k + y_k = h(x_k) + v_k + where w ~ N(0,Q) meaning w is gaussian noise with covariance Q + v ~ N(0,R) meaning v is gaussian noise with covariance R + Inputs: f: function handle for f(x) + z_EKF: "a priori" state estimate + P: "a priori" estimated state covariance + h: fanction handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + args: additional arguments to f(x, *args) + Output: mx_kp1: "a posteriori" state estimate + P_kp1: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + xDim = self.z.size # dimension of the state + mx_kp1 = self.f(self.z, u) # predict next state + A = self.numerical_jac(self.f, self.z, u) # linearize process model about current state + P_kp1 = dot(dot(A,self.P),A.T) + self.Q # proprogate variance + my_kp1 = self.h(mx_kp1, u) # predict future output + H = self.numerical_jac(self.h, mx_kp1, u) # linearize measurement model about predicted next state + + idx = [] + if self.x_his[-1] == y[0]: + idx.append(0) + if self.y_his[-1] == y[1]: + idx.append(1) + if self.v_meas_his[-1] == y[2]: + idx.append(2) + if self.ax_his[-1] == y[3]: + idx.append(3) + if self.ay_his[-1] == y[4]: + idx.append(4) + if self.psiDot_his[-1] == y[5]: + idx.append(5) + + if len(idx) == 6: + print "No measurement data received!" + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = mx_kp1 + else: + H = np.delete(H,(idx),axis=0) + R = np.delete(self.R,(idx),axis=0) + R = np.delete(R,(idx),axis=1) + y = np.delete(y,(idx),axis=0) + my_kp1 = np.delete(my_kp1,(idx),axis=0) + P12 = dot(P_kp1, H.T) # cross covariance + K = dot(P12, inv( dot(H,P12) + R)) # Kalman filter gain + self.z = mx_kp1 + dot(K,(y - my_kp1)) + self.P = dot(dot(K,R),K.T) + dot( dot( (eye(xDim) - dot(K,H)) , P_kp1) , (eye(xDim) - dot(K,H)).T ) + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def ukf(self, y, u): + """ + UKF Unscented Kalman Filter for nonlinear dynamic systems + ekf(f,mx,P,h,z,Q,R) returns state estimate, x and state covariance, P + for nonlinear dynamic system: + x[k] = f(x[k-1],u[k-1]) + v[k-1] + y[k] = h(x[k]) + w[k] + where v ~ N(0,Q) meaning v is gaussian noise with covariance Q + w ~ N(0,R) meaning w is gaussian noise with covariance R + Inputs: f: function handle for f(x) + h: function handle for h(x) + y: current measurement + Q: process noise covariance + R: measurement noise covariance + Output: mx_k: "a posteriori" state estimate + P_k: "a posteriori" state covariance + + Notation: mx_k = E[x_k] and my_k = E[y_k], where m stands for "mean of" + """ + + # sigma-points: generate a list, "sm_km1" + xDim = self.z.size + sqrtnP = cholesky(xDim*self.P) + sm_km1 = list(add(self.z,sqrtnP)) + sm_km1.extend(list(add(self.z,-sqrtnP))) + + # prior update + sx_k = [self.f(s, u) for s in sm_km1] + mx_k = 1.0/len(sx_k)*sum(sx_k) + P_m = self.Q + 1.0/len(sx_k)*sum([np.outer((sx-mx_k),(sx-mx_k)) for sx in sx_k]) + + # posterior update + sy_k = [self.h(s, u) for s in sx_k] + my_k = 1.0/len(sy_k)*sum(sy_k) + P_zz = self.R + 1.0/len(sy_k)*sum([np.outer((sy-my_k),(sy-my_k)) for sy in sy_k]) + + # cross covariance + P_xz = 1.0/len(sy_k)*sum([np.outer((sx_k[i]-mx_k),(sy_k[i]-my_k)) for i in range(len(sy_k))]) + + # Kalman filter + K = dot(P_xz,inv(P_zz)) + self.z = mx_k + dot(K, y-my_k) + self.P = P_m - dot(K, dot(P_zz, K.T)) + + (self.x_est, self.y_est, self.vx_est, self.vy_est, self.ax_est, self.ay_est, self.yaw_est, self.psiDot_est) = self.z + + def numerical_jac(self,func,x,u): + """ + Function to compute the numerical jacobian of a vector valued function + using final differences + """ + # numerical gradient and diagonal hessian + y = func(x,u) + + jac = zeros( (y.size,x.size) ) + eps = 1e-5 + xp = np.copy(x) + + for i in range(x.size): + xp[i] = x[i] + eps/2.0 + yhi = func(xp, u) + xp[i] = x[i] - eps/2.0 + ylo = func(xp, u) + xp[i] = x[i] + jac[:,i] = (yhi - ylo) / eps + return jac + + def f(self, z, u): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + dt = self.dt + zNext = [0]*8 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + return np.array(zNext) + + def h(self, x, u): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*8 + y[0] = x[0] # x + y[1] = x[1] # y + y[2] = x[2] # vx + y[3] = x[4] # a_x + y[4] = x[5] # a_y + y[5] = x[7] # psiDot + y[6] = x[3] # vy + y[7] = x[6] + return np.array(y) + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.x_est_his.append(self.x_est) + self.y_est_his.append(self.y_est) + self.vx_est_his.append(self.vx_est) + self.vy_est_his.append(self.vy_est) + self.v_est_his.append(self.v_est) + self.ax_est_his.append(self.ax_est) + self.ay_est_his.append(self.ay_est) + self.yaw_est_his.append(self.yaw_est) + self.psiDot_est_his.append(self.psiDot_est) + +class ImuClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.yaw 2.psiDot 3.ax 4.ay 5.roll 6.pitch + Measurement history: + 1.yaw_his 2.psiDot_his 3.ax_his 4.ay_his 5.roll_his 6.pitch_his + Time stamp + 1.t0 2.time_his + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('imu/data', Imu, self.imu_callback, queue_size=1) + + # Imu measurement + self.yaw = 0.0 + self.psiDot = 0.0 + self.ax = 0.0 + self.ay = 0.0 + self.roll = 0.0 + self.pitch = 0.0 + + # Imu measurement history + self.yaw_his = [] + self.psiDot_his = [] + self.ax_his = [] + self.ay_his = [] + self.roll_his = [] + self.pitch_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + + # Time for yawDot integration + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + self.prev_time = self.curr_time + + def imu_callback(self,data): + """Unpack message from sensor, IMU""" + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + if self.prev_time > 0: + self.yaw += self.psiDot * (self.curr_time-self.prev_time) + self.prev_time = self.curr_time + + ori = data.orientation + quaternion = (ori.x, ori.y, ori.z, ori.w) + (roll_raw, pitch_raw, dummy) = transformations.euler_from_quaternion(quaternion) + self.roll = roll_raw + self.pitch = pitch_raw + + w_z = data.angular_velocity.z + a_x = data.linear_acceleration.x + a_y = data.linear_acceleration.y + a_z = data.linear_acceleration.z + + self.psiDot = w_z + # Transformation from imu frame to vehicle frame (negative roll/pitch and reversed matrix multiplication to go back) + self.ax = cos(-pitch_raw)*a_x + sin(-pitch_raw)*sin(-roll_raw)*a_y - sin(-pitch_raw)*cos(-roll_raw)*a_z + self.ay = cos(-roll_raw)*a_y + sin(-roll_raw)*a_z + + self.saveHistory() + + + def saveHistory(self): + """ Save measurement data into history array""" + + self.time_his.append(self.curr_time) + + self.yaw_his.append(self.yaw) + self.psiDot_his.append(self.psiDot) + self.ax_his.append(self.ax) + self.ay_his.append(self.ay) + self.roll_his.append(self.roll) + self.pitch_his.append(self.pitch) + + + +class GpsClass(object): + """ Object collecting GPS measurement data + Attributes: + Measurement: + 1.x 2.y + Measurement history: + 1.x_his 2.y_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + + rospy.Subscriber('hedge_pos_ang', hedge_pos_ang, self.gps_callback, queue_size=1) + + # GPS measurement + self.x_1 = 0.0 + self.y_1 = 0.0 + self.x_2 = 0.0 + self.y_2 = 0.0 + + self.x = 0.0 + self.y = 0.0 + self.angle = 0.0 + self.x_ply = 0.0 + self.y_ply = 0.0 + + self.time_his = np.array([0.0]) + self.x_his = np.array([0.0]) + self.y_his = np.array([0.0]) + self.angle_his = np.array([0.0]) + self.x_ply_his = np.array([0.0]) + self.y_ply_his = np.array([0.0]) + + # time stamp + self.t0 = t0 + self.time_his = np.array([0.0]) + self.time_ply_his = np.array([0.0]) + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def gps_callback(self,data): + """Unpack message from sensor, GPS""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + dist = np.sqrt((data.x_m-self.x_his[-1])**2+(data.y_m-self.y_his[-1])**2) + if dist < 0.5: + + if data.address == 16: + self.curr_time_1 = rospy.get_rostime().to_sec() - self.t0 + + self.x_1 = data.x_m + self.y_1 = data.y_m + + self.angle = data.angle + + if data.address == 26: + self.curr_time_2 = rospy.get_rostime().to_sec() - self.t0 + + self.x_2 = data.x_m + self.y_2 = data.y_m + + self.angle = data.angle + + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.x = 0.5 * (self.x_1 + self.x_2) + self.y = 0.5 * (self.y_1 + self.y_2) + + # 1) x(t) ~ c0x + c1x * t + c2x * t^2 + # 2) y(t) ~ c0y + c1y * t + c2y * t^2 + # c_X = [c0x c1x c2x] and c_Y = [c0y c1y c2y] + n_intplt = 50 # 50*0.01=0.5s data + if size(self.x_ply_his,0) > n_intplt: + x_intplt = self.x_ply_his[-n_intplt:] + y_intplt = self.y_ply_his[-n_intplt:] + t_intplt = self.time_ply_his[-n_intplt:]-self.time_ply_his[-n_intplt] + t_matrix = vstack([t_intplt**2, t_intplt, ones(n_intplt)]).T + c_X = linalg.lstsq(t_matrix, x_intplt)[0] + c_Y = linalg.lstsq(t_matrix, y_intplt)[0] + self.x_ply = polyval(c_X, self.curr_time-self.time_ply_his[-n_intplt]) + self.y_ply = polyval(c_Y, self.curr_time-self.time_ply_his[-n_intplt]) + + self.saveHistory() + + def saveHistory(self): + self.time_his = np.append(self.time_his,self.curr_time) + self.x_his = np.append(self.x_his,self.x) + self.y_his = np.append(self.y_his,self.y) + self.angle_his = np.append(self.angle_his, self.angle) + # if self.x_ply_his[-1] != self.x_ply: + self.x_ply_his = np.append(self.x_ply_his,self.x_ply) + self.y_ply_his = np.append(self.y_ply_his,self.y_ply) + self.time_ply_his = np.append(self.time_ply_his,self.curr_time) + + + +class EncClass(object): + """ Object collecting ENC measurement data + Attributes: + Measurement: + 1.v_fl 2.v_fr 3. v_rl 4. v_rr + Measurement history: + 1.v_fl_his 2.v_fr_his 3. v_rl_his 4. v_rr_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('vel_est', Vel_est, self.enc_callback, queue_size=1) + + node_name = rospy.get_name() + if node_name[-1] == "2": + self.use_both_encoders = False + else: + self.use_both_encoders = True + + # ENC measurement + self.v_fl = 0.0 + self.v_fr = 0.0 + self.v_rl = 0.0 + self.v_rr = 0.0 + self.v_meas = 0.0 + + # ENC measurement history + self.v_fl_his = [] + self.v_fr_his = [] + self.v_rl_his = [] + self.v_rr_his = [] + self.v_meas_his = [] + + # time stamp + self.v_count = 0 + self.v_prev = 0.0 + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def enc_callback(self,data): + """Unpack message from sensor, ENC""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.v_fl = data.vel_fl + self.v_fr = data.vel_fr + self.v_rl = data.vel_bl + self.v_rr = data.vel_br + + if self.use_both_encoders: + v_est = (self.v_rl + self.v_rr)/2 + else: + v_est = self.v_rr + + if v_est != self.v_prev: + self.v_meas = v_est + self.v_prev = v_est + self.v_count = 0 + else: + self.v_count += 1 + if self.v_count > 10: # if 10 times in a row the same measurement + self.v_meas = 0 # set velocity measurement to zero + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.v_fl_his.append(self.v_fl) + self.v_fr_his.append(self.v_fr) + self.v_rl_his.append(self.v_rl) + self.v_rr_his.append(self.v_rr) + + self.v_meas_his.append(self.v_meas) + +class EcuClass(object): + """ Object collecting CMD command data + Attributes: + Input command: + 1.a 2.df + Input command history: + 1.a_his 2.df_his + Time stamp + 1.t0 2.time_his 3.curr_time + """ + def __init__(self,t0): + """ Initialization + Arguments: + t0: starting measurement time + """ + rospy.Subscriber('ecu', ECU, self.ecu_callback, queue_size=1) + + # ECU measurement + self.a = 0.0 + self.df = 0.0 + + # ECU measurement history + self.a_his = [] + self.df_his = [] + + # time stamp + self.t0 = t0 + self.time_his = [] + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + def ecu_callback(self,data): + """Unpack message from sensor, ECU""" + self.curr_time = rospy.get_rostime().to_sec() - self.t0 + + self.a = data.motor + self.df = data.servo + + self.saveHistory() + + def saveHistory(self): + self.time_his.append(self.curr_time) + + self.a_his.append(self.a) + self.df_his.append(self.df) + + +if __name__ == '__main__': + try: + main() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc/src/steering_mapping.py b/workspace/src/barc/src/steering_mapping.py new file mode 100755 index 00000000..59962cf8 --- /dev/null +++ b/workspace/src/barc/src/steering_mapping.py @@ -0,0 +1,424 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed at UC +# Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu) and Greg Marcil (grmarcil@berkeley.edu). The cloud +# services integation with ROS was developed by Kiet Lam +# (kiet.lam@berkeley.edu). The web-server app Dator was based on an open source +# project by Bruce Wootton +# --------------------------------------------------------------------------- + +# README: This node serves as an outgoing messaging bus from odroid to arduino +# Subscribes: steering and motor commands on 'ecu' +# Publishes: combined ecu commands as 'ecu_pwm' + +from rospy import init_node, Subscriber, Publisher, get_param, get_rostime +from rospy import Rate, is_shutdown, ROSInterruptException, spin, on_shutdown +from barc.msg import ECU, pos_info +from state_estimation_SensorKinematicModel_lukas import EncClass, ImuClass +import rospy +import numpy as np +import os + +class low_level_control(object): + motor_pwm = 89 + servo_pwm = 90 + str_ang_max = 35 + str_ang_min = -35 + ecu_pub = 0 + ecu_cmd = ECU() + def pwm_converter_callback(self, msg): + # translate from SI units in vehicle model + # to pwm angle units (i.e. to send command signal to actuators) + # convert desired steering angle to degrees, saturate based on input limits + + # Old servo control: + # self.servo_pwm = 91.365 + 105.6*float(msg.servo) + # New servo Control + # if msg.servo < 0.0: # right curve + # self.servo_pwm = 95.5 + 118.8*float(msg.servo) + # elif msg.servo > 0.0: # left curve + # self.servo_pwm = 90.8 + 78.9*float(msg.servo) + + # self.servo_pwm = 90.0 + 89.0*float(msg.servo) + self.servo_pwm = 83.3 + 103.1 * float(msg.servo) + + + # compute motor command + FxR = float(msg.motor) + if FxR == 0: + self.motor_pwm = 90.0 + elif FxR > 0: + #self.motor_pwm = max(94,91 + 6.5*FxR) # using writeMicroseconds() in Arduino + self.motor_pwm = 91 + 6.5*FxR # using writeMicroseconds() in Arduino + + # self.motor_pwm = max(94,90.74 + 6.17*FxR) + #self.motor_pwm = 90.74 + 6.17*FxR + + #self.motor_pwm = max(94,90.12 + 5.24*FxR) + #self.motor_pwm = 90.12 + 5.24*FxR + # Note: Barc doesn't move for u_pwm < 93 + else: # motor break / slow down + self.motor_pwm = 93.5 + 46.73*FxR + # self.motor_pwm = 98.65 + 67.11*FxR + #self.motor = 69.95 + 68.49*FxR + self.update_arduino() + def neutralize(self): + self.motor_pwm = 40 # slow down first + self.servo_pwm = 90 + self.update_arduino() + rospy.sleep(1) # slow down for 1 sec + self.motor_pwm = 80 + self.update_arduino() + def update_arduino(self): + self.ecu_cmd.header.stamp = get_rostime() + self.ecu_cmd.motor = self.motor_pwm + self.ecu_cmd.servo = self.servo_pwm + self.ecu_pub.publish(self.ecu_cmd) + + def pwm_convert_acc(self, acc): + FxR = float(acc) + if FxR == 0: + motor_pwm = 90.0 + elif FxR > 0: + motor_pwm = 91 + 6.5 * FxR # using writeMicroseconds() in Arduino + + else: # motor break / slow down + motor_pwm = 93.5 + 46.73 * FxR + + return motor_pwm + + def pwm_convert_steering(self, steering): + return 81.4 + 89.1 * float(steering) + # return 83.3 + 103.1 * float(steering) + +def arduino_interface(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + init_node('arduino_interface') + llc = low_level_control() + + Subscriber('ecu', ECU, llc.pwm_converter_callback, queue_size = 1) + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size = 1) + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + # process callbacks and keep alive + spin() + +def steering_mapping(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + # init_node('arduino_interface') + + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering_pwm = rospy.get_param(node_name + "/steering_pwm") + acceleration_pwm = rospy.get_param(node_name + "/acceleration_pwm") + acceleration_neg_pwm = rospy.get_param(node_name + "/acceleration_neg_pwm") + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + acceleration_time = 3 # accelerate for four seconds + max_counts = loop_rate * acceleration_time + + """ + if mode == "steering": + max_counts = 100 + + for j in range(60, 130, 10): + i = 0 + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = j + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + i += 1 + rate.sleep() + """ + + if mode == "steering": + max_counts = 1500 + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + i += 1 + rate.sleep() + + elif mode == "acceleration": + + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = acceleration_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + if enc.v_rr > 1e-5: + i += 1 + rate.sleep() + + while not rospy.is_shutdown(): + llc.motor_pwm = acceleration_neg_pwm + llc.servo_pwm = steering_pwm + llc.update_arduino() + + sensor_readings.v_x = enc.v_meas + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + enc.saveHistory() + imu.saveHistory() + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + if mode == "acceleration": + directory_string = str(steering_pwm) + "_" + str(acceleration_pwm) + "_" + str(acceleration_neg_pwm) + else: + directory_string = str(steering_pwm) + "_" + str(acceleration_pwm) + + directory = directory + directory_string + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + + +def corner_stiffness(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering = rospy.get_param(node_name + "/steering") + acceleration = rospy.get_param(node_name + "/acceleration") + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + duration = 60 # accelerate for 60 seconds + max_counts = loop_rate * duration + + pwm_acc = llc.pwm_convert_acc(acceleration) + pwm_steering = llc.pwm_convert_steering(steering) + + print pwm_steering + + i = 0 + + while not rospy.is_shutdown() and i <= max_counts: + llc.motor_pwm = pwm_acc + llc.servo_pwm = pwm_steering + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + if enc.v_rr > 1e-5: + enc.saveHistory() + imu.saveHistory() + i += 1 + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + directory_string = str(steering) + "_" + str(acceleration) + directory = directory + directory_string + + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + +def increasing_acc(): + # launch node, subscribe to motorPWM and servoPWM, publish ecu + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + steering = rospy.get_param(node_name + "/steering") + acceleration_1 = rospy.get_param(node_name + "/acceleration_1") + acceleration_2 = rospy.get_param(node_name + "/acceleration_2") + acceleration_3 = rospy.get_param(node_name + "/acceleration_3") + + acc = [acceleration_1, acceleration_2, acceleration_3] + + llc = low_level_control() + + llc.ecu_pub = Publisher('ecu_pwm', ECU, queue_size=1) + state_pub_pos = rospy.Publisher('pos_info', pos_info, queue_size=1) + sensor_readings = pos_info() + + # Set motor to neutral on shutdown + on_shutdown(llc.neutralize) + + t0 = rospy.get_rostime().to_sec() + imu = ImuClass(t0) + enc = EncClass(t0) + + loop_rate = 20 + rate = rospy.Rate(loop_rate) + duration = 10 # accelerate for 60 seconds + max_counts = loop_rate * duration + + pwm_acc = llc.pwm_convert_acc(acceleration) + pwm_steering = llc.pwm_convert_steering(steering) + + i = 0 + j = 0 + + while not rospy.is_shutdown(): + if i <= max_counts: + llc.motor_pwm = llc.pwm_convert_acc(acc[j]) + llc.servo_pwm = llc.pwm_convert_steering(steering) + llc.update_arduino() + + sensor_readings.v_x = enc.v_rr + sensor_readings.psiDot = imu.psiDot + state_pub_pos.publish(sensor_readings) + + if enc.v_rr > 1e-5: + enc.saveHistory() + imu.saveHistory() + i += 1 + + if i == max_counts and j < 3: + i = 0 + j += 1 + + rate.sleep() + + homedir = os.path.expanduser("~") + directory = homedir + "/barc_debugging/" + mode + "/" + directory_string = str(steering) + "_" + str(acc[0]) + "_" + str(acc[1]) + "_" + str(acc[2]) + directory = directory + directory_string + + try: + os.mkdir(directory) + except: + print "Directory already exists" + + pathSave = directory + "/estimator_imu.npz" + np.savez(pathSave,psiDot_his = imu.psiDot_his, + roll_his = imu.roll_his, + pitch_his = imu.pitch_his, + yaw_his = imu.yaw_his, + ax_his = imu.ax_his, + ay_his = imu.ay_his, + imu_time = imu.time_his) + + pathSave = directory + "/estimator_enc.npz" + np.savez(pathSave,v_fl_his = enc.v_fl_his, + v_fr_his = enc.v_fr_his, + v_rl_his = enc.v_rl_his, + v_rr_his = enc.v_rr_his, + v_meas_his = enc.v_meas_his, + enc_time = enc.time_his) + +############################################################# +if __name__ == '__main__': + try: + init_node('arduino_interface') + node_name = rospy.get_name() + mode = rospy.get_param(node_name + "/mode") + + if mode == "corner_stiffness": + corner_stiffness() + elif mode == "acceleration" or mode == "steering": + steering_mapping() + elif mode == "increasing_acc": + increasing_acc() + else: + print "Chosen mode unavailable." + # arduino_interface() + except ROSInterruptException: + pass diff --git a/workspace/src/barc/src/system_models.py b/workspace/src/barc/src/system_models.py old mode 100644 new mode 100755 index 1893eb55..8112c365 --- a/workspace/src/barc/src/system_models.py +++ b/workspace/src/barc/src/system_models.py @@ -1,231 +1,141 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -from numpy import sin, cos, tan, arctan, array, dot -from numpy import sign, argmin, sqrt -import rospy - -# discrete non-linear bicycle model dynamics -def f_2s(z, u, vhMdl, trMdl, dt, v_x): - """ - process model - input: state z at time k, z[k] := [beta[k], r[k]], (i.e. slip angle and yaw rate) - output: state at next time step (k+1) - """ - - # get states / inputs - beta = z[0] - r = z[1] - d_f = u - - # extract parameters - (a,b,m,I_z) = vhMdl - (trMdlFront, trMdlRear) = trMdl - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan(beta + a*r/v_x) - d_f - a_R = arctan(beta - b*r/v_x) - - # compute tire force - FyF = f_pajecka(trMdlFront, a_F) - FyR = f_pajecka(trMdlRear, a_R) - - # compute next state - beta_next = beta + dt*(-r + (1/(m*v_x))*(FyF*cos(d_f)+FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR); - return array([beta_next, r_next]) - -# discrete non-linear bicycle model dynamics -def f_3s(z, u, vhMdl, trMdl, F_ext, dt): - """ - process model - input: state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) - output: state at next time step z[k+1] - """ - - # get states / inputs - v_x = z[0] - v_y = z[1] - r = z[2] - d_f = u[0] - FxR = u[1] - - # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pajecka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pajecka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - FyR = min(FyR_max, max(-FyR_max, FyR_paj)) - - # compute next state - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([v_x_next, v_y_next, r_next]) - -# discrete non-linear bicycle model dynamics 6-dof -def f_6s(z, u, vhMdl, trMdl, F_ext, dt): - """ - process model - input: state z at time k, z[k] := [X[k], Y[k], phi[k], v_x[k], v_y[k], r[k]]) - output: state at next time step z[k+1] - """ - - # get states / inputs - X = z[0] - Y = z[1] - phi = z[2] - v_x = z[3] - v_y = z[4] - r = z[5] - - d_f = u[0] - FxR = u[1] - - # extract parameters - (a,b,m,I_z) = vhMdl - (a0, Ff) = F_ext - (trMdlFront, trMdlRear) = trMdl - (B,C,mu) = trMdlFront - g = 9.81 - Fn = m*g/2.0 # assuming a = b (i.e. distance from CoG to either axel) - - # limit force to tire friction circle - if FxR >= mu*Fn: - FxR = mu*Fn - - # comptue the front/rear slip [rad/s] - # ref: Hindiyeh Thesis, p58 - a_F = arctan((v_y + a*r)/v_x) - d_f - a_R = arctan((v_y - b*r)/v_x) - - # compute lateral tire force at the front - TM_param = [B, C, mu*Fn] - FyF = -f_pajecka(TM_param, a_F) - - # compute lateral tire force at the rear - # ensure that magnitude of longitudinal/lateral force lie within friction circle - FyR_paj = -f_pajecka(TM_param, a_R) - FyR_max = sqrt((mu*Fn)**2 - FxR**2) - Fy = array([FyR_max, FyR_paj]) - idx = argmin(abs(Fy)) - FyR = Fy[idx] - - # compute next state - X_next = X + dt*(v_x*cos(phi) - v_y*sin(phi)) - Y_next = Y + dt*(v_x*sin(phi) + v_y*cos(phi)) - phi_next = phi + dt*r - v_x_next = v_x + dt*(r*v_y +1/m*(FxR - FyF*sin(d_f)) - a0*v_x**2 - Ff) - v_y_next = v_y + dt*(-r*v_x +1/m*(FyF*cos(d_f) + FyR)) - r_next = r + dt/I_z*(a*FyF*cos(d_f) - b*FyR) - - return array([X_next, Y_next, phi_next, v_x_next, v_y_next, r_next]) - - - -def h_2s(x): - """ - measurement model - state: z := [beta, r], (i.e. slip angle and yaw rate) - output h := r (yaw rate) - """ - C = array([[0, 1]]) - return dot(C, x) - -def h_3s(x): - """ - measurement model - input := state z at time k, z[k] := [v_x[k], v_y[k], r[k]]) - output := [v_x, r] (yaw rate) - """ - C = array([[1, 0, 0], - [0, 0, 1]]) - return dot(C, x) - - -def f_pajecka(trMdl, alpha): - """ - f_pajecka = d*sin(c*atan(b*alpha)) - - inputs : - * trMdl := tire model, a list or tuple of parameters (b,c,d) - * alpha := tire slip angle [radians] - outputs : - * Fy := lateral force from tire [Newtons] - """ - (b,c,d) = trMdl - return d*sin(c*arctan(b*alpha)) - - -def f_KinBkMdl(z,u,vhMdl, dt): - """ - process model - input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] - output: state at next time step z[k+1] - """ - - # get states / inputs - x = z[0] - y = z[1] - psi = z[2] - v = z[3] - - d_f = u[0] - a = u[1] - - # extract parameters - (L_a, L_b) = vhMdl - - # compute slip angle - bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) - - # compute next state - x_next = x + dt*( v*cos(psi + bta) ) - y_next = y + dt*( v*sin(psi + bta) ) - psi_next = psi + dt*v/L_b*sin(bta) - v_next = v + dt*a - - return array([x_next, y_next, psi_next, v_next]) - -def h_KinBkMdl(x): - """ - measurement model - """ - C = array([[0, 0, 1, 0], - [0, 0, 0, 1]]) - return dot(C, x) - +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Attibution Information: The barc project ROS code-base was developed +# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales +# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed +# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was +# based on an open source project by Bruce Wootton +# --------------------------------------------------------------------------- + +from numpy import sin, cos, tan, arctan, array, dot +from numpy import sign, sqrt +import math + +def f_KinBkMdl(z, u, vhMdl, dt, est_mode): + """ + process model + input: state z at time k, z[k] := [x[k], y[k], psi[k], v[k]] + output: state at next time step z[k+1] + Does not account for drift in psi estimation! + -> Either put low trust on measured psi-values or add extra state for drift estimation! + """ + #c = array([0.5431, 1.2767, 2.1516, -2.4169]) + + # get states / inputs + x = z[0] + y = z[1] + psi = z[2] + v = z[3] + + d_f = u[0] + a = u[1] + + # extract parameters + (L_a, L_b) = vhMdl + + # compute slip angle + bta = arctan( L_a / (L_a + L_b) * tan(d_f) ) + + # compute next state + x_next = x + dt*( v*cos(psi + bta) ) + y_next = y + dt*( v*sin(psi + bta) ) + psi_next = psi + dt*v/L_b*sin(bta) + v_next = v + dt*(a - 0.63*sign(v)*v**2) + + return array([x_next, y_next, psi_next, v_next]) + +def h_KinBkMdl(x, u, vhMdl, dt, est_mode): + """ + Measurement model + """ + if est_mode==1: # GPS, IMU, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==2: # IMU, Enc + C = array([[0, 0, 1, 0], + [0, 0, 0, 1]]) + elif est_mode==3: # GPS + C = array([[1, 0, 0, 0], + [0, 1, 0, 0]]) + elif est_mode==4: # GPS, Enc + C = array([[1, 0, 0, 0], + [0, 1, 0, 0], + [0, 0, 0, 1]]) + else: + print("Wrong est_mode") + return dot(C, x) + +def f_SensorKinematicModel(z, u, vhMdl, dt, est_mode): + """ This Sensor model contains a pure Sensor-Model and a Kinematic model. They're independent from each other.""" + (l_A,l_B) = vhMdl + bta = math.atan2(l_A*tan(u[1]),l_A+l_B) + zNext = [0]*14 + zNext = [0]*9 + zNext[0] = z[0] + dt*(cos(z[6])*z[2] - sin(z[6])*z[3]) # x + zNext[1] = z[1] + dt*(sin(z[6])*z[2] + cos(z[6])*z[3]) # y + zNext[2] = z[2] + dt*(z[4]+z[7]*z[3]) # v_x + zNext[3] = z[3] + dt*(z[5]-z[7]*z[2]) # v_y + zNext[4] = z[4] # a_x + zNext[5] = z[5] # a_y + zNext[6] = z[6] + dt*(z[7]) # psi + zNext[7] = z[7] # psidot + zNext[8] = z[8] # drift_psi + + + # zNext[9] = z[9] + dt*(z[12]*cos(z[11] + bta)) # x + # zNext[10] = z[10] + dt*(z[12]*sin(z[11] + bta)) # y + # zNext[11] = z[11] + dt*(z[12]/l_B*sin(bta)) # psi + # zNext[12] = z[12] + dt*(u[0] - 0.5*z[12]) # v + # zNext[13] = z[13] # drift_psi_2 + return array(zNext) + +def h_SensorKinematicModel(x, u, vhMdl, dt, est_mode): + """ This is the measurement model to the kinematic<->sensor model above """ + y = [0]*9 + y = [0]*8 + y[0] = x[0] # x + y[1] = x[1] # y + # y[2] = sqrt(x[2]**2+x[3]**2) # v + y[2] = x[2] # vx + y[3] = x[6]+x[8] # psi + # y[3] = x[6] # psi + y[4] = x[7] # psiDot + y[5] = x[4] # a_x + y[6] = x[5] # a_y + # y[7] = x[2] # vx + y[7] = x[3] # vy + + # y[7] = x[9] # x + # y[8] = x[10] # y + # y[9] = x[11]+x[13] # psi + # y[10] = x[12] # v + # y[11] = x[2] # v_x + # y[12] = x[3] # v_y + + # # this is for 12 measurement, which does not use yaw_meas from imu + # y[0] = x[0] # x + # y[1] = x[1] # y + # # y[2] = sqrt(x[2]**2+x[3]**2) # v + # y[2] = x[2] # vx + # # y[3] = x[6]+x[8] # psi + # # y[3] = x[6] # psi + # y[3 ] = x[7] # psiDot + # y[4 ] = x[4] # a_x + # y[5 ] = x[5] # a_y + + # y[6 ] = x[9] # x + # y[7 ] = x[10] # y + # y[8 ] = x[11]+x[13] # psi + # y[9 ] = x[12] # v + # y[10] = x[2] # v_x + # y[11] = x[3] # v_y + return array(y) diff --git a/workspace/src/barc/src/track.jl b/workspace/src/barc/src/track.jl new file mode 100755 index 00000000..07fb5229 --- /dev/null +++ b/workspace/src/barc/src/track.jl @@ -0,0 +1,373 @@ +#!/usr/bin/env julia + +#= + File name: track.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +using JLD +using PyPlot +using PyCall +using Distances +# pygui(true) + + + +type Track + ds::Float64 + width::Float64 + total_length::Float64 + thetas::Array{Float64} + curvature + + xy_coords::Array{Float64} + xy_outer::Array{Float64} + xy_inner::Array{Float64} + s_coord::Array{Float64} + + shape::AbstractString + + Track() = new() +end + +function init!(track::Track) # Standard Constructor + println("JULIA") + # track.shape = "oval" + # track.shape = "track_2" + track.shape = TRACK_NAME + println(track.shape) + track.ds = 1 // 10 + @assert track.ds > 0. + track.width = TRACK_WIDTH + + track.total_length = 0.0 + + track.thetas = [0.0] # initialize theta + track.curvature = [] # initialize curvature + track.xy_coords = [0.0 0.0] # initialize x and y + track.xy_outer = [0.0 0.0] + track.xy_inner = [0.0 0.0] + + tracks_dir = "" + + try + tracks_dir = readdir(TRACK_DIR) + end + + if contains(tracks_dir, track.shape * ".jld") + println("Loading track $(track.shape)") + load_track!(track) + else + println("Creating track $(track.shape)") + create_track!(track) + # save_track(track) + end + println("total length: ", track.total_length) +end + +function create_track!(track::Track) + if track.shape == "oval" + oval_track!(track) + elseif track.shape == "track_2" + track_2!(track) + elseif track.shape == "track_3" + track_3!(track) + elseif track.shape == "l_shape" + l_shape!(track) + else + error("Track $(track.shape) is not available.") + end + + track.xy_coords = zeros(length(track.thetas), 2) + track.xy_outer = zeros(length(track.thetas), 2) + track.xy_inner = zeros(length(track.thetas), 2) + track.xy_outer[1, 2] = track.width / 2 + track.xy_inner[1, 2] = - track.width / 2 + + track.s_coord = track.ds * ones(length(track.thetas)) + track.s_coord[1] = 0.0 + track.s_coord = cumsum(track.s_coord) + + for i = 2 : length(track.thetas) + theta = track.thetas[i] + delta_theta = track.thetas[i] - track.thetas[i - 1] + + # TODO: Consider the transition from one radius to another + # radius + if abs(delta_theta) > 1e-5 && get_curvature_check(track, track.s_coord[i]) > 1e-5 + radius = 1 / get_curvature_check(track, track.s_coord[i]) + + # TODO: Should be absolute radius?? + chord = 2 * radius * sin(delta_theta / 2) + + # track.xy_coords[i, :] = chord * [cos(theta) sin(theta)] + + initial_point = track.xy_coords[1, :] + s_direction = [initial_point[1] + chord; initial_point[2]] + s_rotated = rotate_around_z(s_direction, track.thetas[i - 1] + delta_theta / 2) + track.xy_coords[i, :] = s_rotated' + track.xy_coords[i - 1, :] + else + # in straight segments the track.ds is not curved and can + # therefore be directly used + track.xy_coords[i, :] = track.xy_coords[i - 1, :] + track.ds * [cos(theta) sin(theta)] + end + track.xy_outer[i, :] = track.width / 2 * [cos(theta + pi / 2) + sin(theta + pi / 2)] + track.xy_inner[i, :] = track.width / 2 * [cos(theta - pi / 2) + sin(theta - pi / 2)] + end + + if track.shape == "l_shape" + track.xy_coords[140 : 160, 2] -= cumsum(0.0505319492481863 / 21 * ones(21)) + track.xy_coords[161 : end, 2] -= 0.0505319492481863 + end + + # track.xy_coords = cumsum(track.xy_coords) + track.xy_outer += track.xy_coords + track.xy_inner += track.xy_coords + + #= + println(track.xy_coords[1, :]) + println(track.xy_coords[end, :]) + plot(track.xy_coords[:, 1], track.xy_coords[:, 2], "ko") + axis("equal") + plt[:show]() + plt[:pause](30.0) + =# + + # Make sure that the start and end point of the track align + if sum(abs2(track.xy_coords[1, :] - track.xy_coords[end, :])) > 1e-2 + println(track.xy_coords[1, :]) + println(track.xy_coords[end, :]) + error("Track starting and end points don't align") + end + # @assert sum(abs2(track.xy_coords[1, :] - track.xy_coords[end, :])) < 1e-3 + + # track.total_length = round(Int64, length(track.thetas) * track.ds) + # track.total_length = (length(track.thetas) - 1) * track.ds +end + +function oval_track!(track::Track) + # add_segment!(track, 1.0, 0.0) + # add_segment!(track, 4.5, 1.0 * pi) + # add_segment!(track, 2.0, 0.0) + # add_segment!(track, 4.5, 1.0 * pi) + # add_segment!(track, 1.0, 0.0) + + add_segment!(track, 1.0, 0.0) + add_segment!(track, 6.0, 1.0 * pi) + add_segment!(track, 2.0, 0.0) + add_segment!(track, 6.0, 1.0 * pi) + add_segment!(track, 1.0, 0.0) + + #= + add_segment!(track, 1.0, 0.0) + add_segment!(track, 4.5, - pi) + add_segment!(track, 2.0, 0.0) + add_segment!(track, 4.5, - pi) + add_segment!(track, 1.0, 0.0) + =# + + #= + add_segment!(track, 2.0, 0.0) + add_segment!(track, 9.0, - pi) + add_segment!(track, 4.0, 0.0) + add_segment!(track, 9.0, - pi) + add_segment!(track, 2.0, 0.0) + =# + + #= + add_segment!(track, 6.0, 0.0) + add_segment!(track, 14.0, - pi) + add_segment!(track, 12.0, 0.0) + add_segment!(track, 14.0, - pi) + add_segment!(track, 6.0, 0.0) + =# +end + +function l_shape!(track::Track) + #= + add_segment!(track, 1.5, 0.0) + add_segment!(track, 2.2, 1.0 * pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, 1.0 * pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, - 1.0 * pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, 1.0 * pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, 1.0 * pi / 2) + add_segment!(track, 3.0, 0.0) + add_segment!(track, 2.2, 1.0 * pi / 2) + add_segment!(track, 1.5, 0.0) + =# + + add_segment!(track, 1.5, 0.0) + add_segment!(track, 2.2, pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, - pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, pi / 2) + add_segment!(track, 0.1, 0.0) + add_segment!(track, 2.2, pi / 2) + add_segment!(track, 3.0, 0.0) + add_segment!(track, 2.2, pi / 2) + add_segment!(track, 1.5, 0.0) +end + +function track_3!(track::Track) + denom = 3.5 + small = false + + if small + add_segment!(track, 1.7, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 0.5, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 1.2, pi / (2 * denom)) + add_segment!(track, 1.2, - pi / denom) + add_segment!(track, 1.2, pi / (2 * denom)) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 0.5, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 1.8, 0.0) + else + add_segment!(track, 1.0, 0.0) + add_segment!(track, 3.0, 1.0 * pi / 2) + add_segment!(track, 1.0, 0.0) + add_segment!(track, 3.0, 1.0 * pi / 2) + add_segment!(track, 2.0, 0.0) + add_segment!(track, 3.0, 1.0 * pi / 2) + add_segment!(track, 1.0, 0.0) + add_segment!(track, 3.0, 1.0 * pi / 2) + add_segment!(track, 1.0, 0.0) + + #= + add_segment!(track, 3.0, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 2.0, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 2.0, pi / (2 * denom)) + add_segment!(track, 2.0, - pi / denom) + add_segment!(track, 2.0, pi / (2 * denom)) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 2.0, 0.0) + add_segment!(track, 2.0, - pi / 2) + add_segment!(track, 2.8, 0.0) + =# + end +end + +function add_segment!(track::Track, length::Float64, angle::Float64) + num_pieces = round(Int64, length / track.ds) + # radius = length / angle + curvature = angle / length # 1 / radius + + angle_per_segment = angle / num_pieces + thetas_segment = angle_per_segment * ones(num_pieces) + thetas_segment[1] += track.thetas[end] + append!(track.thetas, cumsum(thetas_segment)) + # append!(track.curvature, curvature * ones(num_pieces)) + + start_interval = track.total_length + end_interval = start_interval + length + kappa = [(start_interval, end_interval), curvature] + if size(track.curvature, 1) == 0 + track.curvature = [kappa] + else + track.curvature = [track.curvature kappa] + end + track.total_length += length +end + +function get_s_coord(track::Track, s) + if length(s) == 1 + return s % track.total_length + elseif length(s) == 6 + s[1] = s[1] % track.total_length + return s + else + error("s-coordinate is not matching any of the predefined types.") + end +end + +function get_curvature(track::Track, s::Float64) + s = get_s_coord(track, s) + + for i = 1 : size(track.curvature, 2) + start_interval = track.curvature[1, i][1] + end_interval = track.curvature[1, i][2] + + if s >= start_interval && s < end_interval + return track.curvature[2, i] + end + if s <= start_interval && abs(track.curvature[2, i]) > 1e-5 + return track.curvature[2, i] + end + end +end + +function get_curvature_check(track::Track, s::Float64) + delta = 1e-5 + s_plus = s + delta + s_minus = s - delta + + curvature_plus = get_curvature(track, s_plus) + curvature_minus = get_curvature(track, s_minus) + + if abs(curvature_plus) > abs(curvature_minus) + return curvature_plus + else + return curvature_minus + end +end + +function get_theta(track::Track, s::Float64) + s = get_s_coord(track, s) + + distances_to_s = colwise(Euclidean(), track.s_coord[:, 1]', [s]) + s_indeces = sortperm(distances_to_s)[1 : 2] + delta_theta = track.thetas[s_indeces[2]] - track.thetas[s_indeces[2]] + theta = track.thetas[s_indeces[1]] + delta_theta / track.ds + + return theta +end + +function save_track(track::Track) + filename = ascii(TRACK_DIR * track.shape * ".jld") + jldopen(filename, "w") do file + JLD.write(file, "total_length", track.total_length) + JLD.write(file, "thetas", track.thetas) + JLD.write(file, "curvature", track.curvature) + JLD.write(file, "xy_coords", track.xy_coords) + JLD.write(file, "xy_outer", track.xy_outer) + JLD.write(file, "xy_inner", track.xy_inner) + JLD.write(file, "s_coord", track.s_coord) + end +end + +function save_track(track::Track, filename::AbstractString) + jldopen(filename, "w") do file + JLD.write(file, "track", track) + end +end + +function load_track!(track::Track) + Data = load(ascii(TRACK_DIR * track.shape * ".jld")) + track.total_length = Data["total_length"] + track.thetas = Data["thetas"] + track.curvature = Data["curvature"] + track.xy_coords = Data["xy_coords"] + track.xy_outer = Data["xy_outer"] + track.xy_inner = Data["xy_inner"] + track.s_coord = Data["s_coord"] +end + +function load_track!(track::Track, filename::AbstractString) + Data = load(filename) + track = Data["track"] +end diff --git a/workspace/src/barc/src/track.py b/workspace/src/barc/src/track.py new file mode 100644 index 00000000..a5543df6 --- /dev/null +++ b/workspace/src/barc/src/track.py @@ -0,0 +1,322 @@ +#!usr/bin/env python + +''' + File name: track.py + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Python Version: 2.7.12 +''' + +import numpy as np +from numpy import linalg as LA +import os +import json +import pdb +import matplotlib.pyplot as plt + + +class Track: + """docstring for Track""" + + total_length = 0. + s_coords = np.zeros(1) + + thetas = [0.0] + curvature = [] + xy_coords = np.zeros(2) + xy_outer = np.zeros(2) + xy_inner = np.zeros(2) + + # tracks_dir = os.listdir(os.getcwd() + "/../tracks") + tracks_dir = "/home/lukas/barc/workspace/src/barc/tracks" + + def __init__(self, ds, shape, width): + self.ds = ds + assert self.ds > 0. + self.shape = shape + self.width = width + + if self.shape + ".npy" in self.tracks_dir: + print "Loading track " + self.shape + # self.load_track() + else: + print "Creating track " + self.shape + self.create_track() + # self.save_track() + + print "Total length of track: " + str(self.total_length) + + @classmethod + def from_json(cls, filename): + with open(filename) as f: + track_dict = json.load(f) + + ds = track_dict["ds"] + shape = track_dict["shape"] + width = track_dict["width"] + track = cls(ds, shape, width) + + return track + + def create_track(self): + if self.shape == "oval": + self.oval_track() + elif self.shape == "test": + self.test_track() + elif self.shape == "l_shape": + self.l_shape() + else: + raise ValueError("Track %s is not available" % self.shape) + + num_points = len(self.thetas) + + # Transform lists to numpy arrays + self.thetas = np.asarray(self.thetas) + self.curvature = np.asarray(self.curvature) + + self.xy_coords = np.zeros((num_points, 2)) + self.xy_outer = np.zeros((num_points, 2)) + self.xy_inner = np.zeros((num_points, 2)) + self.xy_outer[0, 1] = self.width / 2 + self.xy_inner[0, 1] = - self.width / 2 + + self.s_coords = self.ds * np.ones(num_points) + self.s_coords[0] = 0.0 + self.s_coords = np.cumsum(self.s_coords) + + sign_x = 1.0 + sign_y = 1.0 + + for i in range(1, num_points): + theta = self.thetas[i] + delta_theta = theta - self.thetas[i - 1] + + if abs(delta_theta) > 1e-5 and abs(self.get_curvature_check(self.s_coords[i])) > 1e-5: + radius = abs(1. / self.get_curvature_check(self.s_coords[i])) + + chord = 2 * radius * np.sin(abs(delta_theta) / 2) + + initial_point = self.xy_coords[0, :] + s_direction = np.array([initial_point[0] + chord, initial_point[1]]) + s_rotated = rotate_around_z(s_direction, self.thetas[i - 1] + delta_theta / 2) + self.xy_coords[i, :] = s_rotated + self.xy_coords[i - 1, :] + else: + self.xy_coords[i, :] = self.xy_coords[i - 1, :] + self.ds * np.array([np.cos(theta), np.sin(theta)]) + + self.xy_outer[i, :] = self.width / 2 * np.array([np.cos(theta + np.pi / 2), + np.sin(theta + np.pi / 2)]) + self.xy_inner[i, :] = self.width / 2 * np.array([np.cos(theta - np.pi / 2), + np.sin(theta - np.pi / 2)]) + + self.xy_outer += self.xy_coords + self.xy_inner += self.xy_coords + + # assert LA.norm(self.xy_coords[0, :] - self.xy_coords[- 1, :]) < 1e-3 + + if sum(abs(self.xy_coords[0, :] - self.xy_coords[- 1, :])) > 1e-2: + print(self.xy_coords[0, :]) + print(self.xy_coords[- 1, :]) + print("Track starting and end point don't align.") + # exit() + + def oval_track(self): + # self.add_segment(1.0, 0.0) + # self.add_segment(4.5, np.pi) + # self.add_segment(2.0, 0.0) + # self.add_segment(4.5, np.pi) + # self.add_segment(1.0, 0.0) + + self.add_segment(1.0, 0.0) + self.add_segment(6.0, np.pi) + self.add_segment(2.0, 0.0) + self.add_segment(6.0, np.pi) + self.add_segment(1.0, 0.0) + + """ + self.add_segment(1.0, 0.0) + self.add_segment(4.5, - np.pi) + self.add_segment(2.0, 0.0) + self.add_segment(4.5, - np.pi) + self.add_segment(1.0, 0.0) + """ + + """ + self.add_segment(2.0, 0.0) + self.add_segment(9.0, - np.pi) + self.add_segment(4.0, 0.0) + self.add_segment(9.0, - np.pi) + self.add_segment(2.0, 0.0) + """ + """ + self.add_segment(6.0, 0.0) + self.add_segment(14.0, - np.pi) + self.add_segment(12.0, 0.0) + self.add_segment(14.0, - np.pi) + self.add_segment(6.0, 0.0) + """ + + def l_shape(self): + self.add_segment(1.5, 0.0) + self.add_segment(2.2, np.pi / 2) + self.add_segment(0.1, 0.0) + self.add_segment(2.2, np.pi / 2) + self.add_segment(0.1, 0.0) + self.add_segment(2.2, - np.pi / 2) + self.add_segment(0.1, 0.0) + self.add_segment(2.2, np.pi / 2) + self.add_segment(0.1, 0.0) + self.add_segment(2.2, np.pi / 2) + self.add_segment(3.0, 0.0) + self.add_segment(2.2, np.pi / 2) + self.add_segment(1.5, 0.0) + + def test_track(self): + denominator = 3.5 + small = False + + if small: + self.add_segment(1.7, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(0.5, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.2, np.pi / (2 * denominator)) + self.add_segment(1.2, - np.pi / denominator) + self.add_segment(1.2, np.pi / (2 * denominator)) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(0.5, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.8, 0.0) + else: + self.add_segment(1.0, 0.0) + self.add_segment(3.0, np.pi / 2) + self.add_segment(1.0, 0.0) + self.add_segment(3.0, np.pi / 2) + self.add_segment(2.0, 0.0) + self.add_segment(3.0, np.pi / 2) + self.add_segment(1.0, 0.0) + self.add_segment(3.0, np.pi / 2) + self.add_segment(1.0, 0.0) + + """ + self.add_segment(2.5, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.0, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.5, np.pi / (2 * denominator)) + self.add_segment(1.5, - np.pi / denominator) + self.add_segment(1.5, np.pi / (2 * denominator)) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.0, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(1.9, 0.0) + """ + + """ + self.add_segment(3.0, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(2.0, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(2.0, np.pi / (2 * denominator)) + self.add_segment(2.0, - np.pi / denominator) + self.add_segment(2.0, np.pi / (2 * denominator)) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(2.0, 0.0) + self.add_segment(2.0, - np.pi / 2) + self.add_segment(2.8, 0.0) + """ + + def add_segment(self, length, angle): + num_pieces = int(round(length / self.ds)) + # radius = length / angle + curvature = angle / length # = 1 / radius + + angle_per_segment = angle / (num_pieces) + thetas_segment = angle_per_segment * np.ones(num_pieces) + thetas_segment[0] += self.thetas[- 1] + self.thetas += (np.cumsum(thetas_segment)).tolist() + + # self.curvature += (curvature * np.ones(num_pieces)).tolist() + + start_interval = self.total_length + end_interval = start_interval + length + kappa = [(start_interval, end_interval), curvature] + self.curvature.append(kappa) + self.total_length += length + + def get_s(self, s): + if isinstance(s, np.float64): + s = abs(s) % self.total_length + return s + elif s.shape[0] == 6: + s[0] = abs(s[0]) % self.total_length + return s + else: + ValueError("The s-coordinate is not matching any of the " + "predefined types.") + + def get_curvature(self, s): + s = self.get_s(s) + + for i in range(len(self.curvature)): + interval = self.curvature[i][0] + start_interval = interval[0] + end_interval = interval[1] + + if s >= start_interval and s < end_interval: + curvature = self.curvature[i][1] + return curvature + if s <= end_interval and abs(self.curvature[i][1]) > 1e-5: + curvature = self.curvature[i][1] + return curvature + """ + elif s == 0.0: + curvature = 0.0 + return curvature + """ + def get_curvature_check(self, s): + delta = 1e-5 + s_plus = s + delta + s_minus = s - delta + + curvature_plus = self.get_curvature(s_plus) + curvature_minus = self.get_curvature(s_minus) + + if abs(curvature_plus) > abs(curvature_minus): + return curvature_plus + else: + return curvature_minus + + def get_theta(self, s): + return 0 + + def save_track(self): + return 0 + + def load_track(self): + return 0 + + +def rotate_around_z(xy_coords, angle): + rotation_matrix = np.array([[np.cos(angle), - np.sin(angle)], + [np.sin(angle), np.cos(angle)]]) + + return np.dot(rotation_matrix, xy_coords) + + +if __name__ == "__main__": + import matplotlib.pyplot as plt + + track = Track(1. / 10, "oval", 0.8) + # track = Track.from_json("track.json") + + # plt.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], "ro") + # plt.plot(track.xy_inner[:, 0], track.xy_inner[:, 1], "bo") + # plt.plot(track.xy_outer[:, 0], track.xy_outer[:, 1], "go") + # plt.axis("equal") + # plt.show() + + plt.plot(track.curvature, "ro") + # plt.axis("equal") + plt.show() + + pdb.set_trace() diff --git a/workspace/src/barc/src/transformations.jl b/workspace/src/barc/src/transformations.jl new file mode 100755 index 00000000..a4beed74 --- /dev/null +++ b/workspace/src/barc/src/transformations.jl @@ -0,0 +1,275 @@ +#!/usr/bin/env julia + +#= + File name: transformations.jl + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Julia Version: 0.4.7 +=# + +using Distances + +include("track.jl") + +function s_to_xy(track::Track, s_coord::Array{Float64}) + # TODO: handle the case for the first and final element + # TODO: assuming the s state to have 4 elements + # TODO: rename variables + if size(s_coord)[1] == 6 + s, e_y, e_psi, psi_dot, v_x, v_y = s_coord + elseif size(s_coord)[1] == 5 + s, e_y, e_psi, v_x, v_y = s_coord + else + s, e_y, e_psi, v = s_coord + end + + s = get_s_coord(track, s) + + # find the two xy in which interval the s_coord lies + distances_to_track = colwise(Euclidean(), track.s_coord[:, 1]', [s]) + two_closest_indices = sortperm(distances_to_track)[1:2] + + if distances_to_track[findmax(two_closest_indices)[1]] < 1e-5 + xy1_index = findmax(two_closest_indices)[1] + else + xy1_index = findmin(two_closest_indices)[1] + end + + s1_xy = track.xy_coords[xy1_index, :] + + theta = track.thetas[xy1_index] + + if xy1_index >= size(track.thetas, 1) + xy1_index = size(track.thetas, 1) - 1 + end + + abs_delta_theta = abs(track.thetas[xy1_index + 1] - theta) + delta_s = s - track.s_coord[xy1_index] + + if abs_delta_theta > 1e-5 && abs(get_curvature(track, s)) > 1e-5 + curvature = get_curvature(track, s) + + if curvature < 0.0 + delta_theta = - abs_delta_theta + else + delta_theta = abs_delta_theta + end + + delta_theta_prime = delta_theta / track.ds * delta_s + + r = 1.0 / curvature - e_y + chord = 2.0 * abs(r) * sin(abs(delta_theta_prime) / 2.0) + else + chord = delta_s + delta_theta = abs_delta_theta + delta_theta_prime = delta_theta / track.ds * delta_s + end + + theta_prime = theta + delta_theta_prime / 2.0 + + initial_point = track.xy_coords[1, :] + e_y_direction = [initial_point[1]; (initial_point[2] + e_y)] + e_y_rotated = rotate_around_z(e_y_direction, theta) + xy_from_origin = s1_xy' + e_y_rotated + + s_direction = [(initial_point[1] + chord); initial_point[2]] + s_rotated = rotate_around_z(s_direction, theta_prime) + e_y_prime = xy_from_origin + s_rotated + + x = e_y_prime[1] + y = e_y_prime[2] + + psi = e_psi + theta_prime + + if size(s_coord)[1] == 6 + return [x, y, v_x, v_y, psi, psi_dot] + elseif size(s_coord)[1] == 5 + return [x, y, v_x, v_y, psi, 0.0] + else + return [x, y, v, 0.0, psi, 0.0] + end + + +end + + +function xy_to_s(track::Track, xy_coord::Array{Float64}) + # TODO: fix this + # TODO: Handle case when the the xy_coord is in between the first + # and final point of s + # TODO: assuming the xy state to have 6 elements, handle different + # types of inputs + x, y, v_x, v_y, psi, psi_dot = xy_coord + + # find the two s in which interval the xy_coord lies + distances_to_track = colwise(Euclidean(), track.xy_coords', [x; y]) + two_closest_indices = sortperm(distances_to_track)[1:2] + if 1 in two_closest_indices + if x >= 0. + s1_index = 1 + else + s1_index = size(track.xy_coords)[1] - 1 + end + else + s1_index = findmin(two_closest_indices)[1] + end + + s1_s = track.s_coord[s1_index] + + if abs(track.thetas[s1_index] - track.thetas[s1_index + 1]) > 1e-5 + if abs(get_curvature(track, track.s_coord[s1_index + 1]) - get_curvature(track, track.s_coord[s1_index])) > 1e-5 + if abs(get_curvature(track, track.s_coord[s1_index + 1])) > 1e-5 + curvature = get_curvature(track, track.s_coord[s1_index + 1]) + # curvature = get_curvature(track, track.s_coord[s1_index]) + else + curvature = get_curvature(track, track.s_coord[s1_index]) + end + else + curvature = get_curvature(track, track.s_coord[s1_index]) + end + + radius = 1. / curvature + radius_abs = abs(radius) + + # Find the center of the circle segment + initial_point = track.xy_coords[1, :] + e_y_direction = [initial_point[1]; (initial_point[2] + radius)] + e_y_rotated = rotate_around_z(e_y_direction, track.thetas[s1_index]) + center_of_segment = e_y_rotated + track.xy_coords[s1_index, :]' + + # Applying the law of cosines + dist_to_s1 = distances_to_track[s1_index] + dist_to_center_segment = colwise(Euclidean(), center_of_segment, + [x; y])[1] + + nominator = radius_abs^2 + dist_to_center_segment^2 - dist_to_s1^2 + denom = 2 * radius_abs * dist_to_center_segment + if abs(nominator - denom) < 1e-5 + abs_phi = 0.0 + else + abs_phi = acos(nominator / denom) + end + + if curvature < 0.0 + phi = - abs_phi + else + phi = abs_phi + end + + #= + plot([x; center_of_segment[1]], [y; center_of_segment[2]], "r-") + plot([x; track.xy_coords[s1_index, 1]], + [y; track.xy_coords[s1_index, 2]], "r-") + plot([track.xy_coords[s1_index, 1]; center_of_segment[1]], + [track.xy_coords[s1_index, 2]; center_of_segment[2]], "r-") + =# + + # determining s and e_y + # take the radius, because s gets projected onto it + delta_s = radius_abs * abs_phi + + # TODO: might be the other way round, but it makes sense to me + # to define it this way + if curvature < 0.0 + e_y = radius_abs - dist_to_center_segment + else + e_y = - (radius_abs - dist_to_center_segment) + end + + # println("Theta: ", track.thetas[s1_index]) + # println("Phi: ", phi) + e_psi = psi - (track.thetas[s1_index] + phi) + else + # using the law of cosines to determine the + # s1 has the smaller s-value, can be further away or closer + theta = track.thetas[s1_index] + # println("Theta: ", theta) + + dist_to_s1 = distances_to_track[s1_index] + dist_to_s2 = distances_to_track[s1_index + 1] + + if dist_to_s1 < 1e-5 + delta_s = 0. + abs_e_y = 0. + elseif dist_to_s1 + dist_to_s2 <= track.ds + 1e-5 + delta_s = dist_to_s1 + abs_e_y = 0. + else + argument = (dist_to_s1^2 + track.ds^2 - dist_to_s2^2) / + (2 * dist_to_s1 * track.ds) + + #= + if argument < - 1.0 && argument >= - 1.0 - 1e-2 + argument = - 1.0 + elseif argument > 1.0 && argument <= 1.0 + 1e-2 + argument = 1.0 + end + =# + + if argument < - 1.0 + argument = - 1.0 + elseif argument > 1.0 + argument = 1.0 + end + + abs_phi = acos(argument) + + if get_curvature(track, track.s_coord[s1_index + 1]) < 0.0 + phi = abs_phi + else + phi = abs_phi + end + delta_s = dist_to_s1 * cos(phi) + abs_e_y = dist_to_s1 * sin(phi) + end + + # no phi added because it is a straight segment + e_psi = psi - theta + + initial_point = track.xy_coords[1, :] + e_y_direction = [initial_point[1]; (initial_point[2] + + (- track.width / 2))] + # (track.width / 2))] + e_y_rotated = rotate_around_z(e_y_direction, theta) + xy_position = track.xy_coords[s1_index, :]' + + delta_s * [cos(theta); sin(theta)] + # [delta_s; 0.0] + center_of_segment = e_y_rotated + xy_position + distance_to_inner_bound = colwise(Euclidean(), center_of_segment, + [x; y]) + if distance_to_inner_bound[1] <= track.width / 2 + e_y = abs_e_y + else + e_y = - abs_e_y + end + end + + s = s1_s + delta_s + + # println("e_psi before: ", e_psi) + + #= + if e_psi < - pi + e_psi = pi + + e_psi -= floor(e_psi / (2 * pi)) * 2 * pi # min(ceil(e_psi / (2 * pi)), - 1.0) * 2 * pi + elseif e_psi > pi + e_psi -= ceil(e_psi / (2 * pi)) * 2 * pi # max(floor(e_psi / (2 * pi)), 1.0) * 2 * pi + end + =# + e_psi = atan2(sin(e_psi), cos(e_psi)) + # e_psi = (e_psi + pi) % (2 * pi) - pi + + # println("e_psi after: ", e_psi) + + # (self.psi - self.theta[self.idx_curr]+pi)%(2*pi)-pi + + return [s, - e_y, e_psi, psi_dot, v_x, v_y] +end + + +function rotate_around_z(xy_coords::Array{Float64}, angle::Float64) + @assert size(xy_coords)[1] == 2 + rotation_matrix = [cos(angle) (- sin(angle)); sin(angle) cos(angle)] + + return rotation_matrix * xy_coords +end diff --git a/workspace/src/barc/src/transformations.py b/workspace/src/barc/src/transformations.py new file mode 100644 index 00000000..811a076c --- /dev/null +++ b/workspace/src/barc/src/transformations.py @@ -0,0 +1,373 @@ +#!usr/bin/env python + +''' + File name: transformations.py + Author: Lukas Brunke + Email: lukas.brunke@tuhh.de + Python Version: 2.7.12 +''' + +import numpy as np +from numpy import linalg as LA + +from track import Track + + +def s_to_xy(s_coord, track): + s, e_y, e_psi, psi_dot, v_x, v_y = s_coord + + s = track.get_s(s) + + distances_to_track = np.array([LA.norm(track.s_coords[i] - s) + for i in range(len(track.s_coords))]) + two_closest_indices = np.argsort(distances_to_track)[: 2] + + if distances_to_track[np.max(two_closest_indices)] < 1e-5: + xy1_index = np.max(two_closest_indices) + else: + xy1_index = np.min(two_closest_indices) + + s1_xy = track.xy_coords[xy1_index, :] + + theta = track.thetas[xy1_index] + abs_delta_theta = abs(track.thetas[xy1_index + 1] - theta) + delta_s = s - track.s_coords[xy1_index] + + # if abs(abs_delta_theta) > 1e-5 and abs(track.get_curvature(track.s_coords[xy1_index + 1])) > 1e-5: + if abs_delta_theta > 1e-5 and abs(track.get_curvature(s)) > 1e-5: + curvature = track.get_curvature(s) + + if curvature < 0.0: + delta_theta = - abs_delta_theta + else: + delta_theta = abs_delta_theta + + delta_theta_prime = delta_theta / track.ds * delta_s + + r = 1.0 / curvature + e_y + chord = 2.0 * abs(r) * np.sin(abs(delta_theta_prime) / 2.0) + else: + chord = delta_s + delta_theta = abs_delta_theta + delta_theta_prime = delta_theta / track.ds * delta_s + + theta_prime = theta + delta_theta_prime / 2.0 + + # delta_xy = chord * np.array([np.cos(theta_prime), np.sin(theta_prime)]) + + initial_point = track.xy_coords[0, :] + e_y_direction = np.array([initial_point[0], initial_point[1] - e_y]) + e_y_rotated = rotate_around_z(e_y_direction, theta) + xy_from_origin = s1_xy + e_y_rotated + + s_direction = np.array([initial_point[0] + chord, initial_point[1]]) + s_rotated = rotate_around_z(s_direction, theta_prime) + + e_y_prime = xy_from_origin + s_rotated + + """ + theta_prime = theta + delta_theta_prime + + delta_xy = chord * np.array([np.cos(theta_prime), np.sin(theta_prime)]) + + initial_point = track.xy_coords[0, :] + e_y_direction = np.array([initial_point[0], initial_point[1] - e_y]) + e_y_rotated = rotate_around_z(e_y_direction, theta_prime) + + xy_from_origin = s1_xy + delta_xy + e_y_prime = e_y_rotated + xy_from_origin + """ + + x = e_y_prime[0] + y = e_y_prime[1] + + # psi = e_psi + theta + delta_theta_prime + psi = e_psi + theta_prime + + # plt.plot([x, track.xy_coords[xy1_index, 0]], [y, track.xy_coords[xy1_index, 1]]) + # plt.plot([x, track.xy_coords[xy1_index + 1, 0]], [y, track.xy_coords[xy1_index + 1, 1]]) + + return np.array([x, y, v_x, v_y, psi, psi_dot]) + + +def xy_to_s(xy_coord, track): + x, y, v_x, v_y, psi, psi_dot = xy_coord + + distances_to_track = np.array([LA.norm(track.xy_coords[i, :] - xy_coord[: 2]) + for i in range(len(track.s_coords))]) + two_closest_indices = np.argsort(distances_to_track)[: 2] + + if 0 in two_closest_indices: + if x >= 0.: + s1_index = 0 + else: + s1_index = track.xy_coords.shape[0] - 2 + else: + s1_index = np.min(two_closest_indices) + + s1_s = track.s_coords[s1_index] + theta = track.thetas[s1_index] + delta_theta = track.thetas[s1_index + 1] - theta + + dist_to_s1 = distances_to_track[s1_index] + dist_to_s2 = distances_to_track[s1_index + 1] + + if abs(delta_theta) > 1e-5: + if abs(track.get_curvature(track.s_coords[s1_index + 1]) - track.get_curvature(track.s_coords[s1_index])) > 1e-5: + if abs(track.get_curvature(track.s_coords[s1_index + 1])) > 1e-5: + curvature = track.get_curvature(track.s_coords[s1_index + 1]) + else: + curvature = track.get_curvature(track.s_coords[s1_index]) + else: + curvature = track.get_curvature(track.s_coords[s1_index]) + + radius = 1. / curvature + radius_abs = abs(radius) + + # Find the center of the circle segment + initial_point = track.xy_coords[0, :] + e_y_direction = np.array([initial_point[0], initial_point[1] + radius]) + e_y_rotated = rotate_around_z(e_y_direction, theta) + center_of_segment = e_y_rotated + track.xy_coords[s1_index, :] + + # Applying the law of cosines + dist_to_center_segment = LA.norm(center_of_segment - xy_coord[: 2]) + + nominator = radius_abs ** 2 + dist_to_center_segment ** 2 - dist_to_s1 ** 2 + denominator = 2 * radius_abs * dist_to_center_segment + if abs(nominator - denominator) < 1e-5: + abs_phi = 0.0 + else: + abs_phi = np.arccos(nominator / denominator) + + # print("curvature: ", curvature) + + if curvature < 0.0: + phi = - abs_phi + else: + phi = abs_phi + + delta_s = radius_abs * abs_phi + if curvature < 0.0: + e_y = radius_abs - dist_to_center_segment + else: + e_y = - (radius_abs - dist_to_center_segment) + + e_psi = psi - (theta + phi) + + theta = (theta + phi) + else: + if dist_to_s1 < 1e-5: + delta_s = 0.0 + abs_e_y = 0.0 + phi = 0.0 + elif dist_to_s1 + dist_to_s2 <= track.ds + 1e-5: # added tolerance + delta_s = dist_to_s1 + abs_e_y = 0.0 + phi = 0.0 + else: + # print("Here") + abs_phi = np.arccos((dist_to_s1 ** 2 + track.ds ** 2 - dist_to_s2 ** 2) / + (2 * dist_to_s1 * track.ds)) + if track.get_curvature(track.s_coords[s1_index + 1]) < 0.0: + phi = abs_phi + else: + phi = abs_phi + + delta_s = dist_to_s1 * np.cos(phi) + abs_e_y = dist_to_s1 * np.sin(phi) + + curvature = track.get_curvature(track.s_coords[s1_index + 1]) # track.curvature[s1_index + 1] + theta = track.thetas[s1_index] + + e_psi = psi - theta + + initial_point = track.xy_coords[0, :] + # print("initial_point: ", initial_point) + e_y_direction = np.array([initial_point[0], + initial_point[1] - track.width / 2]) # to inner bound + # print("Ey direction: ", e_y_direction) + e_y_rotated = rotate_around_z(e_y_direction, theta) + # print("Ey rotated: ", e_y_rotated) + # xy_position = track.xy_coords[s1_index, :] + delta_s * \ + # np.array([np.cos(theta), np.sin(theta)]) + xy_position = track.xy_coords[s1_index, :] + delta_s * np.array([np.cos(theta), np.sin(theta)]) # np.array([delta_s, 0.0]) + center_of_segment = e_y_rotated + xy_position + # print("center of segment: ", center_of_segment) + distance_to_inner_bound = LA.norm(center_of_segment - np.array([x, y])) # track.xy_coords[: 2]) + # print("disance to inner bound: ", distance_to_inner_bound) + if distance_to_inner_bound <= track.width / 2: + e_y = abs_e_y + else: + e_y = - abs_e_y + + s = s1_s + delta_s + + # print("S: ", s) + + + return np.array([s, - e_y, e_psi, psi_dot, v_x, v_y]) + + +def rotate_around_z(xy_coords, angle): + rotation_matrix = np.array([[np.cos(angle), - np.sin(angle)], + [np.sin(angle), np.cos(angle)]]) + + return np.dot(rotation_matrix, xy_coords) + + +if __name__ == "__main__": + import pdb + import matplotlib.pyplot as plt + + # track = Track(0.1, "test", 1.0) + track = Track(0.1, "oval", 1.0) + track = Track(0.1, "l_shape", 1.0) + + pdb.set_trace() + + fig = plt.figure("Race") + ax = fig.add_subplot(1, 1, 1) + # ax = fig.add_subplot(2, 1, 1) + # ax_2 = fig.add_subplot(2, 1, 2) + + # fig_2 = plt.figure("Angles") + # axis = fig_2.add_subplot(1, 1, 1) + + """ + test = np.array([-0.02592649572766198,-0.002358129563365882, + 0.5311734600406155,0.00415122594607335,-6.262578301959858, + -0.03384403792516429]) + test_2 = np.array([0.23317530843876547,-0.0046964218147232855,0.5307739418952622, + -0.0009789572258890383,-6.26154372355634,-0.021930483057318338]) + test_3 = np.array([0.23317530843876547,-0.0046964218147232855,0.5307739418952622, + -0.0009789572258890383,2 * np.pi -6.26154372355634,-0.021930483057318338]) + + print(xy_to_s(test, track)) + print(xy_to_s(test_2, track)) + print(xy_to_s(test_3, track)) + pdb.set_trace() + + ax.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], "k--") + ax.plot(track.xy_inner[:, 0], track.xy_inner[:, 1], "k-") + ax.plot(track.xy_outer[:, 0], track.xy_outer[:, 1], "k-") + + test = np.array([ 3.0114, - 0.4, 0., 0., 0., 0.]) + """ + + num_points = 850 + # delta_s = 0.02 # track.total_length / num_points + delta_s = track.total_length / num_points + + # run_s = 3.0592 + # run_s = 23.8044 + # run_s = 4.68 + run_s = 0.0 + + # plt.plot(track.xy_coords[89:99, 0], track.xy_coords[89:99, 1], "ko") + ss_state = np.zeros((num_points, 6)) + curvature_log = np.zeros((num_points)) + theta_log = np.zeros((num_points)) + center_of_segment_log = np.zeros((num_points, 2)) + + e_y = - 0.5 + + r = 2 / np.pi * 2 + alpha = np.arange(- np.pi, np.pi, 0.005) + ax.plot(r * np.cos(alpha), r * np.sin(alpha) - r, "k-") + ax.plot(r * np.cos(alpha) + 1.5, r * np.sin(alpha) - r, "k-") + ax.plot(r * np.cos(alpha) + 1.5, r * np.sin(alpha) - r - 1.5, "k-") + ax.plot(r * np.cos(alpha) - 1.5, r * np.sin(alpha) - r - 1.5, "k-") + ax.plot(r * np.cos(alpha) - 1.5, r * np.sin(alpha) - r, "k-") + + ax.plot(1.5, - r, "k*") + ax.plot(1.5, - r - 1.5, "k*") + ax.plot(- 1.5, - r - 1.5, "k*") + ax.plot(- 1.5, - r, "k*") + + ax.plot([1.5, 1.5], [- 4 * r, r], "k--") + ax.plot([1.5 + r, 1.5 + r], [- 4 * r, r], "k--") + ax.plot([- 1.5, - 1.5], [- 4 * r, r], "k--") + ax.plot([- 1.5 - r, - 1.5 - r], [- 4 * r, r], "k--") + ax.plot([- 3 * r, 3 * r], [-r, -r], "k--") + ax.plot([- 3 * r, 3 * r], [-r - 1.5, -r - 1.5], "k--") + ax.plot([- 3 * r, 3 * r], [- 2 * r - 1.5, - 2 * r - 1.5], "k--") + ax.plot([- 3 * r, 3 * r], [0.0, 0.0], "k--") + + ax.plot((r - e_y) * np.cos(alpha) + 1.5, (r - e_y) * np.sin(alpha) - r, "r-") + ax.plot((r - e_y) * np.cos(alpha) + 1.5, (r - e_y) * np.sin(alpha) - r - 1.5, "r-") + ax.plot((r - e_y) * np.cos(alpha) - 1.5, (r - e_y) * np.sin(alpha) - r - 1.5, "r-") + ax.plot((r - e_y) * np.cos(alpha) - 1.5, (r - e_y) * np.sin(alpha) - r, "r-") + + for i in range(num_points): + # for i in range(5): + print("i: ", i, run_s) + state = np.array([run_s, e_y, 0.0, 0.0, 0.0, 0.0]) + # state = np.array([run_s, - 0.02, 0.0, 0.0, 0.0, 0.0]) + print("S: ", state) + xy_state = s_to_xy(state, track) + print("XY: ", xy_state) + # print("S: ", ss_state[i, :]) + xxyy_state = s_to_xy(ss_state[i, :], track) + # print("XY: ", xxyy_state) + ax.plot(xy_state[0], xy_state[1], "ro") + ax.plot(xxyy_state[0], xxyy_state[1], "bo") + # ax_2.plot(state[0], 0.0, "ro") + + if run_s == 4.6: + ax.plot([xy_state[0], 1.5], [xy_state[1], - r], "k--") + + run_s += delta_s + + ax.plot(center_of_segment_log[:, 0], center_of_segment_log[:, 1], "b*") + + ax.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], "ko") + + """ + + ax.plot(r * np.cos(alpha) + 1, r * np.sin(alpha) - r, "g-") + ax.plot(r * np.cos(alpha) - 1, r * np.sin(alpha) - r, "g-") + ax.plot(track.xy_coords[:, 0], track.xy_coords[:, 1], "ko") + ax.plot(track.xy_coords[12, 0], track.xy_coords[12, 1], "bo") + ax.plot(track.xy_coords[13, 0], track.xy_coords[13, 1], "co") + + print("Theta 11: ", track.thetas[11]) + print("Theta 12: ", track.thetas[12]) + print("Theta 13: ", track.thetas[13]) + print("Theta 14: ", track.thetas[14]) + + print("xy 11: ", track.xy_coords[11, :]) + print("xy 12: ", track.xy_coords[12, :]) + print("xy 13: ", track.xy_coords[13, :]) + print("xy 14: ", track.xy_coords[14, :]) + """ + + #for i in range(len(track.xy_coords[:, 1])): + # print("s: ", track.s_coords[i], " theta: ", track.thetas[i]) + + # axis.plot(ss_state[:, 0], curvature_log, "b-") + # axis.plot(ss_state[:, 0], theta_log, "r-") + + # ax_2.plot(ss_state[:, 0], ss_state[:, 1], "bo") + + s = track.s_coords + + + # s = 9.05 + + # state = np.array([s, - 0.4, 0.0, 0.0, 0.0, 0.0]) + # xy_state = s_to_xy(state, track) + # print(xy_state) + + # ax.plot(xy_state[0], xy_state[1], "bo") + + # s_index = np.int(np.floor(s / track.ds)) + # ax.plot(track.xy_coords[s_index, 0], track.xy_coords[s_index, 1], "bo") + # ax.plot(track.xy_coords[s_index + 1, 0], track.xy_coords[s_index + 1, 1], "bo") + + plt.axis("equal") + plt.grid() + plt.show() + + + # pdb.set_trace() diff --git a/workspace/src/barc/src/view_car_trajectory.py b/workspace/src/barc/src/view_car_trajectory.py new file mode 100755 index 00000000..d7cd9ff0 --- /dev/null +++ b/workspace/src/barc/src/view_car_trajectory.py @@ -0,0 +1,305 @@ +#!/usr/bin/env python + +# --------------------------------------------------------------------------- +# Licensing Information: You are free to use or extend these projects for +# education or reserach purposes provided that (1) you retain this notice +# and (2) you provide clear attribution to UC Berkeley, including a link +# to http://barc-project.com +# +# Author: J. Noonan +# Email: jpnoonan@berkeley.edu +# +# This code provides a way to see the car's trajectory, orientation, and velocity profile in +# real time with referenced to the track defined a priori. +# +# --------------------------------------------------------------------------- + +import rospy +from Localization_helpers import Track +from barc.msg import ECU, pos_info, Vel_est, mpc_solution +from sensor_msgs.msg import Imu +from marvelmind_nav.msg import hedge_imu_fusion +from numpy import eye, array, zeros, diag, unwrap, tan, cos, sin, vstack, linalg, append, ones, polyval, delete, size, empty, linspace +from numpy import ones, polyval, delete, size +from tf import transformations +import math +import matplotlib.pyplot as plt +import numpy as np +# import pylab + +global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev, real_x_vals, real_y_vals +global pos_info_x_vals, pos_info_y_vals, pos_info_s +global v_vals, t_vals, psi_curr, psi_raw +global z_x, z_y, SS_x, SS_y, z_vx, SS_vx, z_s, SS_s, z_fore_x, z_fore_y, z_iden_x, z_iden_y # x and y for mpcSol prediction + +gps_x_vals = [0.0] +gps_y_vals = [0.0] +gps_x_prev = 0.0 +gps_y_prev = 0.0 + +pos_info_x_vals = [0.0] +pos_info_y_vals = [0.0] +pos_info_s = 0 + +real_x_vals = [0] +real_y_vals = [0] + +v_vals = [] +t_vals = [] +psi_curr = 0.0 +psi_raw = 0.0 + +z_x = ones(11) +z_y = ones(11) +z_fore_x = ones(11) +z_fore_y = ones(11) + +SS_x = zeros(20) +SS_y = zeros(20) + +z_vx = zeros(11) +z_s = zeros(11) +SS_vx = zeros(20) +SS_s = zeros(20) + +z_iden_x = zeros(30) +z_iden_y = zeros(30) + +def gps_callback(data): + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev, z_vx, SS_vx + + dist = (gps_x_prev - data.x_m)**2 + (gps_y_prev - data.y_m)**2 + if dist < 1: + gps_x_vals.append(data.x_m) + gps_y_vals.append(data.y_m) + gps_x_prev = data.x_m + gps_y_prev = data.y_m + else: + gps_x_vals.append(gps_x_prev) + gps_y_vals.append(gps_y_prev) + gps_x_prev = data.x_m + gps_y_prev = data.y_m + + +def pos_info_callback(data): + global pos_info_x_vals, pos_info_y_vals, pos_info_s + global v_vals, t_vals, psi_curr, psi_raw + + pos_info_x_vals.append(data.x) + pos_info_y_vals.append(data.y) + pos_info_s = data.s + + v_vals.append(data.v) + t_vals.append(rospy.get_rostime().to_sec()) + psi_curr = data.psi + psi_raw = 0 + # psi_raw = data.psi_raw + +def real_val_callback(data): + global real_x_vals, real_y_vals + real_x_vals.append(data.x) + real_y_vals.append(data.y) + + + +def mpcSol_callback(data): + global z_x, z_y, SS_x, SS_y, z_vx, SS_vx, z_s, SS_s, z_fore_x, z_fore_y, z_iden_x, z_iden_y + z_x = data.z_x + z_y = data.z_y + SS_x = data.SS_x + SS_y = data.SS_y + z_vx = data.z_vx + SS_vx = data.SS_vx + z_s = data.z_s + SS_s = data.SS_s + z_fore_x = data.z_fore_x + z_fore_y = data.z_fore_y + z_iden_x = data.z_iden_x + z_iden_y = data.z_iden_y + +# def show(): +# plt.show() + +def view_trajectory(): + + global gps_x_vals, gps_y_vals, gps_x_prev, gps_y_prev, real_x_vals, real_y_vals + global pos_info_x_vals, pos_info_y_vals, pos_info_s + global v_vals, t_vals, psi_curr, psi_raw + global z_x, z_y, SS_x, SS_y, z_vx, SSvx, z_s, SS_s, z_fore_x, z_fore_y, z_iden_x, z_iden_y + + rospy.init_node("car_view_trajectory_node", anonymous=True) + # rospy.on_shutdown(show) + + rospy.Subscriber("hedge_imu_fusion", hedge_imu_fusion, gps_callback, queue_size=1) + rospy.Subscriber("pos_info", pos_info, pos_info_callback, queue_size=1) + rospy.Subscriber("real_val", pos_info, real_val_callback, queue_size=1) + rospy.Subscriber("mpc_solution", mpc_solution, mpcSol_callback, queue_size=1) + + # FLAGS FOR PLOTTING + PRE_FLAG = True + SS_FLAG = True + FORE_FLAG= False + IDEN_FLAG= True + GPS_FLAG = True + YAW_FLAG = True + ETS_TRUE_FLAG = False + + track = Track(rospy.get_param("ds"),rospy.get_param("ey")) + if rospy.get_param("feature_flag"): + track.createFeatureTrack() + else: + track.createRaceTrack() + + fig = plt.figure(figsize=(10,7)) + plt.ion() + ax1 = fig.add_subplot(1, 1, 1) + # ax2 = fig.add_subplot(2, 1, 2) + ax1.plot(track.nodes[0,:],track.nodes[1,:],"k--",alpha=0.4) + ax1.plot(track.nodes_bound1[0,:],track.nodes_bound1[1,:],"r-") + ax1.plot(track.nodes_bound2[0,:],track.nodes_bound2[1,:],"r-") + ax1.grid('on') + ax1.axis('equal') + # ax1.set_ylim([-5.5,1]) + + loop_rate = 50.0 + rate = rospy.Rate(loop_rate) + + car_dx = 2*rospy.get_param("L_a") + car_dy = 0.125 + + car_xs_origin = [car_dx, car_dx, -car_dx, -car_dx, car_dx] + car_ys_origin = [car_dy, -car_dy, -car_dy, car_dy, car_dy] + car_plot, = ax1.plot(car_xs_origin,car_ys_origin,"k-") + + if YAW_FLAG: + yaw_raw_plot, = ax1.plot(car_xs_origin,car_ys_origin,"r-") + + car_center_plot, = ax1.plot(0,0,"ko",alpha=0.4) + + if PRE_FLAG: + pre_plot, = ax1.plot([0 for i in range(11)],[0 for i in range(11)],"b-*") + + if SS_FLAG: + SS_plot, = ax1.plot([0 for i in range(20)],[0 for i in range(20)],"ro",alpha=0.2) + + if FORE_FLAG: + fore_plot, = ax1.plot([0 for i in range(11)],[0 for i in range(11)],"k.") + + if IDEN_FLAG: + iden_plot, = ax1.plot([0 for i in range(30)],[0 for i in range(30)],"go",alpha=0.3) + + if GPS_FLAG: + num = min(len(gps_x_vals),len(gps_y_vals)) + GPS_plot, = ax1.plot(gps_x_vals, gps_y_vals, 'b-', label="GPS data path") + + num = min(len(pos_info_x_vals),len(pos_info_y_vals)) + pos_plot, = ax1.plot(pos_info_x_vals[:num], pos_info_y_vals[:num], 'g-', label="pos data path") + + # ax2 PLOT CHOICE 1: V_X HISTORY PLOT + # t_vals_zeroed = [t - t_vals[0] for t in t_vals] + # num = min(len(t_vals_zeroed),len(v_vals)) + # v_plot, = ax2.plot(t_vals_zeroed[:num], v_vals[:num], 'm-') + # ax2.set_ylim([0,2.5]) + + # ax2 PLOT CHOICE 2: V_X SS AND PREDICTION PLOT + # v_plot, = ax2.plot(z_s,z_vx,"b-") + # SS_v_plot, = ax2.plot(SS_s,SS_vx,"k*") + # ax2.set_xlim([min(z_s), max(SS_s)]) + # ax2.set_ylim([min(min(z_vx),min(SS_vx)), max(max(z_vx),max(SS_vx))]) + + plt.show() + car_frame = np.vstack((np.array(car_xs_origin), np.array(car_ys_origin))) + + counter_buffer = 600 + counter = 1 + while not rospy.is_shutdown(): + # if counter < counter_buffer: + if (pos_info_s>0 and pos_info_s<0.5): + # lap switching trajectory cleaning + gps_x_vals = [0,gps_x_vals[-1]] + gps_y_vals = [0,gps_y_vals[-1]] + pos_info_x_vals = [0,pos_info_x_vals[-1]] + pos_info_y_vals = [0,pos_info_y_vals[-1]] + real_x_vals = [0,real_x_vals[-1]] + real_y_vals = [0,real_y_vals[-1]] + + # gps_x_vals = [0,0] + # gps_y_vals = [0,0] + # pos_info_x_vals = [0,0] + # pos_info_y_vals = [0,0] + + # print("GPS off: ",np.sqrt((gps_x_vals[-1]-pos_info_x_vals[-1])**2+(gps_y_vals[-1]-pos_info_y_vals[-1])**2)) + + if ETS_TRUE_FLAG: + x = real_x_vals[len(real_x_vals)-1] + y = real_y_vals[len(real_y_vals)-1] + else: + x = pos_info_x_vals[len(pos_info_x_vals)-1] + y = pos_info_y_vals[len(pos_info_y_vals)-1] + # ax1.plot(x, y, 'gs', label="Car current pos") + + R = np.matrix([[np.cos(psi_curr), -np.sin(psi_curr)], [np.sin(psi_curr), np.cos(psi_curr)]]) + + rotated_car_frame = R * car_frame + car_xs = np.array(rotated_car_frame[0,:])[0] + x + car_ys = np.array(rotated_car_frame[1,:])[0] + y + + car_plot.set_data([car_xs[i] for i in range(5)], [car_ys[i] for i in range(5)]) + car_center_plot.set_data(x,y) + + if YAW_FLAG: + R_yaw_raw = np.matrix([[np.cos(psi_raw), -np.sin(psi_raw)], [np.sin(psi_raw), np.cos(psi_raw)]]) + yaw_raw_rotated_car_frame = R_yaw_raw * car_frame + car_xs = np.array(rotated_car_frame[0,:])[0] + x + car_ys = np.array(rotated_car_frame[1,:])[0] + y + yaw_raw_plot.set_data([car_xs[i] for i in range(5)], [car_ys[i] for i in range(5)]) + + if GPS_FLAG: + num = min(len(gps_x_vals),len(gps_y_vals)) + GPS_plot.set_data(gps_x_vals[:num], gps_y_vals[:num]) + + if ETS_TRUE_FLAG: + num = min(len(real_x_vals),len(real_y_vals)) + pos_plot.set_data(real_x_vals[:num], real_y_vals[:num]) + else: + num = min(len(pos_info_x_vals),len(pos_info_y_vals)) + pos_plot.set_data(pos_info_x_vals[:num], pos_info_y_vals[:num]) + + # ax2 PLOT CHOICE 1: V_X HISTORY PLOT + # v_plot.set_data(z_vx) + + # ax2 PLOT CHOICE 2: V_X SS AND PREDICTION PLOT + # v_plot.set_data(z_s,z_vx) + # SS_v_plot.set_data(SS_s,SS_vx) + # if len(z_s) == 0: + # pass + # else: + # ax2.set_xlim([min(z_s), max(SS_s)]) + # ax2.set_ylim([min(min(z_vx),min(SS_vx)), max(max(z_vx),max(SS_vx))]) + if PRE_FLAG: + pre_plot.set_data(z_x,z_y) + + if FORE_FLAG: + fore_plot.set_data(z_fore_x,z_fore_y) + + if IDEN_FLAG: + iden_plot.set_data(z_iden_x,z_iden_y) + + if SS_FLAG: + SS_plot.set_data(SS_x,SS_y) + + fig.canvas.draw() + counter+=1 + # print(counter) + rate.sleep() + + + # plt.ioff() + # plt.show() + + +if __name__ == '__main__': + try: + view_trajectory() + except rospy.ROSInterruptException: + pass diff --git a/workspace/src/barc_gui/license.txt b/workspace/src/barc_gui/license.txt deleted file mode 100644 index a9651ed2..00000000 --- a/workspace/src/barc_gui/license.txt +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2016 Kiet Lam, Jon Gonzales - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/workspace/src/barc_gui/package.xml b/workspace/src/barc_gui/package.xml deleted file mode 100644 index e9f5b982..00000000 --- a/workspace/src/barc_gui/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - barc_gui - 0.0.0 - The BARC GUI package - mpc - MIT - catkin - rospy - rqt_gui - rqt_gui_py - rospy - rqt_gui - rqt_gui_py - - - - diff --git a/workspace/src/barc_gui/plugin.xml b/workspace/src/barc_gui/plugin.xml deleted file mode 100644 index 1910e9f7..00000000 --- a/workspace/src/barc_gui/plugin.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - system-help - UI - - - diff --git a/workspace/src/barc_gui/scripts/barc_gui b/workspace/src/barc_gui/scripts/barc_gui deleted file mode 100755 index c7b2b6fa..00000000 --- a/workspace/src/barc_gui/scripts/barc_gui +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from barc_gui.gui import MyGUI - -plugin = 'barc_gui' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin))#, plugin_argument_provider=Dot.add_arguments)) diff --git a/workspace/src/barc_gui/scripts/test.py b/workspace/src/barc_gui/scripts/test.py deleted file mode 100755 index 703ac06e..00000000 --- a/workspace/src/barc_gui/scripts/test.py +++ /dev/null @@ -1,23 +0,0 @@ -#!/usr/bin/env python - -import rospy - -import random - -from std_msgs.msg import Float64 -from geometry_msgs.msg import Vector3 - -if __name__ == '__main__': - # Test rosbag - - rospy.init_node('barc_gui_test', anonymous=True) - - # pub = rospy.Publisher('barc_rosbag', Float64, queue_size=10) - pub = rospy.Publisher('enc_data', Vector3, queue_size=10) - # pub2 = rospy.Publisher('imu_data', Float64, queue_size=10) - rate = rospy.Rate(100) - - while not rospy.is_shutdown(): - pub.publish(Vector3(random.random(), random.random(), random.random())) - # pub2.publish(random.random()) - rate.sleep() diff --git a/workspace/src/barc_gui/setup.py b/workspace/src/barc_gui/setup.py deleted file mode 100755 index 1c20ba8d..00000000 --- a/workspace/src/barc_gui/setup.py +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - ## don't do this unless you want a globally visible script - # scripts=['bin/myscript'], - packages=['barc_gui'], - package_dir={'': 'src'}, - scripts=['scripts/barc_gui'] -) - -setup(**d) diff --git a/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui b/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui deleted file mode 100755 index a35a140b..00000000 --- a/workspace/src/barc_gui/src/barc_gui/BARC_experiment.ui +++ /dev/null @@ -1,92 +0,0 @@ - - - MyGUI - - - - 0 - 0 - 436 - 351 - - - - - 0 - 0 - - - - Qt::ClickFocus - - - BARC Experiment - - - - - - Topics (check box for topic you want to record) - - - - - - - - - - - 0 - 0 - - - - Experiment name - - - Qt::AlignBottom|Qt::AlignLeading|Qt::AlignLeft - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignTop - - - - - - - - - - 0 - 0 - - - - Start Recording - - - - - - - - - - diff --git a/workspace/src/barc_gui/src/barc_gui/gui.py b/workspace/src/barc_gui/src/barc_gui/gui.py deleted file mode 100644 index e2d830d4..00000000 --- a/workspace/src/barc_gui/src/barc_gui/gui.py +++ /dev/null @@ -1,341 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import roslib -roslib.load_manifest('barc_gui') - -import subprocess, os, signal, psutil -import json - -import rospy -import rosbag - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - -import std_msgs.msg - -from PyQt4 import QtCore -from PyQt4.QtCore import * -from PyQt4.QtGui import * - -import time - -from data_service.srv import * -from data_service.msg import * - -from threading import * - - -rosbag_dir = os.path.expanduser("~") + '/rosbag' -video_dir = os.path.expanduser("~") + '/video' - - -class UploadThread(Thread): - - def __init__(self, plugin_ob, experiment): - Thread.__init__(self) - self.plugin_ob = plugin_ob - self.experiment = experiment - - - def run(self): - self.plugin_ob.record_data_thread(self.experiment) - - -class MyGUI(Plugin): - - def __init__(self, context): - super(MyGUI, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('MyGUI') - - # Process standalone plugin command-line arguments - from argparse import ArgumentParser - parser = ArgumentParser() - # Add argument(s) to the parser. - parser.add_argument("-q", "--quiet", action="store_true", - dest="quiet", - help="Put plugin in silent mode") - args, unknowns = parser.parse_known_args(context.argv()) - if not args.quiet: - print 'arguments: ', args - print 'unknowns: ', unknowns - - # Create QWidget - self._widget = QWidget() - # Get path to UI file which is a sibling of this file - # in this example the .ui and .py file are in the same folder - ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'BARC_experiment.ui') - # Extend the widget with all attributes and children from UI file - loadUi(ui_file, self._widget) - # Give QObjects reasonable names - self._widget.setObjectName('MyGUIUi') - # Show _widget.windowTitle on left-top of each plugin (when - # it's set in _widget). This is useful when you open multiple - # plugins at once. Also if you open multiple instances of your - # plugin at once, these lines add number to make it easy to - # tell from pane to pane. - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - # Add widget to the user interface - context.add_widget(self._widget) - - self._widget.pushbutton_record.clicked[bool].connect(self._handle_record) - self.record_started = False - - self.setup_topics_list() - self.p = None - - self.initialize() - - - def initialize(self): - if not os.path.isdir(video_dir): - os.makedirs(video_dir) - - if not os.path.isdir(rosbag_dir): - os.makedirs(rosbag_dir) - - - def get_experiment_name(self): - return self._widget.experiment_name.text().replace(' ', '_') - - - def setup_topics_list(self): - topics = ['imu', 'encoder', 'ecu', 'ultrasound', 'video'] - - for t in topics: - item = QListWidgetItem(t) - item.setCheckState(True) - - self._widget.listview_topics.addItem(item) - - - def start_record_video(self): - - print 'Recording video...' - - command = 'rosrun image_view video_recorder image:=/image_raw _max_depth_range:=0' - self.p_video = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=video_dir) - - - def stop_record_video(self, experiment): - print 'Stopping record video' - - if not self.p_video: - print 'Not stopping record video because p_video NONE' - return - - command = 'rosnode list' - out = subprocess.check_output(command, shell=True) - for li in out.split('\n'): - if 'video_recorder' in li: - command_kill = 'rosnode kill %s' % li - ps = subprocess.Popen(command_kill, stdin=subprocess.PIPE, shell=True) - ps.communicate() - - command = 'mv output.avi %s.avi' % experiment - subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=video_dir) - - self.p_video = None - - rospy.wait_for_service('register_video') - self.register_video = rospy.ServiceProxy('register_video', RegisterVideo) - - try: - self.register_video(experiment, video_dir + '/%s.avi' % experiment) - except Exception as e: - pass - - - def _handle_record(self, checked): - if not self.get_experiment_name(): - msg = QMessageBox() - msg.setText('Need an experiment name!') - msg.exec_() - return - - record_topics = [] - for index in range(self._widget.listview_topics.count()): - item = self._widget.listview_topics.item(index) - if item.checkState(): - record_topics.append(item.text()) - - if self.record_started: - self.stop_record_data(self.current_experiment) - self.stop_record_video(self.current_experiment) - - self.record_started = False - - self._widget.label_experiment.setText('Experiment name') - self._widget.pushbutton_record.setText('Start Recording') - - else: - self.record_started = True - self.current_experiment = self.get_experiment_name() - date = time.strftime("%Y.%m.%d") - self.current_experiment += '_' + date + '_' + time.strftime('%H.%M') - - self._widget.pushbutton_record.setText('Stop Recording') - self._widget.label_experiment.setText('Recording...') - self.time = time.time() - self.start_record_data(record_topics, self.current_experiment) - - if 'video' in record_topics: - self.start_record_video() - - - def start_record_data(self, topics, experiment): - command = 'rosbag record ' - - for topic in topics: - command += topic + ' ' - - command += ' -O %s' % experiment - - self.p = subprocess.Popen(command, stdin=subprocess.PIPE, shell=True, cwd=rosbag_dir) - - - def stop_record_data(self, experiment): - if not self.p: - return - - uploader = UploadThread(self, experiment) - uploader.start() - - - def record_data_thread(self, experiment): - command = 'rosnode list' - out = subprocess.check_output(command, shell=True) - - for li in out.split('\n'): - if 'record' in li: - command_kill = 'rosnode kill %s' % li - ps = subprocess.Popen(command_kill, stdin=subprocess.PIPE, shell=True) - ps.communicate() - - rospy.wait_for_service('send_data') - self.send_data = rospy.ServiceProxy('send_data', DataForward, persistent=True) - - self._widget.label_experiment.setText('Uploading data...') - self.upload_data(experiment) - self._widget.label_experiment.setText('Experiment name') - self._widget.pushbutton_record.setText('Start Recording') - - - def upload_data(self, experiment): - rosbag_file = os.path.abspath(rosbag_dir + '/' + experiment + '.bag') - - for i in range(100): - if os.path.isfile(rosbag_file): - break - #TODO: Can we get rid of this sleep? - time.sleep(1.0) - - if not os.path.isfile(rosbag_file): - return - - convert_to_time = 1/(10.0**(9)) - - bag = rosbag.Bag(rosbag_file) - - chunk_size = 50 - chunk_dict = dict() - chunk_msg = dict() - chunk_ts = dict() - - for topic, msg, t in bag.read_messages(): - ts = t.nsecs * convert_to_time - ts += (t.secs - self.time) - - if topic not in chunk_dict: - chunk_dict[topic] = 1 - - if topic not in chunk_msg: - chunk_msg[topic] = [] - - if topic not in chunk_ts: - chunk_ts[topic] = [] - - chunk_dict[topic] += 1 - chunk_msg[topic].append(msg) - chunk_ts[topic].append(ts) - - for k, v in chunk_dict.items(): - if v > chunk_size: - self.upload_message(k, chunk_msg[k], chunk_ts[k], experiment) - del chunk_msg[k] - del chunk_dict[k] - del chunk_ts[k] - - for k, v in chunk_dict.items(): - self.upload_message(k, chunk_msg[k], chunk_ts[k], experiment) - - os.remove(rosbag_file) - - - def upload_message(self, topic, msgs, tss, experiment): - vars_list = ['roll', 'pitch', 'yaw', - 'roll_rate', 'pitch_rate', 'yaw_rate', - 'acc_x', 'acc_y', 'acc_z', - 'encoder_FL', 'encoder_FR','encoder_BL','encoder_BR', - 'motor', 'servo', - 'ultrasound_front','ultrasound_back','ultrasound_left','ultrasound_right'] - - signal_dict = dict() - - for v in vars_list: - signal_dict[v] = [] - - for msg in msgs: - # Inertia measurement unit - if topic == 'imu': - (roll, pitch, yaw, acc_x, acc_y, acc_z, roll_rate, pitch_rate, yaw_rate) = msg.value - - # Encoder - if topic == 'encoder': - encoder_FL = msg.FL - encoder_FR = msg.FR - encoder_BL = msg.BL - encoder_BR = msg.BR - - if topic == 'ultrasound': - ultrasound_front = msg.front - ultrasound_back = msg.back - ultrasound_left = msg.left - ultrasound_right = msg.right - - # Electronic control unit - if topic == 'ecu': - motor = msg.motor - servo = msg.servo - - # Python introspection from list 'vars_list' - for v in vars_list: - if v in dict(globals(), **locals()): - signal_dict[v].append(dict(globals(), **locals())[v]) - - for v in vars_list: - if signal_dict[v]: - time_signal = TimeSignal() - time_signal.name = v - time_signal.timestamps = tss - time_signal.signal = json.dumps(signal_dict[v]) - try: - self.send_data(time_signal, None, experiment) - except Exception as e: - pass diff --git a/workspace/src/barc_gui/src/barc_gui/test.py b/workspace/src/barc_gui/src/barc_gui/test.py deleted file mode 100644 index 32310187..00000000 --- a/workspace/src/barc_gui/src/barc_gui/test.py +++ /dev/null @@ -1,51 +0,0 @@ -import sys -import urllib2 - -from PyQt4 import QtCore, QtGui - - -class DownloadThread(QtCore.QThread): - - data_downloaded = QtCore.pyqtSignal(object) - - def __init__(self, url): - QtCore.QThread.__init__(self) - self.url = url - - def run(self): - info = urllib2.urlopen(self.url).info() - self.data_downloaded.emit('%s\n%s' % (self.url, info)) - - -class MainWindow(QtGui.QWidget): - def __init__(self): - super(MainWindow, self).__init__() - self.list_widget = QtGui.QListWidget() - self.button = QtGui.QPushButton("Start") - self.button.clicked.connect(self.start_download) - layout = QtGui.QVBoxLayout() - layout.addWidget(self.button) - layout.addWidget(self.list_widget) - self.setLayout(layout) - - def start_download(self): - urls = ['http://google.com', 'http://twitter.com', 'http://yandex.ru', - 'http://stackoverflow.com/', 'http://www.youtube.com/'] - self.threads = [] - for url in urls: - downloader = DownloadThread(url) - downloader.data_downloaded.connect(self.on_data_ready) - self.threads.append(downloader) - downloader.start() - - def on_data_ready(self, data): - print data - self.list_widget.addItem(unicode(data)) - - -if __name__ == "__main__": - app = QtGui.QApplication(sys.argv) - window = MainWindow() - window.resize(640, 480) - window.show() - sys.exit(app.exec_()) diff --git a/workspace/src/data_service/CMakeLists.txt b/workspace/src/data_service/CMakeLists.txt deleted file mode 100644 index 0dbf885e..00000000 --- a/workspace/src/data_service/CMakeLists.txt +++ /dev/null @@ -1,194 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(data_service) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - genmsg - message_generation -) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder - add_message_files( - FILES - CustomSignal.msg - TimeData.msg - TimeSeries.msg - TimeSignal.msg -) - -# Generate services in the 'srv' folder -add_service_files( - FILES - DataForward.srv - DataRetrieve.srv - RegisterExperiment.srv - RegisterSetting.srv - RegisterVideo.srv -) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a run_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include -# LIBRARIES data_service - CATKIN_DEPENDS rospy std_msgs message_runtime -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a C++ library -# add_library(data_service -# src/${PROJECT_NAME}/data_service.cpp -# ) - -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(data_service ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -# add_executable(data_service_node src/data_service_node.cpp) - -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(data_service_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(data_service_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS data_service data_service_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_data_service.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) diff --git a/workspace/src/data_service/include/.gitignore b/workspace/src/data_service/include/.gitignore deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/data_service/license.txt b/workspace/src/data_service/license.txt deleted file mode 100644 index 23422cde..00000000 --- a/workspace/src/data_service/license.txt +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2016 Kiet Lam - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/workspace/src/data_service/msg/CustomSignal.msg b/workspace/src/data_service/msg/CustomSignal.msg deleted file mode 100644 index dc97b3fa..00000000 --- a/workspace/src/data_service/msg/CustomSignal.msg +++ /dev/null @@ -1,2 +0,0 @@ -string id -string signal \ No newline at end of file diff --git a/workspace/src/data_service/msg/TimeData.msg b/workspace/src/data_service/msg/TimeData.msg deleted file mode 100644 index b742826f..00000000 --- a/workspace/src/data_service/msg/TimeData.msg +++ /dev/null @@ -1,2 +0,0 @@ -float64 timestamp -float64[] value diff --git a/workspace/src/data_service/msg/TimeSeries.msg b/workspace/src/data_service/msg/TimeSeries.msg deleted file mode 100644 index 619f8768..00000000 --- a/workspace/src/data_service/msg/TimeSeries.msg +++ /dev/null @@ -1 +0,0 @@ -TimeData[] series diff --git a/workspace/src/data_service/msg/TimeSignal.msg b/workspace/src/data_service/msg/TimeSignal.msg deleted file mode 100644 index 0b675da7..00000000 --- a/workspace/src/data_service/msg/TimeSignal.msg +++ /dev/null @@ -1,3 +0,0 @@ -string name -float64[] timestamps -string signal diff --git a/workspace/src/data_service/package.xml b/workspace/src/data_service/package.xml deleted file mode 100644 index 731a2466..00000000 --- a/workspace/src/data_service/package.xml +++ /dev/null @@ -1,16 +0,0 @@ - - - data_service - 0.0.0 - The data_service package - mpc - MIT - message_generation - message_runtime - catkin - rospy - std_msgs - rospy - std_msgs - - diff --git a/workspace/src/data_service/scripts/.gitignore b/workspace/src/data_service/scripts/.gitignore deleted file mode 100644 index 36785238..00000000 --- a/workspace/src/data_service/scripts/.gitignore +++ /dev/null @@ -1 +0,0 @@ -default.cfg \ No newline at end of file diff --git a/workspace/src/data_service/scripts/base.py b/workspace/src/data_service/scripts/base.py deleted file mode 100644 index 346beadd..00000000 --- a/workspace/src/data_service/scripts/base.py +++ /dev/null @@ -1,198 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import json -import os -from uuid import uuid4 -import multiprocessing -import requests -import time -from data_connection import DataConnection - - -import os - - -class Configurator(object): - """ - Manages a config for the Local Computer - """ - - def __init__(self, **kwargs): - if "filename" in kwargs: - with open(kwargs["filename"], 'r') as input: - self.config = json.loads(input.read()) - else: - self.config = { - # ENVIRONMENT VARIBALE OR ROS ENVIRONMENT VARIABLE - 'name': os.environ['TEAM_NAME'], - 'server': os.environ['DATOR_SERVER'], - 'secret_uuid': str(uuid4()), - 'registration_token': 'abcd', - 'id': None - } - - def write_config(self, filename): - """ - Save current config to a json file. - """ - with open(filename, 'w') as output: - output.write(json.dumps(self.config, indent=4)) - - def get_config(self): - return self.config - - def set_config(self, config): - self.config = config - - -CONFIG_LOCATION = "/home/odroid/default.cfg" - - -def init_configurator(CONFIG_LOCATION=CONFIG_LOCATION): - """ Register local computer if not done previously. - :return: The configurator for this local computer - """ - if os.path.isfile(CONFIG_LOCATION): - configurator = Configurator(filename=CONFIG_LOCATION) - print "Found local configurator at {}".format(CONFIG_LOCATION) - else: - configurator = Configurator() - configurator.write_config(CONFIG_LOCATION) - - data_connection = DataConnection(configurator) - my_reg_token = str(uuid4()) - print "Registering to {} with token {}".format(configurator.get_config()['server'], my_reg_token) - data_connection.register(my_reg_token, CONFIG_LOCATION) - configurator.write_config(CONFIG_LOCATION) - - return configurator - - -def periodic_eval(refresh_time_sec, program, should_stop, shared_val, data_connection): - while not should_stop.value: - try: - eval(compile(program, '', 'exec')) - time.sleep(refresh_time_sec) - except BaseException as e: - print ("Error running uploaded program {}".format(e)) - if e.message: - print e.message - return periodic_eval - - -class WorkerPool(object): - - def __init__(self, data_connection): - self.job_list = {} - self.shared_val = multiprocessing.Value('i',0) - self.data_connection = data_connection - - def start_program(self, program_id, refresh_time_sec, program): - if program_id not in self.job_list.keys(): - should_stop = multiprocessing.Value('b', False) - self.job_list[program_id] = [should_stop, - multiprocessing.Process(target=periodic_eval, args=( - refresh_time_sec, - program, - should_stop, - self.shared_val, - self.data_connection - ))] - self.job_list[program_id][1].start() - else: - print "Program id {} already running".format(program_id) - - def stop_program(self, program_id): - try: - job = self.job_list[program_id] - self.job_list[program_id][0].value = True - self.job_list[program_id][1].join(20) - del self.job_list[program_id] - print "Stopped job for program id {}".format(program_id) - except BaseException as e: - print "Failed to stop program {}".format(program_id) - - def stop(self): - for program_id in self.job_list.keys(): - self.stop_program(program_id) - -COMMAND_NOOP = 0 -COMMAND_DONE = 1 -COMMAND_LOAD_PROGRAM = 2 -COMMAND_STOP_PROGRAM = 3 - - -class CommandHandler(object): - - def __init__(self, worker_pool, data_connection): - self.worker_pool = worker_pool - self.data_connection = data_connection - self.handler_map = { - COMMAND_LOAD_PROGRAM: self.handle_load, - COMMAND_STOP_PROGRAM: self.handle_stop, - } - - def handle_commands(self, commands): - done = False - for command in commands: - - print ("Rcx command {}".format(command["type"])) - if command['type'] == COMMAND_NOOP: - pass - elif command['type'] == COMMAND_DONE: - done = True - else: - self.handler_map[command['type']](command) - - data_connection.deactivate_command(command) - return done - - def handle_load(self, command): - program_command = json.loads(command['json_command']) - program = self.data_connection.get_program(program_command['program_id']) - print ("Starting program {}".format(program['id'])) - self.worker_pool.start_program(program['id'], program['sleep_time_sec'], program['code']) - - def handle_stop(self, command): - program_command = json.loads(command['json_command']) - program = self.data_connection.get_program(program_command['program_id']) - - self.worker_pool.stop_program(program['id']) - - -if __name__ == '__main__': - """ - Main loop. Handle commands until done. - """ - - configurator = init_configurator() - data_connection = DataConnection(configurator) - data_connection.update_config(CONFIG_LOCATION) - worker_pool = WorkerPool(data_connection) - command_handler = CommandHandler(worker_pool, data_connection) - - done = False - data_connection.set_local_computer_status(is_running=True) - while not done: - commands = data_connection.get_new_commands() - done = command_handler.handle_commands(commands) - time.sleep(configurator.get_config()['command_refresh_sec']) - - worker_pool.stop() - data_connection.set_local_computer_status(is_running=False) - print("got shared_value {}".format(worker_pool.shared_val.value)) - - print("Received done command. Shutting down.") diff --git a/workspace/src/data_service/scripts/data_connection.py b/workspace/src/data_service/scripts/data_connection.py deleted file mode 100644 index 4797cd40..00000000 --- a/workspace/src/data_service/scripts/data_connection.py +++ /dev/null @@ -1,364 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import json -import uuid -import requests -import datetime as tmz -import pytz -import delorean - - -class DataConnection(object): - - def _get_csrf(self): - """ - :return: Get the csrf from the local session - """ - response = self.client.get(url="{}/noop/".format(self.configurator.get_config()['server'])) - return response.cookies['csrftoken'] - - def __init__(self, configurator): - self.configurator = configurator - self.client = requests.session() - self.csrf = self._get_csrf() - - def _api_url(self, resource_name): - """ - Get restful endpoint for resource type - :param resource_name: The type of resource requested (e.g. "local_computer") - :return: The base api url to connect to the data server - """ - return "{}/api/v1/{}/?format=json".format(self.configurator.get_config()['server'], resource_name) - - def _item_url(self, resource_name, resource_id): - """ - Get restful endpoint for a specific instance of a resource. - :param resource_name: The type of resource requested - :param id: The id of the resource - :return: A specific resource instance - """ - return "{}/api/v1/{}/{}/?format=json".format(self.configurator.get_config()['server'], resource_name, - resource_id) - - def _data_item_url(self, resource_name, resource_id): - """ - Add data points to the given resource - :param resource_name: The type of resource requested - :param id: The id of the resource - :return: A specific data_api resource instance - """ - return "{}/data_api/v1/{}/{}/?format=json".format(self.configurator.get_config()['server'], - resource_name, resource_id) - - def register(self, registration_token=None, file_name=None): - """ - Call to register a new local computer. - """ - config = self.configurator.get_config() - if registration_token: - config["registration_token"] = registration_token - - config["secret_uuid"] = str(uuid.uuid4()) - - response = requests.post(self._api_url('local_computer'), - data=json.dumps(self.configurator.get_config()), - headers=self.post_header()) - - new_config = json.loads(response.content) - self.configurator.set_config(new_config) - - def update_config(self, config_location): - """ - :param config_location: Location of local config file - Update local config with global config. - """ - config = self.configurator.get_config() - url = self._item_url('local_computer', config['id']) - response = requests.get(url, headers= self.sec_header()) - if self.check_response_ok(response): - updated_config = json.loads(response.content) - for key in updated_config.keys(): - config[key] = updated_config[key] - self.configurator.write_config(config_location) - - def set_local_computer_status(self, is_running): - """ - :param isRunning: - """ - config = self.configurator.get_config() - config['is_running'] = is_running - url = self._item_url('local_computer', config['id']) - response = requests.put(url, data=json.dumps(self.configurator.get_config()), headers=self.sec_header()) - - def get_new_commands(self): - """ - :return: Commands from the server for this local computer that should be executed. - """ - config = self.configurator.get_config() - id = config["id"] - url = self._api_url('command') + "&is_local_computer_id={}&is_executed=false".format(id) - response = requests.get(url, headers = self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content)['objects'] - return [] - - def deactivate_command(self, command): - """ - Indicate that a command has been executed. - :param command: - :return: - """ - config = self.configurator.get_config() - id = config["id"] - command['is_executed'] = True - url = self._item_url('command', command['id']) + "&is_local_computer_id={}".format(id) - response = requests.put(url, data=json.dumps(command), headers=self.sec_header()) - self.check_response_ok(response) - - def get_program(self, program_id): - """ - :param program_id: - :return: the program object or None if it doesn't exist - """ - config = self.configurator.get_config() - url = self._item_url('program', program_id) - response = self.client.get(url, headers=self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content) - return None - - - def get_or_create_experiment(self, experiment_name): - """ - Get or create an experiment object for this local computer - :param experiment_name: - :return: The experiment object dict. - """ - config = self.configurator.get_config() - # print config - url = self._api_url('experiment') - params = {'name': experiment_name, 'local_computer_id': config['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - - def add_signal_points(self, signal_id, signal_points): - """ Add sorted time/value points to a signal - :param signal_id: The id of the signal to add points to - :param signal_points: [[,],[, - """ - config = self.configurator.get_config() - url = self._data_item_url('signal', signal_id) - # print 'URL:', url - # print 'HEADERS:', self.post_header() - response = self.client.post(url, data=json.dumps(signal_points), headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting signal data {}".format(response.content) - print "Raising Exception!" - raise Exception("Error posting signal data {}".format(response.content)) - - def add_signal_points_by_name(self, signal_name, signal_points): - """ - Add time/value points to a signal object belonging to this local_computer. - :param signal_name: - :param signal_points: points in format[[,], ...] - """ - config = self.configurator.get_config() - - url = self._api_url('signal') - response = self.client.get(url, params={'name': signal_name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - signal_id = json.loads(response.content)['objects'][0]['id'] - self.add_signal_points(signal_id, signal_points) - - def get_signal_points(self, signal_id): - """ - Get the data points from the given signal. - :param signal_id: - :return: a list of signal point in format[[,], ...] - """ - config = self.configurator.get_config() - - url = self._data_item_url('signal', signal_id) - response = self.client.get(url,headers=self.sec_header()) - if self.check_response_ok(response): - return json.loads(response.content) - return [] - - def get_signal_points_by_name(self, name): - """ - Get the indicated signal from the local computer - :param name: The name of the signal - :return: a list of time/value points [[,] ...] - """ - config = self.configurator.get_config() - url = self._api_url('signal') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - return self.get_signal_points(json.loads(response.content)['objects'][0]['id']) - - def get_or_create_signal(self, signal_name, experiment): - """ - Get or create a signal object for this local computer - :param signal_name: experiment: The experiment - :return: The signal object dict. - """ - config = self.configurator.get_config() - # print config - url = self._api_url('signal') - - params = {'name': signal_name, 'local_computer_id': config['id'], - 'experiment_id': experiment['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def get_or_create_setting(self, key, experiment): - """ - Get or create a setting object for this local computer - :param setting: Setting key - :return: The setting object dict. - """ - config =self.configurator.get_config() - url = self._api_url('setting') - params = {'key': key, 'local_computer_id': config['id'], - 'experiment_id': experiment['id']} - - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def write_setting(self, value, setting_id): - config =self.configurator.get_config() - url = self._data_item_url('setting', setting_id) - - params = dict() - params['value'] = value - - response = self.client.post(url, data=json.dumps(params), headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting setting data {}".format(response.content) - print "Raising Exception!" - raise Exception("Error posting setting data {}".format(response.content)) - - - def get_or_create_blob(self, blob_name): - """ - Get or create a new blob object for this local computer - :param blob_name: - :return: the blob object dict. - """ - config =self.configurator.get_config() - url = self._api_url('blob') - params = {'name': blob_name, 'local_computer_id': config['id']} - response = self.client.get(url, params=params, headers=self.sec_header()) - if len(json.loads(response.content)['objects']) == 0: - response = self.client.post(url, data=json.dumps(params), headers=self.post_header()) - response = self.client.get(url, params=params, headers=self.sec_header()) - - return json.loads(response.content)['objects'][0] - - def set_blob_data(self, id, blob_data): - """ - Store data in a blob object. - :param id: - :param blob_data: String representation of blob to store - :return: - """ - url = self._data_item_url('blob', id) - response = self.client.post(url, data=blob_data, headers=self.post_header()) - if not DataConnection.check_response_ok(response): - print "Error posting blob data {}".format(response.content) - - def set_blob_data_by_name(self, name, blob_data): - config = self.configurator.get_config() - url = self._api_url('blob') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - self.set_blob_data(json.loads(response.content)['objects'][0]['id'], blob_data) - - - def get_blob_data(self, id): - url = self._data_item_url('blob', id) - response = self.client.get(url, headers=self.sec_header()) - return response.content - - def get_blob_data_by_name(self, name): - config = self.configurator.get_config() - url = self._api_url('blob') - response = self.client.get(url, params={'name': name, 'local_computer_id': config['id']}, - headers=self.sec_header()) - return self.get_blob_data(json.loads(response.content)['objects'][0]['id']) - - def create_event(self, event_type, info): - config = self.configurator.get_config() - url = self._api_url('event') - response = self.client.post(url, data=json.dumps({'type': event_type, 'info': info, - 'local_computer_id': config['id']}), - headers=self.post_header()) - if not self.check_response_ok(response): - print "Error creating event {}: {}".format(event_type, info) - - def sec_header(self, base_header=None): - auth_header = {'auth_key': self.configurator.get_config()["secret_uuid"], - 'content-type': 'application/json'} - if base_header is None: - return auth_header - auth_header.update(base_header) - - def post_header(self): - auth_header = {'auth_key': self.configurator.get_config()["secret_uuid"], - 'content-type': 'application/json', "X-CSRFToken": self.csrf} - return auth_header - - @classmethod - def check_response_ok(cls, response): - """ - :return: True if http response is a 200 class response. False info otherwise. - """ - if 200 <= response.status_code < 300: - return True - else: - response_string = "WARNING Command lookup failed: status: {} reason: {}".format(response.status_code, - response.reason) - if response.content: - response_string += response.content - print response_string - return False - - @classmethod - def millisec_to_utc(cls, millisec): - return tmz.datetime.fromtimestamp(float(millisec), tz=pytz.UTC) - - @classmethod - def utc_to_millisec(cls, dt): -# return delorean.Delorean(dt, timezone="UTC") -# print 'DELORRREAANN' -# print delorean.Delorean(dt, timezone="UTC").epoch - return delorean.Delorean(dt, timezone="UTC").epoch diff --git a/workspace/src/data_service/scripts/raw_test.py b/workspace/src/data_service/scripts/raw_test.py deleted file mode 100755 index efeb9f29..00000000 --- a/workspace/src/data_service/scripts/raw_test.py +++ /dev/null @@ -1,34 +0,0 @@ -#!/home/odroid/.virtualenvs/research/bin/python - -import time -from data_connection import * -from base import * -import datetime -import pytz - -# create data connection -configurator = init_configurator() -print configurator.config -data_connection = DataConnection(configurator) - -# signal = data_connection.get_or_create_signal("test_signal_saw_1") -signal = data_connection.get_or_create_signal("test_signal") - -# print signal - -# t = datetime.datetime.now(tz=pytz.UTC) -# print "got datetime {}".format(t) - -# tvec = [t + datetime.timedelta(seconds=i*.01) for i in range(10)] -# tsvec = [DataConnection.utc_to_millisec(at) for at in tvec] - -# # create a sawtooth wave -# signal_vector = [[ats-tsvec[0], ats] for ats in tsvec] - -# print signal_vector - -# data_connection.add_signal_points(signal['id'], signal_vector) - - -# Uncomment here to get data -print data_connection.get_signal_points(signal['id']) diff --git a/workspace/src/data_service/scripts/service.py b/workspace/src/data_service/scripts/service.py deleted file mode 100755 index a929c3cd..00000000 --- a/workspace/src/data_service/scripts/service.py +++ /dev/null @@ -1,208 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# ---------------------------------------------------------------------------#!/usr/bin/env python - -import time - -import rospy -import json -import datetime -import pytz - -from std_msgs.msg import Float64MultiArray - -from data_connection import * -from base import * - -from data_service.srv import * -from data_service.msg import * - - -configurator = init_configurator() -data_connection = DataConnection(configurator) - -signal_name_map = dict() -blob_name_map = dict() -experiment_name_map = dict() - -response_ok = 'Ok' - - -def get_experiment(experiment_name): - - if experiment_name not in experiment_name_map: - print 'Requesting new experiment' - experiment = data_connection.get_or_create_experiment(experiment_name) - experiment_name_map[experiment_name] = experiment - - return experiment_name_map[experiment_name] - - -def get_time_signal(signal_name, experiment_name): - map_key = (signal_name, experiment_name) - - experiment = get_experiment(experiment_name) - - if map_key not in signal_name_map: - print 'Requesting new time signal' - signal = data_connection.get_or_create_signal(signal_name, experiment) - signal_name_map[map_key] = signal - - return signal_name_map[map_key] - - -def get_blob_signal(signal_name): - - if signal_name not in blob_name_map: - print 'Requesting new blob signal' - signal = data_connection.get_or_create_blob(signal_name) - blob_name_map[signal_name] = signal - - return blob_name_map[signal_name] - - -def send_time_signal(time_signal, experiment_name): - signal_name = time_signal.name - signal = get_time_signal(signal_name, experiment_name) - response = response_ok - - timestamps = time_signal.timestamps - signals = json.loads(time_signal.signal) - signal_points = [] - - for ts, sig in zip(timestamps, signals): - if type(sig) is list: - signal_points.append([ts] + sig) - else: - signal_points.append([ts, sig]) - # sig.append(ts) - # signal_points.append(sig) - - data_connection.add_signal_points(signal['id'], - signal_points) - return response - - -def send_custom_signal(custom_signal, experiment_id): - signal = get_blob_signal(custom_signal.id) - response = response_ok - - try: - data_connection.set_blob_data(signal['id'], custom_signal.signal, experiment_id) - except Exception as e: - response = str(e) - - return response - - -def handle_send_data(req): - response = response_ok - - if req.time_signal != None and req.time_signal.name != '': - try: - send_time_signal(req.time_signal, req.experiment_name) - except Exception as e: - response = str(e) - print e - print 'Updating cache and retrying' - map_key = (req.time_signal.name, req.experiment_name) - if req.experiment_name in experiment_name_map: - del experiment_name_map[req.experiment_name] - if map_key in signal_name_map: - del signal_name_map[map_key] - send_time_signal(req.time_signal, req.experiment_name) - -# if req.custom_signal != None and req.custom_signal.id != '': -# response = send_custom_signal(req.custom_signal) -# if response != response_ok: -# return DataForwardResponse(response) - - return DataForwardResponse(response) - - -def handle_retrieve_data(req): - if req.id is None or req.id == '': - return None - - signal_id = req.id - - signal_response = CustomSignal() - signal_response.id = req.id - - if req.is_time: - signal = get_time_signal(signal_id) - try: - response = data_connection.get_signal_points(signal['id']) - signal_response.signal = json.dumps(response) - except Exception as e: - signal_response.signal = str(e) - return DataRetrieveResponse(signal_response) - else: - signal = get_blob_signal(signal_id) - try: - response = data_connection.get_blob_data(signal['id']) - signal_response.signal = response - except Exception as e: - signal_response.signal = str(e) - return DataRetrieveResponse(signal_response) - - return DataRetrieveResponse(signal_response) - - -def handle_register_experiment(req): - response = data_connection.get_or_create_experiment(req.experiment) - return RegisterExperimentResponse(response['id']) - - -def handle_register_setting(req): - response = response_ok - try: - setting = data_connection.get_or_create_setting(req.key) - data_connection.write_setting(req.key, req.value) - except Exception as e: - response = str(e) - - return RegisterSettingResponse(response) - - -def register_video(experiment, video_path): - try: - setting = data_connection.get_or_create_setting('video', experiment) - data_connection.write_setting(video_path, setting['id']) - return response_ok - except Exception as e: - return str(e) - - -def handle_register_video(req): - experiment = get_experiment(req.experiment) - response = register_video(experiment, req.path) - print response - return RegisterVideoResponse(response) - - -def send_data_service(): - rospy.init_node('data_service', anonymous=True) - s1 = rospy.Service('send_data', DataForward, handle_send_data) - s2 = rospy.Service('retrieve_data', DataRetrieve, handle_retrieve_data) - e1 = rospy.Service('register_experiment', RegisterExperiment, handle_register_experiment) - v1 = rospy.Service('register_video', RegisterVideo, handle_register_video) - - settings_service = rospy.Service('register_setting', RegisterSetting, handle_register_setting) - - rospy.spin() - - -if __name__ == "__main__": - send_data_service() diff --git a/workspace/src/data_service/scripts/test.py b/workspace/src/data_service/scripts/test.py deleted file mode 100755 index 8dd7bb10..00000000 --- a/workspace/src/data_service/scripts/test.py +++ /dev/null @@ -1,56 +0,0 @@ -#!/usr/bin/env python -import rospy - -import time -from data_connection import * -from base import * -import datetime -import pytz - -from data_service.srv import * -from data_service.msg import * - -from std_msgs.msg import Float64MultiArray - - -import json - -if __name__ == '__main__': - - rospy.wait_for_service('send_data') - rospy.wait_for_service('register_experiment') - rospy.wait_for_service('register_setting') - - experiment_name = 'ex3' - - # generate and send signal - try: - t = datetime.datetime.now(tz=pytz.UTC) - tvec = [t + datetime.timedelta(seconds=i*.01) for i in range(10)] - tsvec = [DataConnection.utc_to_millisec(at) for at in tvec] - # signal_vector = [ats-tsvec[0] for ats in tsvec] - signal_vector = [float(i)*15.3 for i in range(len(tsvec))] - signal_vs = [] - - for s in signal_vector: - signal_vs.append([s, 3]) - - send_data = rospy.ServiceProxy('send_data', DataForward) - time_signal = TimeSignal() - time_signal.name = 'mpc_sig2' - time_signal.timestamps = signal_vector - time_signal.signal = json.dumps(signal_vs) - - print "Time Signal", time_signal - print "Experiment name", experiment_name - - response = send_data(time_signal, None, experiment_name) - except rospy.ServiceException, e: - print 'Call to service failed: %s' %e - - - # # Register a setting - register_setting = rospy.ServiceProxy('register_setting', RegisterSetting) - - register_setting('I', '0.20') - register_setting('P', '0.60') diff --git a/workspace/src/data_service/scripts/upload.py b/workspace/src/data_service/scripts/upload.py deleted file mode 100755 index 3a1eece9..00000000 --- a/workspace/src/data_service/scripts/upload.py +++ /dev/null @@ -1,103 +0,0 @@ -#!/usr/bin/env python - -# --------------------------------------------------------------------------- -# Licensing Information: You are free to use or extend these projects for -# education or reserach purposes provided that (1) you retain this notice -# and (2) you provide clear attribution to UC Berkeley, including a link -# to http://barc-project.com -# -# Attibution Information: The barc project ROS code-base was developed -# at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales -# (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed -# by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was -# based on an open source project by Bruce Wootton -# --------------------------------------------------------------------------- - -import os, sys -import time - -os.chdir('/home/odroid/barc/workspace/src/data_service/scripts') - -from data_connection import * -from base import * - -proj_path = '/home/odroid/barc/Dator' - -os.environ.setdefault('DJANGO_SETTINGS_MODULE', 'dator.settings') -sys.path.append(proj_path) -os.chdir(proj_path) - -from django.core.wsgi import get_wsgi_application -application = get_wsgi_application() - -from data_api.models import * - -import boto3 - -CLOUD_CONFIG_LOCATION = '/home/odroid/cloud.cfg' - - -S3_VIDEOS_BUCKET = 'datorvideos' - -rosbag_dir = os.path.expanduser("~") + '/rosbag' -video_dir = os.path.expanduser("~") + '/video' - - -if __name__ == '__main__': - configurator = init_configurator(CLOUD_CONFIG_LOCATION) - data_connection = DataConnection(configurator) - - s3_client = boto3.client('s3') - s3 = boto3.resource('s3') - - for sig in Signal.objects.all(): - try: - experiment = data_connection.get_or_create_experiment(sig.experiment.name) - - for setting in sig.experiment.setting_set.all(): - setting_remote = data_connection.get_or_create_setting(setting.key, experiment) - - #TODO: Check if AWS S3 token exists - if setting.key == 'video': - if setting.value.startswith(video_dir): - key_name = '%s_%s.avi' % (os.environ['TEAM_NAME'], sig.experiment.name) - - bucket = s3.Bucket(S3_VIDEOS_BUCKET) - bucket.Acl().put(ACL='public-read') - - obj = s3.Object(S3_VIDEOS_BUCKET, key_name) - print 'Uploading video' - video_path = '%s/%s.avi' %(video_dir, sig.experiment.name) - obj.put(Body=open(video_path, 'rb')) - obj.Acl().put(ACL='public-read') - - print 'Finished uploading video' - url = '{}/{}/{}'.format(s3_client.meta.endpoint_url, - S3_VIDEOS_BUCKET, key_name) - setting.value = url - setting.save() - - os.remove(video_path) - - data_connection.write_setting(setting.value, setting_remote['id']) - - signal = data_connection.get_or_create_signal(sig.name, experiment) - - try: - lst = LocalSignalTag.objects.filter(signal__name=sig.name, signal__experiment__name=sig.experiment.name)[0] - except (LocalSignalTag.DoesNotExist, IndexError) as e: - lst = LocalSignalTag() - lst.signal = sig - lst.uploaded = False - - if not lst.uploaded: - print 'Uploading' - data_connection.add_signal_points(signal['id'], sig.get_data()) - lst.uploaded = True - lst.save() - print 'Finished Uploading' - - lst.save() - - except Exception as e: - print e diff --git a/workspace/src/data_service/srv/DataForward.srv b/workspace/src/data_service/srv/DataForward.srv deleted file mode 100644 index 7cf208ca..00000000 --- a/workspace/src/data_service/srv/DataForward.srv +++ /dev/null @@ -1,5 +0,0 @@ -TimeSignal time_signal -CustomSignal custom_signal -string experiment_name ---- -string response diff --git a/workspace/src/data_service/srv/DataRetrieve.srv b/workspace/src/data_service/srv/DataRetrieve.srv deleted file mode 100644 index 916122d0..00000000 --- a/workspace/src/data_service/srv/DataRetrieve.srv +++ /dev/null @@ -1,4 +0,0 @@ -string id -bool is_time ---- -CustomSignal signal \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterExperiment.srv b/workspace/src/data_service/srv/RegisterExperiment.srv deleted file mode 100644 index 1842fe63..00000000 --- a/workspace/src/data_service/srv/RegisterExperiment.srv +++ /dev/null @@ -1,3 +0,0 @@ -string experiment ---- -int32 experiment_id \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterSetting.srv b/workspace/src/data_service/srv/RegisterSetting.srv deleted file mode 100644 index 54e669b6..00000000 --- a/workspace/src/data_service/srv/RegisterSetting.srv +++ /dev/null @@ -1,4 +0,0 @@ -string key -string value ---- -string response \ No newline at end of file diff --git a/workspace/src/data_service/srv/RegisterVideo.srv b/workspace/src/data_service/srv/RegisterVideo.srv deleted file mode 100644 index 35a01246..00000000 --- a/workspace/src/data_service/srv/RegisterVideo.srv +++ /dev/null @@ -1,4 +0,0 @@ -string experiment -string path ---- -string response \ No newline at end of file diff --git a/workspace/src/marvelmind_nav/CHANGELOG.rst b/workspace/src/marvelmind_nav/CHANGELOG.rst new file mode 100755 index 00000000..ca2e2c7e --- /dev/null +++ b/workspace/src/marvelmind_nav/CHANGELOG.rst @@ -0,0 +1,21 @@ +Changelog for package marvelmind_nav + +1.0.8 (2018-03-15) +------------------ + +1.0.7 (2018-03-13) +------------------ + +1.0.6 (2017-02-10) +------------------ + +1.0.5 (2016-09-15) +------------------ + +1.0.4 (2016-09-13) +------------------ + +1.0.3 (2016-09-13) +------------------ +* initial commit +* Contributors: smoker77 diff --git a/workspace/src/barc_gui/CMakeLists.txt b/workspace/src/marvelmind_nav/CMakeLists.txt old mode 100644 new mode 100755 similarity index 80% rename from workspace/src/barc_gui/CMakeLists.txt rename to workspace/src/marvelmind_nav/CMakeLists.txt index 81332996..407757ad --- a/workspace/src/barc_gui/CMakeLists.txt +++ b/workspace/src/marvelmind_nav/CMakeLists.txt @@ -1,13 +1,16 @@ cmake_minimum_required(VERSION 2.8.3) -project(barc_gui) +project(marvelmind_nav) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS + roscpp rospy - rqt_gui - rqt_gui_py + std_msgs + message_generation + genmsg + visualization_msgs ) ## System dependencies are found with CMake's conventions @@ -17,7 +20,7 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() +# catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -44,11 +47,17 @@ catkin_python_setup() ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) +add_message_files( + DIRECTORY msg + FILES + hedge_pos.msg + hedge_pos_a.msg + hedge_pos_ang.msg + beacon_pos_a.msg + hedge_imu_raw.msg + hedge_imu_fusion.msg + beacon_distance.msg +) ## Generate services in the 'srv' folder # add_service_files( @@ -65,10 +74,10 @@ catkin_python_setup() # ) ## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) +generate_messages( + DEPENDENCIES + std_msgs +) ################################################ ## Declare ROS dynamic reconfigure parameters ## @@ -100,9 +109,10 @@ catkin_python_setup() ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES rqt_car -# CATKIN_DEPENDS rospy rqt_gui rqt_gui_py +INCLUDE_DIRS include/${PROJECT_NAME}/ +# LIBRARIES marvelmind_nav +CATKIN_DEPENDS message_runtime +# CATKIN_DEPENDS roscpp rospy std_msgs # DEPENDS system_lib ) @@ -116,38 +126,47 @@ catkin_package( include_directories( ${catkin_INCLUDE_DIRS} ) +include_directories(include ${roscpp_INCLUDE_DIRS}) ## Declare a C++ library -# add_library(rqt_car -# src/${PROJECT_NAME}/rqt_car.cpp -# ) +add_library(marvelmind_nav + src/marvelmind_hedge.c +) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure -# add_dependencies(rqt_car ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +#add_dependencies(marvelmind_nav ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable -# add_executable(rqt_car_node src/rqt_car_node.cpp) +add_executable(hedge_rcv_bin +src/hedge_rcv_bin.cpp +src/marvelmind_hedge.c +) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(rqt_car_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(hedge_rcv_bin marvelmind_nav_generate_messages_cpp) -## Specify libraries to link a library or executable target against -# target_link_libraries(rqt_car_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(hedge_rcv_bin + ${catkin_LIBRARIES} +) -############# -## Install ## -############# +##################### + +add_executable(subscriber_test +src/subscriber_test.cpp +) +add_dependencies(subscriber_test marvelmind_nav_generate_messages_cpp) -install(PROGRAMS scripts/barc_gui - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_link_libraries(subscriber_test + ${catkin_LIBRARIES} ) + +############# +## Install ## +############# + # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html @@ -159,14 +178,14 @@ install(PROGRAMS scripts/barc_gui # ) ## Mark executables and/or libraries for installation -# install(TARGETS rqt_car rqt_car_node +# install(TARGETS marvelmind_nav marvelmind_nav_node # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ +#install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE @@ -184,7 +203,7 @@ install(PROGRAMS scripts/barc_gui ############# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_rqt_car.cpp) +# catkin_add_gtest(${PROJECT_NAME}-test test/test_marvelmind_nav.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() diff --git a/workspace/src/marvelmind_nav/include/marvelmind_nav/marvelmind_hedge.h b/workspace/src/marvelmind_nav/include/marvelmind_nav/marvelmind_hedge.h new file mode 100755 index 00000000..9c54aa95 --- /dev/null +++ b/workspace/src/marvelmind_nav/include/marvelmind_nav/marvelmind_hedge.h @@ -0,0 +1,178 @@ +#pragma once +#include +#include +#include + +#define DATA_INPUT_SEMAPHORE "/data_input_semaphore" + +struct PositionValue +{ + uint8_t address; + uint32_t timestamp; + int32_t x, y, z;// coordinates in millimeters + uint8_t flags; + + double angle; + + bool highResolution; + + bool ready; + bool processed; +}; + +struct RawIMUValue +{ + int16_t acc_x; + int16_t acc_y; + int16_t acc_z; + + int16_t gyro_x; + int16_t gyro_y; + int16_t gyro_z; + + int16_t compass_x; + int16_t compass_y; + int16_t compass_z; + + uint32_t timestamp; + + bool updated; +}; + +struct FusionIMUValue +{ + int32_t x; + int32_t y; + int32_t z;// coordinates in mm + + int16_t qw; + int16_t qx; + int16_t qy; + int16_t qz;// quaternion, normalized to 10000 + + int16_t vx; + int16_t vy; + int16_t vz;// velocity, mm/s + + int16_t ax; + int16_t ay; + int16_t az;// acceleration, mm/s^2 + + uint32_t timestamp; + + bool updated; +}; + +struct RawDistanceItem +{ + uint8_t address_beacon; + uint32_t distance;// distance, mm +}; +struct RawDistances +{ + uint8_t address_hedge; + struct RawDistanceItem distances[4]; + + bool updated; +}; + +struct StationaryBeaconPosition +{ + uint8_t address; + int32_t x, y, z;// coordinates in millimeters + + bool updatedForMsg; + bool highResolution; +}; +#define MAX_STATIONARY_BEACONS 30 +struct StationaryBeaconsPositions +{ + uint8_t numBeacons; + struct StationaryBeaconPosition beacons[MAX_STATIONARY_BEACONS]; + + bool updated; +}; + +struct MarvelmindHedge +{ +// serial port device name (physical or USB/virtual). It should be provided as +// an argument: +// /dev/ttyACM0 - typical for Linux / Raspberry Pi +// /dev/tty.usbmodem1451 - typical for Mac OS X + const char * ttyFileName; + +// Baud rate. Should be match to baudrate of hedgehog-beacon +// default: 9600 + uint32_t baudRate; + +// maximum count of measurements of coordinates stored in buffer +// default: 3 + uint8_t maxBufferedPositions; + +// buffer of measurements + struct PositionValue * positionBuffer; + + struct StationaryBeaconsPositions positionsBeacons; + + struct RawIMUValue rawIMU; + struct FusionIMUValue fusionIMU; + + struct RawDistances rawDistances; + +// verbose flag which activate console output +// default: False + bool verbose; + +// pause flag. If True, class would not read serial data + bool pause; + +// If True, thread would exit from main loop and stop + bool terminationRequired; + +// receiveDataCallback is callback function to recieve data + void (*receiveDataCallback)(struct PositionValue position); + void (*anyInputPacketCallback)(); + +// private variables + uint8_t lastValuesCount_; + uint8_t lastValues_next; + bool haveNewValues_; +#ifdef WIN32 + HANDLE thread_; + CRITICAL_SECTION lock_; +#else + pthread_t thread_; + pthread_mutex_t lock_; +#endif +}; + +#define POSITION_DATAGRAM_ID 0x0001 +#define BEACONS_POSITIONS_DATAGRAM_ID 0x0002 +#define POSITION_DATAGRAM_HIGHRES_ID 0x0011 +#define BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID 0x0012 +#define IMU_RAW_DATAGRAM_ID 0x0003 +#define BEACON_RAW_DISTANCE_DATAGRAM_ID 0x0004 +#define IMU_FUSION_DATAGRAM_ID 0x0005 + +struct MarvelmindHedge * createMarvelmindHedge (); +void destroyMarvelmindHedge (struct MarvelmindHedge * hedge); +void startMarvelmindHedge (struct MarvelmindHedge * hedge); + +void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew); +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position); + +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew); +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions); +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address); + +void stopMarvelmindHedge (struct MarvelmindHedge * hedge); + +#ifdef WIN32 +#define DEFAULT_TTY_FILENAME "\\\\.\\COM3" +#else +#define DEFAULT_TTY_FILENAME "/dev/ttyACM0" +#endif // WIN32 diff --git a/workspace/src/marvelmind_nav/msg/beacon_distance.msg b/workspace/src/marvelmind_nav/msg/beacon_distance.msg new file mode 100755 index 00000000..56d2a3f6 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/beacon_distance.msg @@ -0,0 +1,3 @@ +uint8 address_hedge +uint8 address_beacon +float64 distance_m diff --git a/workspace/src/marvelmind_nav/msg/beacon_pos_a.msg b/workspace/src/marvelmind_nav/msg/beacon_pos_a.msg new file mode 100755 index 00000000..2f71c076 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/beacon_pos_a.msg @@ -0,0 +1,4 @@ +uint8 address +float64 x_m +float64 y_m +float64 z_m diff --git a/workspace/src/marvelmind_nav/msg/hedge_imu_fusion.msg b/workspace/src/marvelmind_nav/msg/hedge_imu_fusion.msg new file mode 100755 index 00000000..3c18b884 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/hedge_imu_fusion.msg @@ -0,0 +1,15 @@ +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +float64 qw +float64 qx +float64 qy +float64 qz +float64 vx +float64 vy +float64 vz +float64 ax +float64 ay +float64 az + diff --git a/workspace/src/marvelmind_nav/msg/hedge_imu_raw.msg b/workspace/src/marvelmind_nav/msg/hedge_imu_raw.msg new file mode 100755 index 00000000..a2b78594 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/hedge_imu_raw.msg @@ -0,0 +1,10 @@ +int64 timestamp_ms +int16 acc_x +int16 acc_y +int16 acc_z +int16 gyro_x +int16 gyro_y +int16 gyro_z +int16 compass_x +int16 compass_y +int16 compass_z diff --git a/workspace/src/marvelmind_nav/msg/hedge_pos.msg b/workspace/src/marvelmind_nav/msg/hedge_pos.msg new file mode 100755 index 00000000..84a917dc --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/hedge_pos.msg @@ -0,0 +1,5 @@ +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags diff --git a/workspace/src/marvelmind_nav/msg/hedge_pos_a.msg b/workspace/src/marvelmind_nav/msg/hedge_pos_a.msg new file mode 100755 index 00000000..1c826020 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/hedge_pos_a.msg @@ -0,0 +1,6 @@ +uint8 address +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags diff --git a/workspace/src/marvelmind_nav/msg/hedge_pos_ang.msg b/workspace/src/marvelmind_nav/msg/hedge_pos_ang.msg new file mode 100755 index 00000000..f4168ec0 --- /dev/null +++ b/workspace/src/marvelmind_nav/msg/hedge_pos_ang.msg @@ -0,0 +1,7 @@ +uint8 address +int64 timestamp_ms +float64 x_m +float64 y_m +float64 z_m +uint8 flags +float64 angle diff --git a/workspace/src/marvelmind_nav/package.xml b/workspace/src/marvelmind_nav/package.xml new file mode 100755 index 00000000..b48ad9dc --- /dev/null +++ b/workspace/src/marvelmind_nav/package.xml @@ -0,0 +1,61 @@ + + + marvelmind_nav + 1.0.8 + Marvelmind local navigation system + + + + + + smoker77 + + + + + + BSD + + + + + + + http://marvelmind.com + + + + + + + smoker77 + + + + + + + message_generation + + + + message_runtime + + + catkin + roscpp + rospy + std_msgs + visualization_msgs + roscpp + rospy + std_msgs + visualization_msgs + + + + + + + + diff --git a/workspace/src/marvelmind_nav/src/hedge_rcv_bin.cpp b/workspace/src/marvelmind_nav/src/hedge_rcv_bin.cpp new file mode 100755 index 00000000..cbff5367 --- /dev/null +++ b/workspace/src/marvelmind_nav/src/hedge_rcv_bin.cpp @@ -0,0 +1,373 @@ +#include +#include +#include +#include +#include +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/hedge_pos_ang.h" +#include "marvelmind_nav/beacon_pos_a.h" +#include "marvelmind_nav/hedge_imu_raw.h" +#include "marvelmind_nav/hedge_imu_fusion.h" +#include "marvelmind_nav/beacon_distance.h" +extern "C" +{ +#include "marvelmind_nav/marvelmind_hedge.h" +} + +#include + +#define ROS_NODE_NAME "hedge_rcv_bin" +#define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define HEDGE_POSITION_WITH_ANGLE_TOPIC_NAME "/hedge_pos_ang" +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" +#define HEDGE_IMU_RAW_TOPIC_NAME "/hedge_imu_raw" +#define HEDGE_IMU_FUSION_TOPIC_NAME "/hedge_imu_fusion" +#define BEACON_RAW_DISTANCE_TOPIC_NAME "/beacon_raw_distance" + +struct MarvelmindHedge * hedge= NULL; + +static uint32_t hedge_timestamp_prev= 0; +marvelmind_nav::hedge_pos hedge_pos_noaddress_msg;// hedge coordinates message (old version without address) for publishing to ROS topic +marvelmind_nav::hedge_pos_a hedge_pos_msg;// hedge coordinates message for publishing to ROS topic +marvelmind_nav::hedge_pos_ang hedge_pos_ang_msg;// hedge coordinates and angle message for publishing to ROS topic +marvelmind_nav::beacon_pos_a beacon_pos_msg;// stationary beacon coordinates message for publishing to ROS topic +marvelmind_nav::hedge_imu_raw hedge_imu_raw_msg;// raw IMU data message for publishing to ROS topic +marvelmind_nav::hedge_imu_fusion hedge_imu_fusion_msg;// IMU fusion data message for publishing to ROS topic +marvelmind_nav::beacon_distance beacon_raw_distance_msg;// Raw distance message for publishing to ROS topic + +static sem_t *sem; +struct timespec ts; + +//////////////////////////////////////////////////////////////////////// + +void semCallback() +{ + sem_post(sem); +} + +static int hedgeReceivePrepare(int argc, char **argv) +{ + // get port name from command line arguments (if specified) + const char * ttyFileName; + if (argc==2) ttyFileName=argv[1]; + else ttyFileName=DEFAULT_TTY_FILENAME; + + // Init + hedge=createMarvelmindHedge (); + if (hedge==NULL) + { + ROS_INFO ("Error: Unable to create MarvelmindHedge"); + return -1; + } + hedge->ttyFileName=ttyFileName; + hedge->verbose=true; // show errors and warnings + hedge->anyInputPacketCallback= semCallback; + startMarvelmindHedge (hedge); +} + +static bool hedgeReceiveCheck(void) +{ + if (hedge->haveNewValues_) + { + struct PositionValue position; + getPositionFromMarvelmindHedge (hedge, &position); + + hedge_pos_msg.address= position.address; + hedge_pos_ang_msg.address= position.address; + + hedge_pos_msg.flags= position.flags; + hedge_pos_noaddress_msg.flags= position.flags; + hedge_pos_ang_msg.flags= position.flags; + if (hedge_pos_msg.flags&(1<<1))// flag of timestamp format + { + hedge_pos_msg.timestamp_ms= position.timestamp;// msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp; + } + else + { + hedge_pos_msg.timestamp_ms= position.timestamp*15.625;// alpha-cycles ==> msec + hedge_pos_noaddress_msg.timestamp_ms= position.timestamp*15.625; + } + hedge_pos_ang_msg.timestamp_ms= position.timestamp; + + hedge_pos_msg.x_m= position.x/1000.0; + hedge_pos_msg.y_m= position.y/1000.0; + hedge_pos_msg.z_m= position.z/1000.0; + + hedge_pos_noaddress_msg.x_m= position.x/1000.0; + hedge_pos_noaddress_msg.y_m= position.y/1000.0; + hedge_pos_noaddress_msg.z_m= position.z/1000.0; + + hedge_pos_ang_msg.x_m= position.x/1000.0; + hedge_pos_ang_msg.y_m= position.y/1000.0; + hedge_pos_ang_msg.z_m= position.z/1000.0; + + hedge_pos_ang_msg.angle= position.angle; + + hedge->haveNewValues_=false; + + return true; + } + return false; +} + +static bool beaconReceiveCheck(void) +{ + uint8_t i; + struct StationaryBeaconsPositions positions; + struct StationaryBeaconPosition *bp= NULL; + bool foundUpd= false; + uint8_t n; + + getStationaryBeaconsPositionsFromMarvelmindHedge (hedge, &positions); + n= positions.numBeacons; + if (n == 0) + return false; + + for(i=0;iupdatedForMsg) + { + clearStationaryBeaconUpdatedFlag(hedge, bp->address); + foundUpd= true; + break; + } + } + if (!foundUpd) + return false; + if (bp == NULL) + return false; + + beacon_pos_msg.address= bp->address; + beacon_pos_msg.x_m= bp->x/1000.0; + beacon_pos_msg.y_m= bp->y/1000.0; + beacon_pos_msg.z_m= bp->z/1000.0; + + return true; +} + +static bool hedgeIMURawReceiveCheck(void) +{ + if (!hedge->rawIMU.updated) + return false; + + hedge_imu_raw_msg.acc_x= hedge->rawIMU.acc_x; + hedge_imu_raw_msg.acc_y= hedge->rawIMU.acc_y; + hedge_imu_raw_msg.acc_z= hedge->rawIMU.acc_z; + + hedge_imu_raw_msg.gyro_x= hedge->rawIMU.gyro_x; + hedge_imu_raw_msg.gyro_y= hedge->rawIMU.gyro_y; + hedge_imu_raw_msg.gyro_z= hedge->rawIMU.gyro_z; + + hedge_imu_raw_msg.compass_x= hedge->rawIMU.compass_x; + hedge_imu_raw_msg.compass_y= hedge->rawIMU.compass_y; + hedge_imu_raw_msg.compass_z= hedge->rawIMU.compass_z; + + hedge_imu_raw_msg.timestamp_ms= hedge->rawIMU.timestamp; + + hedge->rawIMU.updated= false; + + return true; +} + +static bool hedgeIMUFusionReceiveCheck(void) +{ + if (!hedge->fusionIMU.updated) + return false; + + hedge_imu_fusion_msg.x_m= hedge->fusionIMU.x/1000.0; + hedge_imu_fusion_msg.y_m= hedge->fusionIMU.y/1000.0; + hedge_imu_fusion_msg.z_m= hedge->fusionIMU.z/1000.0; + + hedge_imu_fusion_msg.qw= hedge->fusionIMU.qw/10000.0; + hedge_imu_fusion_msg.qx= hedge->fusionIMU.qx/10000.0; + hedge_imu_fusion_msg.qy= hedge->fusionIMU.qy/10000.0; + hedge_imu_fusion_msg.qz= hedge->fusionIMU.qz/10000.0; + + hedge_imu_fusion_msg.vx= hedge->fusionIMU.vx/1000.0; + hedge_imu_fusion_msg.vy= hedge->fusionIMU.vy/1000.0; + hedge_imu_fusion_msg.vz= hedge->fusionIMU.vz/1000.0; + + hedge_imu_fusion_msg.ax= hedge->fusionIMU.ax/1000.0; + hedge_imu_fusion_msg.ay= hedge->fusionIMU.ay/1000.0; + hedge_imu_fusion_msg.az= hedge->fusionIMU.az/1000.0; + + hedge_imu_fusion_msg.timestamp_ms= hedge->fusionIMU.timestamp; + + hedge->fusionIMU.updated= false; + + return true; +} + +static void getRawDistance(uint8_t index) +{ + beacon_raw_distance_msg.address_hedge= hedge->rawDistances.address_hedge; + beacon_raw_distance_msg.address_beacon= hedge->rawDistances.distances[index].address_beacon; + beacon_raw_distance_msg.distance_m= hedge->rawDistances.distances[index].distance/1000.0; +} + + +/** + * Node for Marvelmind hedgehog binary streaming data processing + */ +int main(int argc, char **argv) +{uint8_t beaconReadIterations; + uint8_t rawDistanceReadIterations; + // initialize ROS node + ros::init(argc, argv, ROS_NODE_NAME); + + sem = sem_open(DATA_INPUT_SEMAPHORE, O_CREAT, 0777, 0); + + // prepare hedgehog data receiver module + hedgeReceivePrepare(argc, argv); + + // ROS node reference + ros::NodeHandle n; + + // Register topics for puplishing messages + ros::Publisher hedge_pos_ang_publisher = n.advertise(HEDGE_POSITION_WITH_ANGLE_TOPIC_NAME, 1000); + ros::Publisher hedge_pos_publisher = n.advertise(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000); + ros::Publisher hedge_pos_noaddress_publisher = n.advertise(HEDGE_POSITION_TOPIC_NAME, 1000); + + ros::Publisher beacons_pos_publisher = n.advertise(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000); + + ros::Publisher hedge_imu_raw_publisher = n.advertise(HEDGE_IMU_RAW_TOPIC_NAME, 1000); + ros::Publisher hedge_imu_fusion_publisher = n.advertise(HEDGE_IMU_FUSION_TOPIC_NAME, 1000); + + ros::Publisher beacon_distance_publisher = n.advertise(BEACON_RAW_DISTANCE_TOPIC_NAME, 1000); + + + // 200 Hz + ros::Rate loop_rate(200); + + // default values for position message + hedge_pos_ang_msg.address= 0; + hedge_pos_ang_msg.timestamp_ms = 0; + hedge_pos_ang_msg.x_m = 0.0; + hedge_pos_ang_msg.y_m = 0.0; + hedge_pos_ang_msg.z_m = 0.0; + hedge_pos_ang_msg.flags = (1<<0);// 'data not available' flag + hedge_pos_ang_msg.angle= 0.0; + + hedge_pos_msg.address= 0; + hedge_pos_msg.timestamp_ms = 0; + hedge_pos_msg.x_m = 0.0; + hedge_pos_msg.y_m = 0.0; + hedge_pos_msg.z_m = 0.0; + hedge_pos_msg.flags = (1<<0);// 'data not available' flag + + hedge_pos_noaddress_msg.timestamp_ms = 0; + hedge_pos_noaddress_msg.x_m = 0.0; + hedge_pos_noaddress_msg.y_m = 0.0; + hedge_pos_noaddress_msg.z_m = 0.0; + hedge_pos_noaddress_msg.flags = (1<<0);// 'data not available' flag + + beacon_pos_msg.address= 0; + beacon_pos_msg.x_m = 0.0; + beacon_pos_msg.y_m = 0.0; + beacon_pos_msg.z_m = 0.0; + + + while (ros::ok()) + { + if (hedge->terminationRequired) + { + break; + } + + if (clock_gettime(CLOCK_REALTIME, &ts) == -1) + { + ROS_INFO("clock_gettime"); + return -1; + } + ts.tv_sec += 2; + sem_timedwait(sem,&ts); + + if (hedgeReceiveCheck()) + {// hedgehog data received + ROS_INFO("Address: %d, timestamp: %d, %d, X=%.3f Y= %.3f Z=%.3f Angle: %.1f flags=%d", + (int) hedge_pos_ang_msg.address, + (int) hedge_pos_ang_msg.timestamp_ms, + (int) (hedge_pos_ang_msg.timestamp_ms - hedge_timestamp_prev), + (float) hedge_pos_ang_msg.x_m, (float) hedge_pos_ang_msg.y_m, (float) hedge_pos_ang_msg.z_m, + (float) hedge_pos_ang_msg.angle, + (int) hedge_pos_msg.flags); + hedge_pos_ang_publisher.publish(hedge_pos_ang_msg); + hedge_pos_publisher.publish(hedge_pos_msg); + hedge_pos_noaddress_publisher.publish(hedge_pos_noaddress_msg); + + hedge_timestamp_prev= hedge_pos_ang_msg.timestamp_ms; + } + + beaconReadIterations= 0; + while(beaconReceiveCheck()) + {// stationary beacons data received + ROS_INFO("Stationary beacon: Address: %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + beacons_pos_publisher.publish(beacon_pos_msg); + + if ((beaconReadIterations++)>4) + break; + } + + if (hedgeIMURawReceiveCheck()) + { + ROS_INFO("Raw IMU: Timestamp: %08d, aX=%05d aY=%05d aZ=%05d gX=%05d gY=%05d gZ=%05d cX=%05d cY=%05d cZ=%05d", + (int) hedge_imu_raw_msg.timestamp_ms, + (int) hedge_imu_raw_msg.acc_x, (int) hedge_imu_raw_msg.acc_y, (int) hedge_imu_raw_msg.acc_z, + (int) hedge_imu_raw_msg.gyro_x, (int) hedge_imu_raw_msg.gyro_y, (int) hedge_imu_raw_msg.gyro_z, + (int) hedge_imu_raw_msg.compass_x, (int) hedge_imu_raw_msg.compass_y, (int) hedge_imu_raw_msg.compass_z); + hedge_imu_raw_publisher.publish(hedge_imu_raw_msg); + } + + if (hedgeIMUFusionReceiveCheck()) + { + ROS_INFO("IMU fusion: Timestamp: %08d, X=%.3f Y= %.3f Z=%.3f q=%.3f,%.3f,%.3f,%.3f v=%.3f,%.3f,%.3f a=%.3f,%.3f,%.3f", + (int) hedge_imu_fusion_msg.timestamp_ms, + (float) hedge_imu_fusion_msg.x_m, (float) hedge_imu_fusion_msg.y_m, (float) hedge_imu_fusion_msg.z_m, + (float) hedge_imu_fusion_msg.qw, (float) hedge_imu_fusion_msg.qx, (float) hedge_imu_fusion_msg.qy, (float) hedge_imu_fusion_msg.qz, + (float) hedge_imu_fusion_msg.vx, (float) hedge_imu_fusion_msg.vy, (float) hedge_imu_fusion_msg.vz, + (float) hedge_imu_fusion_msg.ax, (float) hedge_imu_fusion_msg.ay, (float) hedge_imu_fusion_msg.az); + hedge_imu_fusion_publisher.publish(hedge_imu_fusion_msg); + } + + rawDistanceReadIterations= 0; + if (hedge->rawDistances.updated) + { + for(uint8_t i=0;i<4;i++) + { + getRawDistance(i); + if (beacon_raw_distance_msg.address_beacon != 0) + { + ROS_INFO("Raw distance: %02d ==> %02d, Distance= %.3f ", + (int) beacon_raw_distance_msg.address_hedge, + (int) beacon_raw_distance_msg.address_beacon, + (float) beacon_raw_distance_msg.distance_m); + beacon_distance_publisher.publish(beacon_raw_distance_msg); + } + } + hedge->rawDistances.updated= false; + } + + ros::spinOnce(); + + loop_rate.sleep(); + } + + // Exit + if (hedge != NULL) + { + stopMarvelmindHedge (hedge); + destroyMarvelmindHedge (hedge); + } + + sem_close(sem); + + return 0; +} diff --git a/workspace/src/marvelmind_nav/src/marvelmind_hedge.c b/workspace/src/marvelmind_nav/src/marvelmind_hedge.c new file mode 100755 index 00000000..9fa339ec --- /dev/null +++ b/workspace/src/marvelmind_nav/src/marvelmind_hedge.c @@ -0,0 +1,1040 @@ +#include +#include +#include +#ifdef WIN32 +#include +#include +#else +#include +#include +#include +#include +#include +#endif // WIN32 +#include "marvelmind_nav/marvelmind_hedge.h" + +////////////////////////////////////////////////////////////////////////////// +// Calculate CRC (Modbus) for array of bytes +// buf: input buffer +// len: size of buffer +// returncode: CRC value +////////////////////////////////////////////////////////////////////////////// +uint16_t CalcCrcModbus_(uint8_t * buf, int len) +{ + uint16_t crc = 0xFFFF; + int pos; + for (pos = 0; pos < len; pos++) + { + crc ^= (uint16_t)buf[pos]; // XOR byte into least sig. byte of crc + int i; + for (i = 8; i != 0; i--) // Loop over each bit + { + if ((crc & 0x0001) != 0) // If the LSB is set + { + crc >>= 1; // Shift right and XOR 0xA001 + crc ^= 0xA001; + } + else // Else LSB is not set + crc >>= 1; // Just shift right + } + } + return crc; +} + +#ifdef WIN32 +#define SERIAL_PORT_HANDLE HANDLE +#define PORT_NOT_OPENED INVALID_HANDLE_VALUE +////////////////////////////////////////////////////////////////////////////// +// Open Serial Port (Windows only) +// portFileName: alias of port (e.g. "COM3"). Add prefix "\\\\.\\" before alias +// to open higher ports (e.g. COM12) +// baudrate: baudRate rate (e.g. 19200) +// verbose: print errors +// returncode: valid handle if port is successfully opened or +// INVALID_HANDLE_VALUE on error +////////////////////////////////////////////////////////////////////////////// +HANDLE OpenSerialPort_ (const char * portFileName, uint32_t baudrate, + bool verbose) +{ + HANDLE ttyHandle = CreateFile( TEXT(portFileName), GENERIC_READ, 0, + NULL, OPEN_EXISTING, 0, NULL); + if (ttyHandle==INVALID_HANDLE_VALUE) + { + if (verbose) + { + puts(portFileName); + puts ("Error: unable to open serial connection " + "(possibly serial port is not available)"); + } + return INVALID_HANDLE_VALUE; + } + COMMTIMEOUTS timeouts= {3000,3000,3000,3000,3000}; + bool returnCode=SetCommTimeouts (ttyHandle, &timeouts); + if (!returnCode) + { + if (verbose) puts ("Error: unable to set serial port timeouts"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + DCB dcb= {0}; + returnCode=GetCommState (ttyHandle, &dcb); + if (!returnCode) + { + if (verbose) puts ("Error: unable to get serial port parameters"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + dcb.BaudRate = baudrate; + dcb.fAbortOnError=true; + returnCode=SetCommState (ttyHandle, &dcb); + if (!returnCode) + { + if (verbose) puts ("Error: unable to set serial port parameters"); + CloseHandle (ttyHandle); + return INVALID_HANDLE_VALUE; + } + return ttyHandle; +} +#else +////////////////////////////////////////////////////////////////////////////// +// Converts baudrate value to baudRate code (Linux only) +// baudrate: value of baudRate rate (e.g. 19200) +// verbose: show errors +// returncode: code of baudRate rate (e.g. B19200) +////////////////////////////////////////////////////////////////////////////// +uint32_t _GetBaudCode (uint32_t baudrate, bool verbose) +{ + switch (baudrate) + { + case 50: + return B50; + case 75: + return B75; + case 110: + return B110; + case 134: + return B134; + case 150: + return B150; + case 200: + return B200; + case 300: + return B300; + case 600: + return B600; + case 1200: + return B1200; + case 1800: + return B1800; + case 2400: + return B2400; + case 4800: + return B4800; + case 9600: + return B9600; + case 19200: + return B19200; + case 38400: + return B38400; + case 57600: + return B57600; + case 115200: + return B115200; + case 230400: + return B230400; + case 460800: + return B460800; + case 500000: + return B500000; + case 576000: + return B576000; + case 921600: + return B921600; + case 1000000: + return B1000000; + case 1152000: + return B1152000; + default: + if (verbose) + printf ("Warning: unsupported baudrate %u. Using 9600.\n", + baudrate); + return B9600; + } +} + +#define SERIAL_PORT_HANDLE int +#define PORT_NOT_OPENED -1 +////////////////////////////////////////////////////////////////////////////// +// Open Serial Port (Linux only) +// portFileName: alias of port (e.g. "/dev/ttyACM0") +// baudrate: baudRate rate (e.g. 19200) +// verbose: show errors +// returncode: valid handle if port is successfully opened or -1 on error +////////////////////////////////////////////////////////////////////////////// +int OpenSerialPort_ (const char * portFileName, uint32_t baudrate, bool verbose) +{ + int ttyHandle = open(portFileName, O_RDWR| O_NONBLOCK | O_NDELAY ); + if (ttyHandle<0) + { + if (verbose) + { + puts(portFileName); + puts ("Error: unable to open serial connection " + "(possibly serial port is not available)"); + } + return -1; + } + struct termios ttyCtrl; + memset (&ttyCtrl, 0, sizeof ttyCtrl); + if ( tcgetattr ( ttyHandle, &ttyCtrl ) != 0 ) + { + if (verbose) puts ("Error: unable to get serial port parameters"); + return -1; + } + + uint32_t baudCode=_GetBaudCode(baudrate, verbose); + cfsetospeed (&ttyCtrl, baudCode); + cfsetispeed (&ttyCtrl, baudCode); + // 8N1, no flow control + ttyCtrl.c_cflag &= ~(PARENB|CSTOPB|CSIZE|CRTSCTS); + ttyCtrl.c_cflag |= CS8; + // no signaling chars, no echo, no canonical processing + ttyCtrl.c_lflag = 0; + ttyCtrl.c_oflag = 0; // no remapping, no delays + ttyCtrl.c_cc[VMIN] = 0; // read doesn't block + ttyCtrl.c_cc[VTIME] = 30; // 3 seconds read timeout + ttyCtrl.c_cflag |= CREAD | CLOCAL; // turn on READ & ignore ctrl lines + ttyCtrl.c_iflag &= ~(IXON | IXOFF | IXANY);// turn off s/w flow ctrl + ttyCtrl.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); // make raw + ttyCtrl.c_oflag &= ~OPOST; // make raw + tcflush(ttyHandle, TCIFLUSH ); // Flush port + if (tcsetattr (ttyHandle, TCSANOW, &ttyCtrl) != 0) + { + if (verbose) puts ("Error: unable to set serial port parameters"); + return -1; + } + return ttyHandle; +} +#endif + +////////////////////////////////////////////////////////////////////////// + +static int16_t get_int16(uint8_t *buffer) +{ + int16_t res= buffer[0] | + (((uint16_t ) buffer[1])<<8); + + return res; +} + +static uint32_t get_uint32(uint8_t *buffer) +{ + uint32_t res= buffer[0] | + (((uint32_t ) buffer[1])<<8) | + (((uint32_t ) buffer[2])<<16) | + (((uint32_t ) buffer[3])<<24); + + return res; +} + +static int32_t get_int32(uint8_t *buffer) +{ + int32_t res= buffer[0] | + (((uint32_t ) buffer[1])<<8) | + (((uint32_t ) buffer[2])<<16) | + (((uint32_t ) buffer[3])<<24); + + return res; +} + +////////////////////////////////////////////////////////////////////////// + +static uint8_t markPositionReady(struct MarvelmindHedge * hedge) +{uint8_t ind= hedge->lastValues_next; + uint8_t indCur= ind; + + hedge->positionBuffer[ind].ready= + true; + hedge->positionBuffer[ind].processed= + false; + ind++; + if (ind>=hedge->maxBufferedPositions) + ind=0; + if (hedge->lastValuesCount_maxBufferedPositions) + hedge->lastValuesCount_++; + hedge->haveNewValues_=true; + + hedge->lastValues_next= ind; + + return indCur; +} + +static struct PositionValue process_position_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[16]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int16_t vx= buffer[9] | + (((uint16_t ) buffer[10])<<8); + hedge->positionBuffer[ind].x= vx*10;// millimeters + + int16_t vy= buffer[11] | + (((uint16_t ) buffer[12])<<8); + hedge->positionBuffer[ind].y= vy*10;// millimeters + + int16_t vz= buffer[13] | + (((uint16_t ) buffer[14])<<8); + hedge->positionBuffer[ind].z= vz*10;// millimeters + + hedge->positionBuffer[ind].flags= buffer[15]; + + uint16_t vang= buffer[17] | + (((uint16_t ) buffer[18])<<8); + hedge->positionBuffer[ind].angle= ((float) (vang & 0x0fff))/10.0f; + + hedge->positionBuffer[ind].highResolution= false; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct PositionValue process_position_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t ind= hedge->lastValues_next; + + hedge->positionBuffer[ind].address= + buffer[22]; + hedge->positionBuffer[ind].timestamp= + buffer[5] | + (((uint32_t ) buffer[6])<<8) | + (((uint32_t ) buffer[7])<<16) | + (((uint32_t ) buffer[8])<<24); + + int32_t vx= buffer[9] | + (((uint32_t ) buffer[10])<<8) | + (((uint32_t ) buffer[11])<<16) | + (((uint32_t ) buffer[12])<<24); + hedge->positionBuffer[ind].x= vx; + + int32_t vy= buffer[13] | + (((uint32_t ) buffer[14])<<8) | + (((uint32_t ) buffer[15])<<16) | + (((uint32_t ) buffer[16])<<24); + hedge->positionBuffer[ind].y= vy; + + int32_t vz= buffer[17] | + (((uint32_t ) buffer[18])<<8) | + (((uint32_t ) buffer[19])<<16) | + (((uint32_t ) buffer[20])<<24); + hedge->positionBuffer[ind].z= vz; + + hedge->positionBuffer[ind].flags= buffer[21]; + + uint16_t vang= buffer[23] | + (((uint16_t ) buffer[24])<<8); + hedge->positionBuffer[ind].angle= ((float) (vang & 0x0fff))/10.0f; + + hedge->positionBuffer[ind].highResolution= true; + + ind= markPositionReady(hedge); + + return hedge->positionBuffer[ind]; +} + +static struct StationaryBeaconPosition *getOrAllocBeacon(struct MarvelmindHedge * hedge,uint8_t address) +{ + uint8_t i; + uint8_t n_used= hedge->positionsBeacons.numBeacons; + + if (n_used != 0) + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + return &hedge->positionsBeacons.beacons[i]; + } + } + + if (n_used >= (MAX_STATIONARY_BEACONS-1)) + return NULL; + + hedge->positionsBeacons.numBeacons= (n_used + 1); + return &hedge->positionsBeacons.beacons[n_used]; +} + +static void process_beacons_positions_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int16_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*8)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x*10;// millimeters + b->y= y*10;// millimeters + b->z= z*10;// millimeters + + b->highResolution= false; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + +static void process_beacons_positions_highres_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{ + uint8_t n= buffer[5];// number of beacons in packet + uint8_t i,ofs; + uint8_t address; + int32_t x,y,z; + struct StationaryBeaconPosition *b; + + if ((1+n*14)!=buffer[4]) + return;// incorrect size + + for(i=0;iaddress= address; + b->x= x; + b->y= y; + b->z= z; + + b->highResolution= true; + b->updatedForMsg= true; + + hedge->positionsBeacons.updated= true; + } + } +} + +static void process_imu_raw_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t *dataBuf= &buffer[5]; + + hedge->rawIMU.acc_x= get_int16(&dataBuf[0]); + hedge->rawIMU.acc_y= get_int16(&dataBuf[2]); + hedge->rawIMU.acc_z= get_int16(&dataBuf[4]); + + // + hedge->rawIMU.gyro_x= get_int16(&dataBuf[6]); + hedge->rawIMU.gyro_y= get_int16(&dataBuf[8]); + hedge->rawIMU.gyro_z= get_int16(&dataBuf[10]); + + // + hedge->rawIMU.compass_x= get_int16(&dataBuf[12]); + hedge->rawIMU.compass_y= get_int16(&dataBuf[14]); + hedge->rawIMU.compass_z= get_int16(&dataBuf[16]); + + hedge->rawIMU.timestamp= get_uint32(&dataBuf[24]); + + hedge->rawIMU.updated= true; +} + +static void process_imu_fusion_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t *dataBuf= &buffer[5]; + + hedge->fusionIMU.x= get_int32(&dataBuf[0]); + hedge->fusionIMU.y= get_int16(&dataBuf[4]); + hedge->fusionIMU.z= get_int16(&dataBuf[8]); + + hedge->fusionIMU.qw= get_int16(&dataBuf[12]); + hedge->fusionIMU.qx= get_int16(&dataBuf[14]); + hedge->fusionIMU.qy= get_int16(&dataBuf[16]); + hedge->fusionIMU.qz= get_int16(&dataBuf[18]); + + hedge->fusionIMU.vx= get_int16(&dataBuf[20]); + hedge->fusionIMU.vy= get_int16(&dataBuf[22]); + hedge->fusionIMU.vz= get_int16(&dataBuf[24]); + + hedge->fusionIMU.ax= get_int16(&dataBuf[26]); + hedge->fusionIMU.ay= get_int16(&dataBuf[28]); + hedge->fusionIMU.az= get_int16(&dataBuf[30]); + + hedge->fusionIMU.timestamp= get_uint32(&dataBuf[34]); + + hedge->fusionIMU.updated= true; +} + +static void process_raw_distances_datagram(struct MarvelmindHedge * hedge, uint8_t *buffer) +{uint8_t *dataBuf= &buffer[5]; + uint8_t ofs, i; + + hedge->rawDistances.address_hedge= dataBuf[0]; + + ofs= 1; + for(i=0;i<4;i++) + { + hedge->rawDistances.distances[i].address_beacon= dataBuf[ofs+0]; + hedge->rawDistances.distances[i].distance= get_uint32(&dataBuf[ofs+1]); + ofs+= 6; + } + + hedge->rawDistances.updated= true; +} + +//////////////////// + +enum +{ + RECV_HDR, + RECV_DGRAM +}; + +////////////////////////////////////////////////////////////////////////////// +// Thread function started by MarvelmindHedge_start +////////////////////////////////////////////////////////////////////////////// + +void +#ifndef WIN32 +* +#endif // WIN32 +Marvelmind_Thread_ (void* param) +{ + struct MarvelmindHedge * hedge=(struct MarvelmindHedge*) param; + struct PositionValue curPosition; + uint8_t input_buffer[256]; + uint8_t recvState=RECV_HDR; // current state of receive data + uint8_t nBytesInBlockReceived=0; // bytes received + uint16_t dataId; + #ifndef WIN32 + struct pollfd fds[1]; + int pollrc; + #endif + + SERIAL_PORT_HANDLE ttyHandle=OpenSerialPort_(hedge->ttyFileName, + hedge->baudRate, hedge->verbose); + if (ttyHandle==PORT_NOT_OPENED) hedge->terminationRequired=true; + else if (hedge->verbose) printf ("Opened serial port %s with baudrate %u\n", + hedge->ttyFileName, hedge->baudRate); + + while (hedge->terminationRequired==false) + { + uint8_t receivedChar; + bool readSuccessed=true; +#ifdef WIN32 + DWORD nBytesRead; + readSuccessed=ReadFile (ttyHandle, &receivedChar, 1, &nBytesRead, NULL); +#else + int32_t nBytesRead; + fds[0].fd = ttyHandle; + fds[0].events = POLLIN ; + pollrc = poll( fds, 1, 1000); + if (pollrc<=0) continue; + if ((fds[0].revents & POLLIN )==0) continue; + + nBytesRead=read(ttyHandle, &receivedChar, 1); + if (nBytesRead<0) readSuccessed=false; +#endif + if (nBytesRead && readSuccessed) + { + bool goodByte= false; + input_buffer[nBytesInBlockReceived]= receivedChar; + switch (recvState) + { + case RECV_HDR: + switch(nBytesInBlockReceived) + { + case 0: + goodByte= (receivedChar == 0xff); + break; + case 1: + goodByte= (receivedChar == 0x47); + break; + case 2: + goodByte= true; + break; + case 3: + dataId= (((uint16_t) receivedChar)<<8) + input_buffer[2]; + goodByte= (dataId == POSITION_DATAGRAM_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_ID) || + (dataId == POSITION_DATAGRAM_HIGHRES_ID) || + (dataId == BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID) || + (dataId == IMU_RAW_DATAGRAM_ID) || + (dataId == IMU_FUSION_DATAGRAM_ID) || + (dataId == BEACON_RAW_DISTANCE_DATAGRAM_ID); + break; + case 4: + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + goodByte= (receivedChar == 0x10); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + goodByte= true; + break; + case POSITION_DATAGRAM_HIGHRES_ID: + goodByte= (receivedChar == 0x16); + break; + case IMU_RAW_DATAGRAM_ID: + goodByte= (receivedChar == 0x20); + break; + case IMU_FUSION_DATAGRAM_ID: + goodByte= (receivedChar == 0x2a); + break; + case BEACON_RAW_DISTANCE_DATAGRAM_ID: + goodByte= (receivedChar == 0x20); + break; + } + if (goodByte) + recvState=RECV_DGRAM; + break; + } + if (goodByte) + { + // correct header byte + nBytesInBlockReceived++; + } + else + { + // ...or incorrect + recvState=RECV_HDR; + nBytesInBlockReceived=0; + } + break; + case RECV_DGRAM: + nBytesInBlockReceived++; + if (nBytesInBlockReceived>=7+input_buffer[4]) + { + // parse dgram + uint16_t blockCrc= + CalcCrcModbus_(input_buffer,nBytesInBlockReceived); + if (blockCrc==0) + { +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + switch(dataId ) + { + case POSITION_DATAGRAM_ID: + // add to positionBuffer + curPosition= process_position_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_ID: + process_beacons_positions_datagram(hedge, input_buffer); + break; + case POSITION_DATAGRAM_HIGHRES_ID: + // add to positionBuffer + curPosition= process_position_highres_datagram(hedge, input_buffer); + break; + case BEACONS_POSITIONS_DATAGRAM_HIGHRES_ID: + process_beacons_positions_highres_datagram(hedge, input_buffer); + break; + case IMU_RAW_DATAGRAM_ID: + process_imu_raw_datagram(hedge, input_buffer); + break; + case IMU_FUSION_DATAGRAM_ID: + process_imu_fusion_datagram(hedge, input_buffer); + break; + case BEACON_RAW_DISTANCE_DATAGRAM_ID: + process_raw_distances_datagram(hedge, input_buffer); + break; + } +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + // callback + + if (hedge->anyInputPacketCallback) + { + hedge->anyInputPacketCallback(); + } + + if (hedge->receiveDataCallback) + { + if (dataId == POSITION_DATAGRAM_ID) + { + hedge->receiveDataCallback (curPosition); + } + } + } + // and repeat + recvState=RECV_HDR; + nBytesInBlockReceived=0; + } + } + } + } +#ifndef WIN32 + return NULL; +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Create an initialize MarvelmindHedge structure +// returncode: pointer to structure on success or NULL on error +////////////////////////////////////////////////////////////////////////////// +struct MarvelmindHedge * createMarvelmindHedge () +{ + struct MarvelmindHedge * hedge=malloc (sizeof (struct MarvelmindHedge)); + if (hedge) + { + hedge->ttyFileName=DEFAULT_TTY_FILENAME; + hedge->baudRate=9600; + hedge->maxBufferedPositions=1; + hedge->positionBuffer=NULL; + hedge->verbose=false; + hedge->receiveDataCallback=NULL; + hedge->anyInputPacketCallback= NULL; + hedge->lastValuesCount_=0; + hedge->lastValues_next= 0; + hedge->haveNewValues_=false; + hedge->terminationRequired= false; + + hedge->rawIMU.updated= false; + hedge->fusionIMU.updated= false; + hedge->rawDistances.updated= false; +#ifdef WIN32 + InitializeCriticalSection(&hedge->lock_); +#else + pthread_mutex_init (&hedge->lock_, NULL); +#endif + } + else puts ("Not enough memory"); + return hedge; +} + +////////////////////////////////////////////////////////////////////////////// +// Initialize and start work thread +////////////////////////////////////////////////////////////////////////////// +void startMarvelmindHedge (struct MarvelmindHedge * hedge) +{uint8_t i; + + hedge->positionBuffer= + malloc(sizeof (struct PositionValue)*hedge->maxBufferedPositions); + if (hedge->positionBuffer==NULL) + { + if (hedge->verbose) puts ("Not enough memory"); + hedge->terminationRequired=true; + return; + } + for(i=0;imaxBufferedPositions;i++) + { + hedge->positionBuffer[i].ready= false; + hedge->positionBuffer[i].processed= false; + } + for(i=0;ipositionsBeacons.beacons[i].updatedForMsg= false; + } + hedge->positionsBeacons.numBeacons= 0; + hedge->positionsBeacons.updated= false; + +#ifdef WIN32 + _beginthread (Marvelmind_Thread_, 0, hedge); +#else + pthread_create (&hedge->thread_, NULL, Marvelmind_Thread_, hedge); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Write average position coordinates +// hedge: MarvelmindHedge structure +// position: pointer to PositionValue for write coordinates +// returncode: true if position is valid +////////////////////////////////////////////////////////////////////////////// + +static bool getPositionFromMarvelmindHedgeByAddress (struct MarvelmindHedge * hedge, + struct PositionValue * position, uint8_t address) +{ + uint8_t i; + int32_t avg_x=0, avg_y=0, avg_z=0; + double avg_ang= 0.0; + uint32_t max_timestamp=0; + bool position_valid; + bool highRes= false; + uint8_t flags= 0; +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + if (hedge->lastValuesCount_) + { + uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t nFound= 0; + if (hedge->lastValuesCount_lastValuesCount_; + for (i=0; ipositionBuffer[i].address != address) + continue; + if (!hedge->positionBuffer[i].ready) + continue; + if (hedge->positionBuffer[i].processed) + continue; + if (address == 0) + address= hedge->positionBuffer[i].address; + nFound++; + avg_x+=hedge->positionBuffer[i].x; + avg_y+=hedge->positionBuffer[i].y; + avg_z+=hedge->positionBuffer[i].z; + avg_ang+= hedge->positionBuffer[i].angle; + + flags= hedge->positionBuffer[i].flags; + if (flags&(1<<0)) + { + position_valid=false; + } + + if (hedge->positionBuffer[i].highResolution) + highRes= true; + hedge->positionBuffer[i].processed= true; + if (hedge->positionBuffer[i].timestamp>max_timestamp) + max_timestamp=hedge->positionBuffer[i].timestamp; + } + if (nFound != 0) + { + avg_x/=nFound; + avg_y/=nFound; + avg_z/=nFound; + avg_ang/=nFound; + position_valid=true; + } else + { + position_valid=false; + } + } + else position_valid=false; +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + if (!position_valid) + { + flags|= (1<<0);// coordiantes not available + } + position->address= address; + position->x=avg_x; + position->y=avg_y; + position->z=avg_z; + position->angle= avg_ang; + position->timestamp=max_timestamp; + position->ready= position_valid; + position->highResolution= highRes; + position->flags= flags; + return position_valid; +} + +bool getPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct PositionValue * position) +{ + return getPositionFromMarvelmindHedgeByAddress(hedge, position, 0); +}; + +////////////////////////////////////////////////////////////////////////////// +// Print average position coordinates +// onlyNew: print only new positions +////////////////////////////////////////////////////////////////////////////// +void printPositionFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew) +{uint8_t i,j; + double xm,ym,zm; + + if (hedge->haveNewValues_ || (!onlyNew)) + { + struct PositionValue position; + uint8_t real_values_count=hedge->maxBufferedPositions; + uint8_t addresses[real_values_count]; + uint8_t addressesNum= 0; + + for(i=0;ipositionBuffer[i].address; + bool alreadyProcessed= false; + if (addressesNum != 0) + for(j=0;jhaveNewValues_=false; + } + } +} + +////////////////////////////////////////////////////////////////////////////// +// Get positions of stationary beacons +// hedge: MarvelmindHedge structure +// positions: pointer to structure for write coordinates +////////////////////////////////////////////////////////////////////////////// + +bool getStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + struct StationaryBeaconsPositions * positions) +{ +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + *positions= hedge->positionsBeacons; + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif + + return true; +} + +void clearStationaryBeaconUpdatedFlag(struct MarvelmindHedge * hedge, uint8_t address) +{uint8_t i,n; + +#ifdef WIN32 + EnterCriticalSection(&hedge->lock_); +#else + pthread_mutex_lock (&hedge->lock_); +#endif + + n= hedge->positionsBeacons.numBeacons; + + for(i=0;ipositionsBeacons.beacons[i].address == address) + { + hedge->positionsBeacons.beacons[i].updatedForMsg= false; + } + } + +#ifdef WIN32 + LeaveCriticalSection(&hedge->lock_); +#else + pthread_mutex_unlock (&hedge->lock_); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Print stationary beacons positions +// onlyNew: print only new positions +////////////////////////////////////////////////////////////////////////////// +void printStationaryBeaconsPositionsFromMarvelmindHedge (struct MarvelmindHedge * hedge, + bool onlyNew) +{struct StationaryBeaconsPositions positions; + double xm,ym,zm; + + getStationaryBeaconsPositionsFromMarvelmindHedge(hedge, &positions); + + if (positions.updated || (!onlyNew)) + {uint8_t i; + uint8_t n= hedge->positionsBeacons.numBeacons; + struct StationaryBeaconPosition *b; + + for(i=0;ix)/1000.0; + ym= ((double) b->y)/1000.0; + zm= ((double) b->z)/1000.0; + if (positions.beacons[i].highResolution) + { + printf ("Stationary beacon: address: %d, X: %.3f, Y: %.3f, Z: %.3f \n", + b->address,xm, ym, zm); + } else + { + printf ("Stationary beacon: address: %d, X: %.2f, Y: %.2f, Z: %.2f \n", + b->address,xm, ym, zm); + } + } + + hedge->positionsBeacons.updated= false; + } +} + +////////////////////////////////////////////////////////////////////////////// +// Stop work thread +////////////////////////////////////////////////////////////////////////////// +void stopMarvelmindHedge (struct MarvelmindHedge * hedge) +{ + hedge->terminationRequired=true; + if (hedge->verbose) puts ("stopping"); +#ifdef WIN32 + WaitForSingleObject (hedge->thread_, INFINITE); +#else + pthread_join (hedge->thread_, NULL); +#endif +} + +////////////////////////////////////////////////////////////////////////////// +// Destroy structures to free memory (You must call stopMarvelmindHedge +// first) +////////////////////////////////////////////////////////////////////////////// +void destroyMarvelmindHedge (struct MarvelmindHedge * hedge) +{ + if (hedge->positionBuffer) free (hedge->positionBuffer); + free (hedge); +} diff --git a/workspace/src/marvelmind_nav/src/subscriber_test.cpp b/workspace/src/marvelmind_nav/src/subscriber_test.cpp new file mode 100755 index 00000000..a7fedcf8 --- /dev/null +++ b/workspace/src/marvelmind_nav/src/subscriber_test.cpp @@ -0,0 +1,219 @@ +#include "ros/ros.h" +#include "std_msgs/String.h" +#include "marvelmind_nav/hedge_pos.h" +#include "marvelmind_nav/hedge_pos_a.h" +#include "marvelmind_nav/hedge_pos_ang.h" +#include "marvelmind_nav/beacon_pos_a.h" +#include "marvelmind_nav/hedge_imu_raw.h" +#include "marvelmind_nav/hedge_imu_fusion.h" +#include "marvelmind_nav/beacon_distance.h" +#include + +#define ROS_NODE_NAME "subscriber_test" +#define HEDGE_POSITION_TOPIC_NAME "/hedge_pos" +#define HEDGE_POSITION_ADDRESSED_TOPIC_NAME "/hedge_pos_a" +#define HEDGE_POSITION_WITH_ANGLE_TOPIC_NAME "/hedge_pos_ang" + +#define BEACONS_POSITION_ADDRESSED_TOPIC_NAME "/beacons_pos_a" + +#define HEDGE_IMU_RAW_TOPIC_NAME "/hedge_imu_raw" +#define HEDGE_IMU_FUSION_TOPIC_NAME "/hedge_imu_fusion" + +#define BEACON_RAW_DISTANCE_TOPIC_NAME "/beacon_raw_distance" + +#define SCALE_HEDGE 3.0 + +ros::Publisher rviz_marker_pub; + +uint32_t rviz_shape; + +float orientation_qx= 0.0; +float orientation_qy= 0.0; +float orientation_qz= 0.0; +float orientation_qw= 1.0; + +typedef enum {objHedge, objBeacon} DrawObjectType; + +void showRvizObject(uint8_t address, float x, float y, float z, DrawObjectType obj) +{uint8_t lifeTime, i; + + if (rviz_marker_pub.getNumSubscribers() < 1) return; + + visualization_msgs::Marker marker; + // Set the frame ID and timestamp. See the TF tutorials for information on these. + marker.header.frame_id = "/my_frame"; + marker.header.stamp = ros::Time::now(); + + // Set the namespace and id for this marker. This serves to create a unique ID + // Any marker sent with the same namespace and id will overwrite the old one + marker.ns = "basic_shapes"; + marker.id = address; + + // Set the marker type + marker.type = rviz_shape; + + // Set the marker action. Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL) + marker.action = visualization_msgs::Marker::ADD; + + // Set the pose of the marker. This is a full 6DOF pose relative to the frame/time specified in the header + marker.pose.position.x = x; + marker.pose.position.y = y; + marker.pose.position.z = z; + if (obj == objHedge) + { + marker.pose.orientation.x = orientation_qx; + marker.pose.orientation.y = orientation_qy; + marker.pose.orientation.z = orientation_qz; + marker.pose.orientation.w = orientation_qw; + } + else + { + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.orientation.w = 1.0; + } + + // Set the scale of the marker -- 1x1x1 here means 1m on a side + marker.scale.x = 0.05*SCALE_HEDGE; + marker.scale.y = 0.05*SCALE_HEDGE; + marker.scale.z = 0.02*SCALE_HEDGE; + // Set the color -- be sure to set alpha to something non-zero! + if (obj == objHedge) + { + marker.color.r = 0.0f; + marker.color.g = 0.0f; + marker.color.b = 1.0f; + } + else + { + marker.color.r = 0.0f; + marker.color.g = 1.0f; + marker.color.b = 0.0f; + } + marker.color.a = 1.0; + + if (obj == objHedge) lifeTime= 5; + else lifeTime= 25; + marker.lifetime = ros::Duration(lifeTime); + + rviz_marker_pub.publish(marker); +} + +void hedgePosAngCallback(const marvelmind_nav::hedge_pos_ang& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: Address= %d, timestamp= %d, X=%.3f Y= %.3f Z=%.3f Angle: %.1f flags=%d", + (int) hedge_pos_msg.address, + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (float) hedge_pos_msg.angle, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(hedge_pos_msg.address,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); + } +} + +/* +void hedgePosCallback(const marvelmind_nav::hedge_pos_a& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: Address= %d, timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.address, + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(hedge_pos_msg.address,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); + } +} + +void hedgePos_noaddressCallback(const marvelmind_nav::hedge_pos& hedge_pos_msg) +{ + ROS_INFO("Hedgehog data: Timestamp= %d, X=%.3f Y= %.3f Z=%.3f flags=%d", + (int) hedge_pos_msg.timestamp_ms, + (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m, + (int) hedge_pos_msg.flags); + + if ((hedge_pos_msg.flags&(1<<0))==0) + { + showRvizObject(0,hedge_pos_msg.x_m, hedge_pos_msg.y_m, hedge_pos_msg.z_m, objHedge); + } +} +* */ + +void beaconsPosCallback(const marvelmind_nav::beacon_pos_a& beacon_pos_msg) +{ + ROS_INFO("Stationary beacon data: Address= %d, X=%.3f Y= %.3f Z=%.3f", + (int) beacon_pos_msg.address, + (float) beacon_pos_msg.x_m, (float) beacon_pos_msg.y_m, (float) beacon_pos_msg.z_m); + + showRvizObject(beacon_pos_msg.address, beacon_pos_msg.x_m, beacon_pos_msg.y_m, beacon_pos_msg.z_m, objBeacon); +} + + +void IMURawCallback(const marvelmind_nav::hedge_imu_raw& hedge_imu_raw_msg) +{ + ROS_INFO("Raw IMU: Timestamp: %08d, aX=%05d aY=%05d aZ=%05d gX=%05d gY=%05d gZ=%05d cX=%05d cY=%05d cZ=%05d", + (int) hedge_imu_raw_msg.timestamp_ms, + (int) hedge_imu_raw_msg.acc_x, (int) hedge_imu_raw_msg.acc_y, (int) hedge_imu_raw_msg.acc_z, + (int) hedge_imu_raw_msg.gyro_x, (int) hedge_imu_raw_msg.gyro_y, (int) hedge_imu_raw_msg.gyro_z, + (int) hedge_imu_raw_msg.compass_x, (int) hedge_imu_raw_msg.compass_y, (int) hedge_imu_raw_msg.compass_z); +} + +void IMUFusionCallback(const marvelmind_nav::hedge_imu_fusion& hedge_imu_fusion_msg) +{ + ROS_INFO("IMU fusion: Timestamp: %08d, X=%.3f Y= %.3f Z=%.3f q=%.3f,%.3f,%.3f,%.3f v=%.3f,%.3f,%.3f a=%.3f,%.3f,%.3f", + (int) hedge_imu_fusion_msg.timestamp_ms, + (float) hedge_imu_fusion_msg.x_m, (float) hedge_imu_fusion_msg.y_m, (float) hedge_imu_fusion_msg.z_m, + (float) hedge_imu_fusion_msg.qw, (float) hedge_imu_fusion_msg.qx, (float) hedge_imu_fusion_msg.qy, (float) hedge_imu_fusion_msg.qz, + (float) hedge_imu_fusion_msg.vx, (float) hedge_imu_fusion_msg.vy, (float) hedge_imu_fusion_msg.vz, + (float) hedge_imu_fusion_msg.ax, (float) hedge_imu_fusion_msg.ay, (float) hedge_imu_fusion_msg.az); + + orientation_qx= hedge_imu_fusion_msg.qx; + orientation_qy= hedge_imu_fusion_msg.qy; + orientation_qz= hedge_imu_fusion_msg.qz; + orientation_qw= hedge_imu_fusion_msg.qw; +} + +void RawDistanceCallback(const marvelmind_nav::beacon_distance& beacon_raw_distance_msg) +{ + ROS_INFO("Raw distance: %02d ==> %02d, Distance= %.3f ", + (int) beacon_raw_distance_msg.address_hedge, + (int) beacon_raw_distance_msg.address_beacon, + (float) beacon_raw_distance_msg.distance_m); +} + + +/** + * Test subscriber node for getting data from Marvelmind publishers nodes + */ +int main(int argc, char **argv) +{ + + // initialize ROS node + ros::init(argc, argv, ROS_NODE_NAME); + + // ROS node reference + ros::NodeHandle rosNode; + + // Declare need to subscribe data from topic + ros::Subscriber subHedgeWithAngle = rosNode.subscribe(HEDGE_POSITION_WITH_ANGLE_TOPIC_NAME, 1000, hedgePosAngCallback); + //ros::Subscriber subHedge = rosNode.subscribe(HEDGE_POSITION_ADDRESSED_TOPIC_NAME, 1000, hedgePosCallback); + //ros::Subscriber subHedge_noaddress = rosNode.subscribe(HEDGE_POSITION_TOPIC_NAME, 1000, hedgePos_noaddressCallback); + ros::Subscriber subBeacons = rosNode.subscribe(BEACONS_POSITION_ADDRESSED_TOPIC_NAME, 1000, beaconsPosCallback); + ros::Subscriber subIMURaw = rosNode.subscribe(HEDGE_IMU_RAW_TOPIC_NAME, 1000, IMURawCallback); + ros::Subscriber subIMUFusion = rosNode.subscribe(HEDGE_IMU_FUSION_TOPIC_NAME, 1000, IMUFusionCallback); + ros::Subscriber subRawDistance = rosNode.subscribe(BEACON_RAW_DISTANCE_TOPIC_NAME, 1000, RawDistanceCallback); + + // Declare publisher for rviz visualization + rviz_marker_pub = rosNode.advertise("visualization_marker", 1); + // Set our initial shape type to be a cube + rviz_shape = visualization_msgs::Marker::CUBE; + + ros::spin(); + + return 0; +} diff --git a/workspace/src/rqt_common_plugins/.gitignore b/workspace/src/rqt_common_plugins/.gitignore deleted file mode 100644 index 64b3b480..00000000 --- a/workspace/src/rqt_common_plugins/.gitignore +++ /dev/null @@ -1,92 +0,0 @@ -*.project -*.pydevproject -*.cproject - -# Created by https://www.gitignore.io - -### Python ### -# Byte-compiled / optimized / DLL files -__pycache__/ -*.py[cod] - -# C extensions -*.so - -# Distribution / packaging -.Python -env/ -build/ -develop-eggs/ -dist/ -downloads/ -eggs/ -lib/ -lib64/ -parts/ -sdist/ -var/ -*.egg-info/ -.installed.cfg -*.egg - -# PyInstaller -# Usually these files are written by a python script from a template -# before PyInstaller builds the exe, so as to inject date/other infos into it. -*.manifest -*.spec - -# Installer logs -pip-log.txt -pip-delete-this-directory.txt - -# Unit test / coverage reports -htmlcov/ -.tox/ -.coverage -.cache -nosetests.xml -coverage.xml - -# Translations -*.mo -*.pot - -# Django stuff: -*.log - -# Sphinx documentation -docs/_build/ - -# PyBuilder -target/ - - -### C++ ### -# Compiled Object files -*.slo -*.lo -*.o -*.obj - -# Precompiled Headers -*.gch -*.pch - -# Compiled Dynamic libraries -*.so -*.dylib -*.dll - -# Fortran module files -*.mod - -# Compiled Static libraries -*.lai -*.la -*.a -*.lib - -# Executables -*.exe -*.out -*.app diff --git a/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst deleted file mode 100644 index af24d752..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/CHANGELOG.rst +++ /dev/null @@ -1,71 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_action -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Now depends on rqt_msg to eliminate GUI files from this package -* Fix; IndexError: list index out of range (`#26 `_) -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt deleted file mode 100644 index 0a2ab5e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/CMakeLists.txt +++ /dev/null @@ -1,9 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_action) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_action/package.xml b/workspace/src/rqt_common_plugins/rqt_action/package.xml deleted file mode 100644 index 2d4f5682..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - rqt_action - 0.3.13 - rqt_action provides a feature to introspect all available ROS - action (from actionlib) types. By utilizing rqt_msg, the output format is - unified with it and rqt_srv. Note that the actions shown on this plugin - is the ones that are stored on your machine, not on the ROS core your rqt - instance connects to. - - Aaron Blasdel - BSD - - - http://ros.org/wiki/rqt_action - https://github.com/ros-visualization/rqt_common_plugins/rqt_action - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Isao Saito - - catkin - - - - rospy - rqt_msg - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_action/plugin.xml b/workspace/src/rqt_common_plugins/rqt_action/plugin.xml deleted file mode 100644 index 0ed78daa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/plugin.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - A Python GUI for browsing available ROS action types. - - - - - folder - Plugins related to ROS actions. - - - mail-read - A Python GUI for browsing available ROS action types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_action/setup.py b/workspace/src/rqt_common_plugins/rqt_action/setup.py deleted file mode 100644 index f18d1442..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_action'], - package_dir={'': 'src'}, -# scripts=['scripts/rqt_action'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/__init__.py b/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py b/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py deleted file mode 100644 index 12b0e060..00000000 --- a/workspace/src/rqt_common_plugins/rqt_action/src/rqt_action/action_plugin.py +++ /dev/null @@ -1,59 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from rqt_msg.messages_widget import MessagesWidget -from rqt_py_common import rosaction - - -class ActionPlugin(Plugin): - def __init__(self, context): - super(ActionPlugin, self).__init__(context) - self.setObjectName('Action') - self._widget = MessagesWidget(rosaction.MODE_ACTION) - self._widget.setWindowTitle('Action Type Browser') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst deleted file mode 100644 index b86d771e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/CHANGELOG.rst +++ /dev/null @@ -1,136 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_bag -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* RQT_BAG: Ensure monotonic clock publishing. - Due to parallelism issues, a message can be published - with a simulated timestamp in the past. This lead to - undesired behaviors when using TF for example. -* Contributors: lsouchet - -0.3.12 (2015-07-24) -------------------- -* Added step-by-step playback capability -* Contributors: Aaron Blasdel, sambrose - -0.3.11 (2015-04-30) -------------------- -* fix viewer plugin relocation issue (`#306 `_) - -0.3.10 (2014-10-01) -------------------- -* fix topic type retrieval for multiple bag files (`#279 `_) -* fix region_changed signal emission when no start/end stamps are set -* improve right-click menu -* improve popup management (`#280 `_) -* implement recording of topic subsets -* sort the list of topics -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix visibility with dark Qt theme (`#263 `_) - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use queue_size for Python publishers only when available (`#243 `_) -* use thread for loading bag files, emit region changed signal used by plotting plugin (`#239 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* fix closing and reopening topic views -* use queue_size for Python publishers - -0.3.5 (2014-05-07) ------------------- -* fix raw view not showing fields named 'msg' (`#226 `_) - -0.3.4 (2014-01-28) ------------------- -* add option to publish clock tim from bag (`#204 `_) - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* fix high cpu load when idle (`#194 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* update rqt_bag plugin interface to work with qt_gui_core 0.2.18 - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) -* fix shutdown of plugin (`#31 `_) -* fix saving parts of a bag (`#96 `_) -* fix long topic names (`#114 `_) -* fix zoom behavior (`#76 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- -* Fix; skips time when resuming playback (`#5 `_) -* Fix; timestamp printing issue (`#6 `_) - -0.2.8 (2013-01-11) ------------------- -* expose command line arguments to rqt_bag script -* added fix to set play/pause button correctly when fastforwarding/rewinding, adjusted time headers to 0m00s instead of 0:00m for ease of reading -* support passing bagfiles on the command line (currently behind --args) - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt deleted file mode 100644 index 8855342d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/CMakeLists.txt +++ /dev/null @@ -1,17 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_bag) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_bag - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox deleted file mode 100644 index f4005088..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_bag/package.xml b/workspace/src/rqt_common_plugins/rqt_bag/package.xml deleted file mode 100644 index 9192c18b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_bag - 0.3.13 - rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - Aaron Blasdel - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_bag - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - Tim Field - - catkin - - python-rospkg - rosbag - rosgraph_msgs - roslib - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml b/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml deleted file mode 100644 index 8ae63b9c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for displaying and replaying ROS bag files. - - - - - folder - Plugins related to logging. - - - applications-other - A Python GUI plugin for displaying and replaying ROS bag files. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui b/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui deleted file mode 100644 index 8aa56770..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/resource/bag_widget.ui +++ /dev/null @@ -1,439 +0,0 @@ - - - Bag - - - - 0 - 0 - 943 - 325 - - - - Bag - - - - 0 - - - 0 - - - - - - - Record Bag - - - - - - - 16 - 16 - - - - - - - - Load Bag - - - - - - - 16 - 16 - - - - - - - - Save Bag - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Go To Beginning - - - - - - - 16 - 16 - - - - - - - - Slower / Rewind - - - - - - - 16 - 16 - - - - - - - - Previous frame - - - - - - - 16 - 16 - - - - - - - - Play / Pause - - - - - - - 16 - 16 - - - - true - - - - - - - Next frame - - - - - - - 16 - 16 - - - - - - - - Fast Forward - - - - - - - 16 - 16 - - - - - - - - Go To End - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Zoom In - - - - - - - 16 - 16 - - - - - - - - Zoom Out - - - - - - - 16 - 16 - - - - - - - - Zoom Home - - - - - - - 16 - 16 - - - - - - - - Qt::Vertical - - - - - - - Toggle Thumbnails - - - - - - - 16 - 16 - - - - true - - - - - - - Qt::Vertical - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Qt::ScrollBarAlwaysOff - - - - - - - - - - 0 - 0 - - - - 0 - - - false - - - - - - - - 125 - 16777215 - - - - Time Stamp of the current playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 180 - 16777215 - - - - Date and time of the playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 0 - 0 - - - - - 110 - 16777215 - - - - Number of Seconds from the beginning of the current playhead - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - 80 - 16777215 - - - - QFrame::Panel - - - QFrame::Sunken - - - 2 - - - - - - - - - - - - - BagGraphicsView - QGraphicsView -
rqt_bag.bag_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag b/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag deleted file mode 100755 index c769a7a5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/scripts/rqt_bag +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_bag.bag import Bag -from rqt_gui.main import Main - -plugin = 'rqt_bag.bag.Bag' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin, plugin_argument_provider=Bag.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/setup.py b/workspace/src/rqt_common_plugins/rqt_bag/setup.py deleted file mode 100644 index ee444b82..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_bag', 'rqt_bag.plugins'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_bag'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py deleted file mode 100644 index 1e8f644a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/__init__.py +++ /dev/null @@ -1,37 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import bag_helper -from plugins.message_view import MessageView -from plugins.topic_message_view import TopicMessageView -from plugins.timeline_renderer import TimelineRenderer -from timeline_cache import TimelineCache diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py deleted file mode 100644 index be26e620..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag.py +++ /dev/null @@ -1,99 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import argparse -import threading - -from qt_gui.plugin import Plugin - -from .bag_widget import BagWidget - -class Bag(Plugin): - """ - Subclass of Plugin to provide interactive bag visualization, playing(publishing) and recording - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Bag, self).__init__(context) - self.setObjectName('Bag') - - args = self._parse_args(context.argv()) - - self._widget = BagWidget(context, args.clock) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def load_bags(): - for bagfile in args.bagfiles: - self._widget.load_bag(bagfile) - - load_thread = threading.Thread(target=load_bags) - load_thread.start() - - def _parse_args(self, argv): - parser = argparse.ArgumentParser(prog='rqt_bag', add_help=False) - Bag.add_arguments(parser) - return parser.parse_args(argv) - - @staticmethod - def _isfile(parser, arg): - if os.path.isfile(arg): - return arg - else: - parser.error("Bag file %s does not exist" % ( arg )) - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_bag plugin') - group.add_argument('--clock', action='store_true', help='publish the clock time') - group.add_argument('bagfiles', type=lambda x: Bag._isfile(parser, x), - nargs='*', default=[], help='Bagfiles to load') - - def shutdown_plugin(self): - self._widget.shutdown_all() - - def save_settings(self, plugin_settings, instance_settings): - # TODO implement saving - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # TODO implement restoring - # v = instance_settings.value(k) - pass - - #def trigger_configuration(self): - # TODO move some of the button functionality to config button if it is "more configy" diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py deleted file mode 100644 index c7ef7b87..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_helper.py +++ /dev/null @@ -1,127 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICTS -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Helper functions for bag files and timestamps. -""" - -import time -import rospy - - -def stamp_to_str(t): - """ - Convert a rospy.Time to a human-readable string. - - @param t: time to convert - @type t: rospy.Time - """ - t_sec = t.to_sec() - if t < rospy.Time.from_sec(60 * 60 * 24 * 365 * 5): - # Display timestamps earlier than 1975 as seconds - return '%.3fs' % t_sec - else: - return time.strftime('%b %d %Y %H:%M:%S', time.localtime(t_sec)) + '.%03d' % (t.nsecs / 1000000) - - -def get_topics(bag): - """ - Get an alphabetical list of all the unique topics in the bag. - - @return: sorted list of topics - @rtype: list of str - """ - return sorted(set([c.topic for c in bag._get_connections()])) - - -def get_start_stamp(bag): - """ - Get the earliest timestamp in the bag. - - @param bag: bag file - @type bag: rosbag.Bag - @return: earliest timestamp - @rtype: rospy.Time - """ - start_stamp = None - for connection_start_stamp in [index[0].time for index in bag._connection_indexes.values()]: - if not start_stamp or connection_start_stamp < start_stamp: - start_stamp = connection_start_stamp - return start_stamp - - -def get_end_stamp(bag): - """ - Get the latest timestamp in the bag. - - @param bag: bag file - @type bag: rosbag.Bag - @return: latest timestamp - @rtype: rospy.Time - """ - end_stamp = None - for connection_end_stamp in [index[-1].time for index in bag._connection_indexes.values()]: - if not end_stamp or connection_end_stamp > end_stamp: - end_stamp = connection_end_stamp - - return end_stamp - - -def get_topics_by_datatype(bag): - """ - Get all the message types in the bag and their associated topics. - - @param bag: bag file - @type bag: rosbag.Bag - @return: mapping from message typename to list of topics - @rtype: dict of str to list of str - """ - topics_by_datatype = {} - for c in bag._get_connections(): - topics_by_datatype.setdefault(c.datatype, []).append(c.topic) - - return topics_by_datatype - - -def get_datatype(bag, topic): - """ - Get the datatype of the given topic. - - @param bag: bag file - @type bag: rosbag.Bag - @return: message typename - @rtype: str - """ - for c in bag._get_connections(topic): - return c.datatype - - return None diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py deleted file mode 100644 index 2722097c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_timeline.py +++ /dev/null @@ -1,830 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy -import rosbag -import time -import threading - - -from python_qt_binding.QtCore import Qt, QTimer, qWarning, Signal -from python_qt_binding.QtGui import QGraphicsScene, QMessageBox - -import bag_helper - -from .timeline_frame import TimelineFrame -from .message_listener_thread import MessageListenerThread -from .message_loader_thread import MessageLoaderThread -from .player import Player -from .recorder import Recorder -from .timeline_menu import TimelinePopupMenu - - -class BagTimeline(QGraphicsScene): - """ - BagTimeline contains bag files, all information required to display the bag data visualization on the screen - Also handles events - """ - status_bar_changed_signal = Signal() - selected_region_changed = Signal(rospy.Time, rospy.Time) - - def __init__(self, context, publish_clock): - """ - :param context: plugin context hook to enable adding rqt_bag plugin widgets as ROS_GUI snapin panes, ''PluginContext'' - """ - super(BagTimeline, self).__init__() - self._bags = [] - self._bag_lock = threading.RLock() - - self.background_task = None # Display string - self.background_task_cancel = False - - # Playing / Recording - self._playhead_lock = threading.RLock() - self._max_play_speed = 1024.0 # fastest X play speed - self._min_play_speed = 1.0 / 1024.0 # slowest X play speed - self._play_speed = 0.0 - self._play_all = False - self._playhead_positions_cvs = {} - self._playhead_positions = {} # topic -> (bag, position) - self._message_loaders = {} - self._messages_cvs = {} - self._messages = {} # topic -> (bag, msg_data) - self._message_listener_threads = {} # listener -> MessageListenerThread - self._player = False - self._publish_clock = publish_clock - self._recorder = None - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - self.wrap = True # should the playhead wrap when it reaches the end? - self.stick_to_end = False # should the playhead stick to the end? - self._play_timer = QTimer() - self._play_timer.timeout.connect(self.on_idle) - self._play_timer.setInterval(3) - - # Plugin popup management - self._context = context - self.popups = {} - self._views = [] - self._listeners = {} - - # Initialize scene - # the timeline renderer fixes use of black pens and fills, so ensure we fix white here for contrast. - # otherwise a dark qt theme will default it to black and the frame render pen will be unreadable - self.setBackgroundBrush(Qt.white) - self._timeline_frame = TimelineFrame(self) - self._timeline_frame.setPos(0, 0) - self.addItem(self._timeline_frame) - - self.background_progress = 0 - self.__closed = False - - def get_context(self): - """ - :returns: the ROS_GUI context, 'PluginContext' - """ - return self._context - - def handle_close(self): - """ - Cleans up the timeline, bag and any threads - """ - if self.__closed: - return - else: - self.__closed = True - self._play_timer.stop() - for topic in self._get_topics(): - self.stop_publishing(topic) - self._message_loaders[topic].stop() - if self._player: - self._player.stop() - if self._recorder: - self._recorder.stop() - if self.background_task is not None: - self.background_task_cancel = True - self._timeline_frame.handle_close() - for bag in self._bags: - bag.close() - for frame in self._views: - if frame.parent(): - self._context.remove_widget(frame) - - # Bag Management and access - def add_bag(self, bag): - """ - creates an indexing thread for each new topic in the bag - fixes the boarders and notifies the indexing thread to index the new items bags - :param bag: ros bag file, ''rosbag.bag'' - """ - self._bags.append(bag) - - bag_topics = bag_helper.get_topics(bag) - - new_topics = set(bag_topics) - set(self._timeline_frame.topics) - - for topic in new_topics: - self._playhead_positions_cvs[topic] = threading.Condition() - self._messages_cvs[topic] = threading.Condition() - self._message_loaders[topic] = MessageLoaderThread(self, topic) - - self._timeline_frame._start_stamp = self._get_start_stamp() - self._timeline_frame._end_stamp = self._get_end_stamp() - self._timeline_frame.topics = self._get_topics() - self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() - # If this is the first bag, reset the timeline - if self._timeline_frame._stamp_left is None: - self._timeline_frame.reset_timeline() - - # Invalidate entire index cache for all topics in this bag - with self._timeline_frame.index_cache_cv: - for topic in bag_topics: - self._timeline_frame.invalidated_caches.add(topic) - if topic in self._timeline_frame.index_cache: - del self._timeline_frame.index_cache[topic] - - self._timeline_frame.index_cache_cv.notify() - - #TODO Rethink API and if these need to be visible - def _get_start_stamp(self): - """ - :return: first stamp in the bags, ''rospy.Time'' - """ - with self._bag_lock: - start_stamp = None - for bag in self._bags: - bag_start_stamp = bag_helper.get_start_stamp(bag) - if bag_start_stamp is not None and (start_stamp is None or bag_start_stamp < start_stamp): - start_stamp = bag_start_stamp - return start_stamp - - def _get_end_stamp(self): - """ - :return: last stamp in the bags, ''rospy.Time'' - """ - with self._bag_lock: - end_stamp = None - for bag in self._bags: - bag_end_stamp = bag_helper.get_end_stamp(bag) - if bag_end_stamp is not None and (end_stamp is None or bag_end_stamp > end_stamp): - end_stamp = bag_end_stamp - return end_stamp - - def _get_topics(self): - """ - :return: sorted list of topic names, ''list(str)'' - """ - with self._bag_lock: - topics = set() - for bag in self._bags: - for topic in bag_helper.get_topics(bag): - topics.add(topic) - return sorted(topics) - - def _get_topics_by_datatype(self): - """ - :return: dict of list of topics for each datatype, ''dict(datatype:list(topic))'' - """ - with self._bag_lock: - topics_by_datatype = {} - for bag in self._bags: - for datatype, topics in bag_helper.get_topics_by_datatype(bag).items(): - topics_by_datatype.setdefault(datatype, []).extend(topics) - return topics_by_datatype - - def get_datatype(self, topic): - """ - :return: datatype associated with a topic, ''str'' - :raises: if there are multiple datatypes assigned to a single topic, ''Exception'' - """ - with self._bag_lock: - datatype = None - for bag in self._bags: - bag_datatype = bag_helper.get_datatype(bag, topic) - if datatype and bag_datatype and (bag_datatype != datatype): - raise Exception('topic %s has multiple datatypes: %s and %s' % (topic, datatype, bag_datatype)) - if bag_datatype: - datatype = bag_datatype - return datatype - - def get_entries(self, topics, start_stamp, end_stamp): - """ - generator function for bag entries - :param topics: list of topics to query, ''list(str)'' - :param start_stamp: stamp to start at, ''rospy.Time'' - :param end_stamp: stamp to end at, ''rospy,Time'' - :returns: entries the bag file, ''msg'' - """ - with self._bag_lock: - from rosbag import bag # for _mergesort - bag_entries = [] - for b in self._bags: - bag_start_time = bag_helper.get_start_stamp(b) - if bag_start_time is not None and bag_start_time > end_stamp: - continue - - bag_end_time = bag_helper.get_end_stamp(b) - if bag_end_time is not None and bag_end_time < start_stamp: - continue - - connections = list(b._get_connections(topics)) - bag_entries.append(b._get_entries(connections, start_stamp, end_stamp)) - - for entry, _ in bag._mergesort(bag_entries, key=lambda entry: entry.time): - yield entry - - def get_entries_with_bags(self, topic, start_stamp, end_stamp): - """ - generator function for bag entries - :param topics: list of topics to query, ''list(str)'' - :param start_stamp: stamp to start at, ''rospy.Time'' - :param end_stamp: stamp to end at, ''rospy,Time'' - :returns: tuple of (bag, entry) for the entries in the bag file, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - from rosbag import bag # for _mergesort - - bag_entries = [] - bag_by_iter = {} - for b in self._bags: - bag_start_time = bag_helper.get_start_stamp(b) - if bag_start_time is not None and bag_start_time > end_stamp: - continue - - bag_end_time = bag_helper.get_end_stamp(b) - if bag_end_time is not None and bag_end_time < start_stamp: - continue - - connections = list(b._get_connections(topic)) - it = iter(b._get_entries(connections, start_stamp, end_stamp)) - bag_by_iter[it] = b - bag_entries.append(it) - - for entry, it in bag._mergesort(bag_entries, key=lambda entry: entry.time): - yield bag_by_iter[it], entry - - def get_entry(self, t, topic): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :param topic: the topic to be accessed, ''str'' - :return: tuple of (bag, entry) corisponding to time t and topic, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry(t, bag._get_connections(topic)) - if bag_entry and (not entry or bag_entry.time > entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_entry_before(self, t): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :return: tuple of (bag, entry) corresponding to time t, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry(t-rospy.Duration(0,1), bag._get_connections()) - if bag_entry and (not entry or bag_entry.time < entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_entry_after(self, t): - """ - Access a bag entry - :param t: time, ''rospy.Time'' - :return: tuple of (bag, entry) corisponding to time t, ''(rosbag.bag, msg)'' - """ - with self._bag_lock: - entry_bag, entry = None, None - for bag in self._bags: - bag_entry = bag._get_entry_after(t, bag._get_connections()) - if bag_entry and (not entry or bag_entry.time < entry.time): - entry_bag, entry = bag, bag_entry - - return entry_bag, entry - - def get_next_message_time(self): - """ - :return: time of the next message after the current playhead position,''rospy.Time'' - """ - if self._timeline_frame.playhead is None: - return None - - _, entry = self.get_entry_after(self._timeline_frame.playhead) - if entry is None: - return self._timeline_frame._start_stamp - - return entry.time - - def get_previous_message_time(self): - """ - :return: time of the next message before the current playhead position,''rospy.Time'' - """ - if self._timeline_frame.playhead is None: - return None - - _, entry = self.get_entry_before(self._timeline_frame.playhead) - if entry is None: - return self._timeline_frame._end_stamp - - return entry.time - - def resume(self): - if (self._player): - self._player.resume() - - ### Copy messages to... - - def start_background_task(self, background_task): - """ - Verify that a background task is not currently running before starting a new one - :param background_task: name of the background task, ''str'' - """ - if self.background_task is not None: - QMessageBox(QMessageBox.Warning, 'Exclamation', 'Background operation already running:\n\n%s' % self.background_task, QMessageBox.Ok).exec_() - return False - - self.background_task = background_task - self.background_task_cancel = False - return True - - def stop_background_task(self): - self.background_task = None - - def copy_region_to_bag(self, filename): - if len(self._bags) > 0: - self._export_region(filename, self._timeline_frame.topics, self._timeline_frame.play_region[0], self._timeline_frame.play_region[1]) - - def _export_region(self, path, topics, start_stamp, end_stamp): - """ - Starts a thread to save the current selection to a new bag file - :param path: filesystem path to write to, ''str'' - :param topics: topics to write to the file, ''list(str)'' - :param start_stamp: start of area to save, ''rospy.Time'' - :param end_stamp: end of area to save, ''rospy.Time'' - """ - if not self.start_background_task('Copying messages to "%s"' % path): - return - # TODO implement a status bar area with information on the current save status - bag_entries = list(self.get_entries_with_bags(topics, start_stamp, end_stamp)) - - if self.background_task_cancel: - return - - # Get the total number of messages to copy - total_messages = len(bag_entries) - - # If no messages, prompt the user and return - if total_messages == 0: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found', QMessageBox.Ok).exec_() - self.stop_background_task() - return - - # Open the path for writing - try: - export_bag = rosbag.Bag(path, 'w') - except Exception: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error opening bag file [%s] for writing' % path, QMessageBox.Ok).exec_() - self.stop_background_task() - return - - # Run copying in a background thread - self._export_thread = threading.Thread(target=self._run_export_region, args=(export_bag, topics, start_stamp, end_stamp, bag_entries)) - self._export_thread.start() - - def _run_export_region(self, export_bag, topics, start_stamp, end_stamp, bag_entries): - """ - Threaded function that saves the current selection to a new bag file - :param export_bag: bagfile to write to, ''rosbag.bag'' - :param topics: topics to write to the file, ''list(str)'' - :param start_stamp: start of area to save, ''rospy.Time'' - :param end_stamp: end of area to save, ''rospy.Time'' - """ - total_messages = len(bag_entries) - update_step = max(1, total_messages / 100) - message_num = 1 - progress = 0 - # Write out the messages - for bag, entry in bag_entries: - if self.background_task_cancel: - break - try: - topic, msg, t = self.read_message(bag, entry.position) - export_bag.write(topic, msg, t) - except Exception as ex: - qWarning('Error exporting message at position %s: %s' % (str(entry.position), str(ex))) - export_bag.close() - self.stop_background_task() - return - - if message_num % update_step == 0 or message_num == total_messages: - new_progress = int(100.0 * (float(message_num) / total_messages)) - if new_progress != progress: - progress = new_progress - if not self.background_task_cancel: - self.background_progress = progress - self.status_bar_changed_signal.emit() - - message_num += 1 - - # Close the bag - try: - self.background_progress = 0 - self.status_bar_changed_signal.emit() - export_bag.close() - except Exception as ex: - QMessageBox(QMessageBox.Warning, 'rqt_bag', 'Error closing bag file [%s]: %s' % (export_bag.filename, str(ex)), QMessageBox.Ok).exec_() - self.stop_background_task() - - def read_message(self, bag, position): - with self._bag_lock: - return bag._read_message(position) - - ### Mouse events - def on_mouse_down(self, event): - if event.buttons() == Qt.LeftButton: - self._timeline_frame.on_left_down(event) - elif event.buttons() == Qt.MidButton: - self._timeline_frame.on_middle_down(event) - elif event.buttons() == Qt.RightButton: - topic = self._timeline_frame.map_y_to_topic(event.y()) - TimelinePopupMenu(self, event, topic) - - def on_mouse_up(self, event): - self._timeline_frame.on_mouse_up(event) - - def on_mouse_move(self, event): - self._timeline_frame.on_mouse_move(event) - - def on_mousewheel(self, event): - self._timeline_frame.on_mousewheel(event) - - # Zooming - - def zoom_in(self): - self._timeline_frame.zoom_in() - - def zoom_out(self): - self._timeline_frame.zoom_out() - - def reset_zoom(self): - self._timeline_frame.reset_zoom() - - def translate_timeline_left(self): - self._timeline_frame.translate_timeline_left() - - def translate_timeline_right(self): - self._timeline_frame.translate_timeline_right() - - ### Publishing - def is_publishing(self, topic): - return self._player and self._player.is_publishing(topic) - - def start_publishing(self, topic): - if not self._player and not self._create_player(): - return False - - self._player.start_publishing(topic) - return True - - def stop_publishing(self, topic): - if not self._player: - return False - - self._player.stop_publishing(topic) - return True - - def _create_player(self): - if not self._player: - try: - self._player = Player(self) - if self._publish_clock: - self._player.start_clock_publishing() - except Exception as ex: - qWarning('Error starting player; aborting publish: %s' % str(ex)) - return False - - return True - - def set_publishing_state(self, start_publishing): - if start_publishing: - for topic in self._timeline_frame.topics: - if not self.start_publishing(topic): - break - else: - for topic in self._timeline_frame.topics: - self.stop_publishing(topic) - - # property: play_all - def _get_play_all(self): - return self._play_all - - def _set_play_all(self, play_all): - if play_all == self._play_all: - return - - self._play_all = not self._play_all - - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - - play_all = property(_get_play_all, _set_play_all) - - def toggle_play_all(self): - self.play_all = not self.play_all - - ### Playing - def on_idle(self): - self._step_playhead() - - def _step_playhead(self): - """ - moves the playhead to the next position based on the desired position - """ - # Reset when the playing mode switchs - if self._timeline_frame.playhead != self.last_playhead: - self.last_frame = None - self.last_playhead = None - self.desired_playhead = None - - if self._play_all: - self.step_next_message() - else: - self.step_fixed() - - def step_fixed(self): - """ - Moves the playhead a fixed distance into the future based on the current play speed - """ - if self.play_speed == 0.0 or not self._timeline_frame.playhead: - self.last_frame = None - self.last_playhead = None - return - - now = rospy.Time.from_sec(time.time()) - if self.last_frame: - # Get new playhead - if self.stick_to_end: - new_playhead = self.end_stamp - else: - new_playhead = self._timeline_frame.playhead + rospy.Duration.from_sec((now - self.last_frame).to_sec() * self.play_speed) - - start_stamp, end_stamp = self._timeline_frame.play_region - - if new_playhead > end_stamp: - if self.wrap: - if self.play_speed > 0.0: - new_playhead = start_stamp - else: - new_playhead = end_stamp - else: - new_playhead = end_stamp - - if self.play_speed > 0.0: - self.stick_to_end = True - - elif new_playhead < start_stamp: - if self.wrap: - if self.play_speed < 0.0: - new_playhead = end_stamp - else: - new_playhead = start_stamp - else: - new_playhead = start_stamp - - # Update the playhead - self._timeline_frame.playhead = new_playhead - - self.last_frame = now - self.last_playhead = self._timeline_frame.playhead - - def step_next_message(self): - """ - Move the playhead to the next message - """ - if self.play_speed <= 0.0 or not self._timeline_frame.playhead: - self.last_frame = None - self.last_playhead = None - return - - if self.last_frame: - if not self.desired_playhead: - self.desired_playhead = self._timeline_frame.playhead - else: - delta = rospy.Time.from_sec(time.time()) - self.last_frame - if delta > rospy.Duration.from_sec(0.1): - delta = rospy.Duration.from_sec(0.1) - self.desired_playhead += delta - - # Get the occurrence of the next message - next_message_time = self.get_next_message_time() - - if next_message_time < self.desired_playhead: - self._timeline_frame.playhead = next_message_time - else: - self._timeline_frame.playhead = self.desired_playhead - - self.last_frame = rospy.Time.from_sec(time.time()) - self.last_playhead = self._timeline_frame.playhead - - ### Recording - - def record_bag(self, filename, all=True, topics=[], regex=False, limit=0): - try: - self._recorder = Recorder(filename, bag_lock=self._bag_lock, all=all, topics=topics, regex=regex, limit=limit) - except Exception, ex: - qWarning('Error opening bag for recording [%s]: %s' % (filename, str(ex))) - return - - self._recorder.add_listener(self._message_recorded) - - self.add_bag(self._recorder.bag) - - self._recorder.start() - - self.wrap = False - self._timeline_frame._index_cache_thread.period = 0.1 - - self.update() - - def toggle_recording(self): - if self._recorder: - self._recorder.toggle_paused() - self.update() - - def _message_recorded(self, topic, msg, t): - if self._timeline_frame._start_stamp is None: - self._timeline_frame._start_stamp = t - self._timeline_frame._end_stamp = t - self._timeline_frame._playhead = t - elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp: - self._timeline_frame._end_stamp = t - - if not self._timeline_frame.topics or topic not in self._timeline_frame.topics: - self._timeline_frame.topics = self._get_topics() - self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype() - - self._playhead_positions_cvs[topic] = threading.Condition() - self._messages_cvs[topic] = threading.Condition() - self._message_loaders[topic] = MessageLoaderThread(self, topic) - - if self._timeline_frame._stamp_left is None: - self.reset_zoom() - - # Notify the index caching thread that it has work to do - with self._timeline_frame.index_cache_cv: - self._timeline_frame.invalidated_caches.add(topic) - self._timeline_frame.index_cache_cv.notify() - - if topic in self._listeners: - for listener in self._listeners[topic]: - try: - listener.timeline_changed() - except Exception, ex: - qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex))) - - ### Views / listeners - def add_view(self, topic, frame): - self._views.append(frame) - - def has_listeners(self, topic): - return topic in self._listeners - - def add_listener(self, topic, listener): - self._listeners.setdefault(topic, []).append(listener) - - self._message_listener_threads[(topic, listener)] = MessageListenerThread(self, topic, listener) - # Notify the message listeners - self._message_loaders[topic].reset() - with self._playhead_positions_cvs[topic]: - self._playhead_positions_cvs[topic].notify_all() - - self.update() - - def remove_listener(self, topic, listener): - topic_listeners = self._listeners.get(topic) - if topic_listeners is not None and listener in topic_listeners: - topic_listeners.remove(listener) - - if len(topic_listeners) == 0: - del self._listeners[topic] - - # Stop the message listener thread - if (topic, listener) in self._message_listener_threads: - self._message_listener_threads[(topic, listener)].stop() - del self._message_listener_threads[(topic, listener)] - self.update() - - ### Playhead - - # property: play_speed - def _get_play_speed(self): - if self._timeline_frame._paused: - return 0.0 - return self._play_speed - - def _set_play_speed(self, play_speed): - if play_speed == self._play_speed: - return - - if play_speed > 0.0: - self._play_speed = min(self._max_play_speed, max(self._min_play_speed, play_speed)) - elif play_speed < 0.0: - self._play_speed = max(-self._max_play_speed, min(-self._min_play_speed, play_speed)) - else: - self._play_speed = play_speed - - if self._play_speed < 1.0: - self.stick_to_end = False - - self.update() - play_speed = property(_get_play_speed, _set_play_speed) - - def toggle_play(self): - if self._play_speed != 0.0: - self.play_speed = 0.0 - else: - self.play_speed = 1.0 - - def navigate_play(self): - self.play_speed = 1.0 - self.last_frame = rospy.Time.from_sec(time.time()) - self.last_playhead = self._timeline_frame.playhead - self._play_timer.start() - - def navigate_stop(self): - self.play_speed = 0.0 - self._play_timer.stop() - - def navigate_previous(self): - self.navigate_stop() - self._timeline_frame.playhead = self.get_previous_message_time() - self.last_playhead = self._timeline_frame.playhead - - def navigate_next(self): - self.navigate_stop() - self._timeline_frame.playhead = self.get_next_message_time() - self.last_playhead = self._timeline_frame.playhead - - def navigate_rewind(self): - if self._play_speed < 0.0: - new_play_speed = self._play_speed * 2.0 - elif self._play_speed == 0.0: - new_play_speed = -1.0 - else: - new_play_speed = self._play_speed * 0.5 - - self.play_speed = new_play_speed - - def navigate_fastforward(self): - if self._play_speed > 0.0: - new_play_speed = self._play_speed * 2.0 - elif self._play_speed == 0.0: - new_play_speed = 2.0 - else: - new_play_speed = self._play_speed * 0.5 - - self.play_speed = new_play_speed - - def navigate_start(self): - self._timeline_frame.playhead = self._timeline_frame.play_region[0] - - def navigate_end(self): - self._timeline_frame.playhead = self._timeline_frame.play_region[1] diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py deleted file mode 100644 index 714abf1c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/bag_widget.py +++ /dev/null @@ -1,354 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import time - -import rospy -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QFileDialog, QGraphicsView, QIcon, QWidget - -import rosbag -import bag_helper -from .bag_timeline import BagTimeline -from .topic_selection import TopicSelection - - -class BagGraphicsView(QGraphicsView): - def __init__(self, parent=None): - super(BagGraphicsView, self).__init__() - - -class BagWidget(QWidget): - """ - Widget for use with Bag class to display and replay bag files - Handles all widget callbacks and contains the instance of BagTimeline for storing visualizing bag data - """ - - set_status_text = Signal(str) - - def __init__(self, context, publish_clock): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(BagWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_bag'), 'resource', 'bag_widget.ui') - loadUi(ui_file, self, {'BagGraphicsView': BagGraphicsView}) - - self.setObjectName('BagWidget') - - self._timeline = BagTimeline(context, publish_clock) - self.graphics_view.setScene(self._timeline) - - self.graphics_view.resizeEvent = self._resizeEvent - self.graphics_view.setMouseTracking(True) - - self.play_icon = QIcon.fromTheme('media-playback-start') - self.pause_icon = QIcon.fromTheme('media-playback-pause') - self.play_button.setIcon(self.play_icon) - self.begin_button.setIcon(QIcon.fromTheme('media-skip-backward')) - self.end_button.setIcon(QIcon.fromTheme('media-skip-forward')) - self.slower_button.setIcon(QIcon.fromTheme('media-seek-backward')) - self.faster_button.setIcon(QIcon.fromTheme('media-seek-forward')) - self.previous_button.setIcon(QIcon.fromTheme('go-previous')) - self.next_button.setIcon(QIcon.fromTheme('go-next')) - self.zoom_in_button.setIcon(QIcon.fromTheme('zoom-in')) - self.zoom_out_button.setIcon(QIcon.fromTheme('zoom-out')) - self.zoom_all_button.setIcon(QIcon.fromTheme('zoom-original')) - self.thumbs_button.setIcon(QIcon.fromTheme('insert-image')) - self.record_button.setIcon(QIcon.fromTheme('media-record')) - self.load_button.setIcon(QIcon.fromTheme('document-open')) - self.save_button.setIcon(QIcon.fromTheme('document-save')) - - self.play_button.clicked[bool].connect(self._handle_play_clicked) - self.thumbs_button.clicked[bool].connect(self._handle_thumbs_clicked) - self.zoom_in_button.clicked[bool].connect(self._handle_zoom_in_clicked) - self.zoom_out_button.clicked[bool].connect(self._handle_zoom_out_clicked) - self.zoom_all_button.clicked[bool].connect(self._handle_zoom_all_clicked) - self.previous_button.clicked[bool].connect(self._handle_previous_clicked) - self.next_button.clicked[bool].connect(self._handle_next_clicked) - self.faster_button.clicked[bool].connect(self._handle_faster_clicked) - self.slower_button.clicked[bool].connect(self._handle_slower_clicked) - self.begin_button.clicked[bool].connect(self._handle_begin_clicked) - self.end_button.clicked[bool].connect(self._handle_end_clicked) - self.record_button.clicked[bool].connect(self._handle_record_clicked) - self.load_button.clicked[bool].connect(self._handle_load_clicked) - self.save_button.clicked[bool].connect(self._handle_save_clicked) - self.graphics_view.mousePressEvent = self._timeline.on_mouse_down - self.graphics_view.mouseReleaseEvent = self._timeline.on_mouse_up - self.graphics_view.mouseMoveEvent = self._timeline.on_mouse_move - self.graphics_view.wheelEvent = self._timeline.on_mousewheel - self.closeEvent = self.handle_close - self.keyPressEvent = self.on_key_press - # TODO when the closeEvent is properly called by ROS_GUI implement that event instead of destroyed - self.destroyed.connect(self.handle_destroy) - - self.graphics_view.keyPressEvent = self.graphics_view_on_key_press - self.play_button.setEnabled(False) - self.thumbs_button.setEnabled(False) - self.zoom_in_button.setEnabled(False) - self.zoom_out_button.setEnabled(False) - self.zoom_all_button.setEnabled(False) - self.previous_button.setEnabled(False) - self.next_button.setEnabled(False) - self.faster_button.setEnabled(False) - self.slower_button.setEnabled(False) - self.begin_button.setEnabled(False) - self.end_button.setEnabled(False) - self.save_button.setEnabled(False) - - self._recording = False - - self._timeline.status_bar_changed_signal.connect(self._update_status_bar) - self.set_status_text.connect(self._set_status_text) - - def graphics_view_on_key_press(self, event): - key = event.key() - if key in (Qt.Key_Left, Qt.Key_Right, Qt.Key_Up, Qt.Key_Down, Qt.Key_PageUp, Qt.Key_PageDown): - # This causes the graphics view to ignore these keys so they can be caught by the bag_widget keyPressEvent - event.ignore() - else: - # Maintains functionality for all other keys QGraphicsView implements - QGraphicsView.keyPressEvent(self.graphics_view, event) - - # callbacks for ui events - def on_key_press(self, event): - key = event.key() - if key == Qt.Key_Space: - self._timeline.toggle_play() - elif key == Qt.Key_Home: - self._timeline.navigate_start() - elif key == Qt.Key_End: - self._handle_end_clicked() - elif key == Qt.Key_Plus or key == Qt.Key_Equal: - self._handle_faster_clicked() - elif key == Qt.Key_Minus: - self._handle_slower_clicked() - elif key == Qt.Key_Left: - self._timeline.translate_timeline_left() - elif key == Qt.Key_Right: - self._timeline.translate_timeline_right() - elif key == Qt.Key_Up or key == Qt.Key_PageUp: - self._handle_zoom_in_clicked() - elif key == Qt.Key_Down or key == Qt.Key_PageDown: - self._handle_zoom_out_clicked() - - def handle_destroy(self, args): - self._timeline.handle_close() - - def handle_close(self, event): - self.shutdown_all() - - event.accept() - - def _resizeEvent(self, event): - # TODO The -2 allows a buffer zone to make sure the scroll bars do not appear when not needed. On some systems (Lucid) this doesn't function properly - # need to look at a method to determine the maximum size of the scene that will maintain a proper no scrollbar fit in the view. - self.graphics_view.scene().setSceneRect(0, 0, self.graphics_view.width() - 2, max(self.graphics_view.height() - 2, self._timeline._timeline_frame._history_bottom)) - - def _handle_publish_clicked(self, checked): - self._timeline.set_publishing_state(checked) - - def _handle_play_clicked(self, checked): - if checked: - self.play_button.setIcon(self.pause_icon) - self._timeline.navigate_play() - else: - self.play_button.setIcon(self.play_icon) - self._timeline.navigate_stop() - - def _handle_next_clicked(self): - self._timeline.navigate_next() - self.play_button.setChecked(False) - self.play_button.setIcon(self.play_icon) - - def _handle_previous_clicked(self): - self._timeline.navigate_previous() - self.play_button.setChecked(False) - self.play_button.setIcon(self.play_icon) - - def _handle_faster_clicked(self): - self._timeline.navigate_fastforward() - self.play_button.setChecked(True) - self.play_button.setIcon(self.pause_icon) - - def _handle_slower_clicked(self): - self._timeline.navigate_rewind() - self.play_button.setChecked(True) - self.play_button.setIcon(self.pause_icon) - - def _handle_begin_clicked(self): - self._timeline.navigate_start() - - def _handle_end_clicked(self): - self._timeline.navigate_end() - - def _handle_thumbs_clicked(self, checked): - self._timeline._timeline_frame.toggle_renderers() - - def _handle_zoom_all_clicked(self): - self._timeline.reset_zoom() - - def _handle_zoom_out_clicked(self): - self._timeline.zoom_out() - - def _handle_zoom_in_clicked(self): - self._timeline.zoom_in() - - def _handle_record_clicked(self): - if self._recording: - self._timeline.toggle_recording() - return - - #TODO Implement limiting by regex and by number of messages per topic - self.topic_selection = TopicSelection() - self.topic_selection.recordSettingsSelected.connect(self._on_record_settings_selected) - - - def _on_record_settings_selected(self, all_topics, selected_topics): - # TODO verify master is still running - filename = QFileDialog.getSaveFileName(self, self.tr('Select prefix for new Bag File'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - prefix = filename[0].strip() - - # Get filename to record to - record_filename = time.strftime('%Y-%m-%d-%H-%M-%S.bag', time.localtime(time.time())) - if prefix.endswith('.bag'): - prefix = prefix[:-len('.bag')] - if prefix: - record_filename = '%s_%s' % (prefix, record_filename) - - rospy.loginfo('Recording to %s.' % record_filename) - - self.load_button.setEnabled(False) - self._recording = True - self._timeline.record_bag(record_filename, all_topics, selected_topics) - - - def _handle_load_clicked(self): - filename = QFileDialog.getOpenFileName(self, self.tr('Load from File'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - self.load_bag(filename[0]) - - def load_bag(self, filename): - qWarning("Loading %s" % filename) - - # QProgressBar can EITHER: show text or show a bouncing loading bar, - # but apparently the text is hidden when the bounding loading bar is - # shown - #self.progress_bar.setRange(0, 0) - self.set_status_text.emit("Loading %s" % filename) - #progress_format = self.progress_bar.format() - #progress_text_visible = self.progress_bar.isTextVisible() - #self.progress_bar.setFormat("Loading %s" % filename) - #self.progress_bar.setTextVisible(True) - - bag = rosbag.Bag(filename) - self.play_button.setEnabled(True) - self.thumbs_button.setEnabled(True) - self.zoom_in_button.setEnabled(True) - self.zoom_out_button.setEnabled(True) - self.zoom_all_button.setEnabled(True) - self.next_button.setEnabled(True) - self.previous_button.setEnabled(True) - self.faster_button.setEnabled(True) - self.slower_button.setEnabled(True) - self.begin_button.setEnabled(True) - self.end_button.setEnabled(True) - self.save_button.setEnabled(True) - self.record_button.setEnabled(False) - self._timeline.add_bag(bag) - qWarning("Done loading %s" % filename ) - # put the progress bar back the way it was - self.set_status_text.emit("") - #self.progress_bar.setFormat(progress_format) - #self.progress_bar.setTextVisible(progress_text_visible) # causes a segfault :( - #self.progress_bar.setRange(0, 100) - # self clear loading filename - - def _handle_save_clicked(self): - filename = QFileDialog.getSaveFileName(self, self.tr('Save selected region to file...'), '.', self.tr('Bag files {.bag} (*.bag)')) - if filename[0] != '': - self._timeline.copy_region_to_bag(filename[0]) - - def _set_status_text(self, text): - if text: - self.progress_bar.setFormat(text) - self.progress_bar.setTextVisible(True) - else: - self.progress_bar.setTextVisible(False) - - def _update_status_bar(self): - if self._timeline._timeline_frame.playhead is None or self._timeline._timeline_frame.start_stamp is None: - return - # TODO Figure out why this function is causing a "RuntimeError: wrapped C/C++ object of %S has been deleted" on close if the playhead is moving - try: - # Background Process Status - self.progress_bar.setValue(self._timeline.background_progress) - - # Raw timestamp - self.stamp_label.setText('%.3fs' % self._timeline._timeline_frame.playhead.to_sec()) - - # Human-readable time - self.date_label.setText(bag_helper.stamp_to_str(self._timeline._timeline_frame.playhead)) - - # Elapsed time (in seconds) - self.seconds_label.setText('%.3fs' % (self._timeline._timeline_frame.playhead - self._timeline._timeline_frame.start_stamp).to_sec()) - - # Play speed - spd = self._timeline.play_speed - if spd != 0.0: - if spd > 1.0: - spd_str = '>> %.0fx' % spd - elif spd == 1.0: - spd_str = '>' - elif spd > 0.0: - spd_str = '> 1/%.0fx' % (1.0 / spd) - elif spd > -1.0: - spd_str = '< 1/%.0fx' % (1.0 / -spd) - elif spd == 1.0: - spd_str = '<' - else: - spd_str = '<< %.0fx' % -spd - self.playspeed_label.setText(spd_str) - else: - self.playspeed_label.setText('') - except: - return - # Shutdown all members - - def shutdown_all(self): - self._timeline.handle_close() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py deleted file mode 100644 index 3c43de32..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/index_cache_thread.py +++ /dev/null @@ -1,86 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading -import time - - -class IndexCacheThread(threading.Thread): - """ - Updates invalid caches. - One thread per timeline. - """ - def __init__(self, timeline): - threading.Thread.__init__(self) - self.timeline = timeline - self._stop_flag = False - self.setDaemon(True) - self.start() - - def run(self): - while not self._stop_flag: - with self.timeline.index_cache_cv: - # Wait until the cache is dirty - while len(self.timeline.invalidated_caches) == 0: - self.timeline.index_cache_cv.wait() - if self._stop_flag: - return - # Update the index for one topic - total_topics = len(self.timeline.topics) - update_step = max(1, total_topics / 100) - topic_num = 1 - progress = 0 - updated = False - for topic in self.timeline.topics: - if topic in self.timeline.invalidated_caches: - updated = (self.timeline._update_index_cache(topic) > 0) - if topic_num % update_step == 0 or topic_num == total_topics: - new_progress = int(100.0 * (float(topic_num) / total_topics)) - if new_progress != progress: - progress = new_progress - if not self._stop_flag: - self.timeline.scene().background_progress = progress - self.timeline.scene().status_bar_changed_signal.emit() - topic_num += 1 - - if updated: - self.timeline.scene().background_progress = 0 - self.timeline.scene().status_bar_changed_signal.emit() - self.timeline.scene().update() - # Give the GUI some time to update - time.sleep(1.0) - - def stop(self): - self._stop_flag = True - cv = self.timeline.index_cache_cv - with cv: - cv.notify() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py deleted file mode 100644 index 371a0b1e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_listener_thread.py +++ /dev/null @@ -1,86 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading - -from python_qt_binding.QtCore import QCoreApplication, QEvent -from python_qt_binding.QtCore import qWarning - - -class ListenerEvent(QEvent): - def __init__(self, data): - super(ListenerEvent, self).__init__(1024) # userdefined event constant - self.data = data - - -class MessageListenerThread(threading.Thread): - """ - Waits for new messages loaded on the given topic, then calls the message listener. - One thread per listener, topic pair. - """ - def __init__(self, timeline, topic, listener): - threading.Thread.__init__(self) - - self.timeline = timeline - self.topic = topic - self.listener = listener - self.bag_msg_data = None - self._stop_flag = False - self.setDaemon(True) - self.start() - - def run(self): - """ - Thread body. loops and notifies the listener of new messages - """ - while not self._stop_flag: - # Wait for a new message - cv = self.timeline._messages_cvs[self.topic] - with cv: - while (self.topic not in self.timeline._messages) or (self.bag_msg_data == self.timeline._messages[self.topic]): - cv.wait() - if self._stop_flag: - return - bag_msg_data = self.timeline._messages[self.topic] - # View the message - self.bag_msg_data = bag_msg_data - try: - event = ListenerEvent(bag_msg_data) - QCoreApplication.postEvent(self.listener, event) - except Exception, ex: - qWarning('Error notifying listener %s: %s' % (type(self.listener), str(ex))) - - def stop(self): - self._stop_flag = True - cv = self.timeline._messages_cvs[self.topic] - with cv: - cv.notify_all() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py deleted file mode 100644 index 61e56ea9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/message_loader_thread.py +++ /dev/null @@ -1,112 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import threading - - -class MessageLoaderThread(threading.Thread): - """ - Waits for a new playhead position on the given topic, then loads the message at that position and notifies the view threads. - - One thread per topic. Maintains a cache of recently loaded messages. - """ - def __init__(self, timeline, topic): - threading.Thread.__init__(self) - - self.timeline = timeline - self.topic = topic - - self.bag_playhead_position = None - - self._message_cache_capacity = 50 - self._message_cache = {} - self._message_cache_keys = [] - - self._stop_flag = False - - self.setDaemon(True) - self.start() - - def reset(self): - self.bag_playhead_position = None - - def run(self): - while not self._stop_flag: - # Wait for a new entry - cv = self.timeline._playhead_positions_cvs[self.topic] - with cv: - while (self.topic not in self.timeline._playhead_positions) or (self.bag_playhead_position == self.timeline._playhead_positions[self.topic]): - cv.wait() - if self._stop_flag: - return - bag, playhead_position = self.timeline._playhead_positions[self.topic] - - self.bag_playhead_position = (bag, playhead_position) - - # Don't bother loading the message if there are no listeners - if not self.timeline.has_listeners(self.topic): - continue - - # Load the message - if playhead_position is None: - msg_data = None - else: - msg_data = self._get_message(bag, playhead_position) - - # Inform the views - messages_cv = self.timeline._messages_cvs[self.topic] - with messages_cv: - self.timeline._messages[self.topic] = (bag, msg_data) - messages_cv.notify_all() # notify all views that a message is loaded - - def _get_message(self, bag, position): - key = '%s%s' % (bag.filename, str(position)) - if key in self._message_cache: - return self._message_cache[key] - - msg_data = self.timeline.read_message(bag, position) - - self._message_cache[key] = msg_data - self._message_cache_keys.append(key) - - if len(self._message_cache) > self._message_cache_capacity: - oldest_key = self._message_cache_keys[0] - del self._message_cache[oldest_key] - self._message_cache_keys.remove(oldest_key) - - return msg_data - - def stop(self): - self._stop_flag = True - cv = self.timeline._playhead_positions_cvs[self.topic] - with cv: - cv.notify_all() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py deleted file mode 100644 index 06e6d6d6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/player.py +++ /dev/null @@ -1,150 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Player listens to messages from the timeline and publishes them to ROS. -""" - -import rospy -import rosgraph_msgs - -from python_qt_binding.QtCore import QObject - -CLOCK_TOPIC = "/clock" - -class Player(QObject): - """ - This object handles publishing messages as the playhead passes over their position - """ - def __init__(self, timeline): - super(Player, self).__init__() - self.timeline = timeline - - self._publishing = set() - self._publishers = {} - - self._publish_clock = False - self._last_clock = rosgraph_msgs.msg.Clock() - self._resume = False - - def resume(self): - self._resume = True - - def is_publishing(self, topic): - return topic in self._publishing - - def start_publishing(self, topic): - if topic in self._publishing: - return - self._publishing.add(topic) - self.timeline.add_listener(topic, self) - - def stop_publishing(self, topic): - if topic not in self._publishing: - return - self.timeline.remove_listener(topic, self) - - if topic in self._publishers: - self._publishers[topic].unregister() - del self._publishers[topic] - - self._publishing.remove(topic) - - def start_clock_publishing(self): - if CLOCK_TOPIC not in self._publishers: - # Activate clock publishing only if the publisher was created successful - self._publish_clock = self.create_publisher(CLOCK_TOPIC, rosgraph_msgs.msg.Clock()) - - def stop_clock_publishing(self): - self._publish_clock = False - if CLOCK_TOPIC in self._publishers: - self._publishers[CLOCK_TOPIC].unregister() - del self._publishers[CLOCK_TOPIC] - - def stop(self): - for topic in list(self._publishing): - self.stop_publishing(topic) - self.stop_clock_publishing() - - def create_publisher(self, topic, msg): - try: - try: - self._publishers[topic] = rospy.Publisher(topic, type(msg), queue_size=100) - except TypeError: - self._publishers[topic] = rospy.Publisher(topic, type(msg)) - return True - except Exception as ex: - # Any errors, stop listening/publishing to this topic - rospy.logerr('Error creating publisher on topic %s for type %s. \nError text: %s' % (topic, str(type(msg)), str(ex))) - if topic != CLOCK_TOPIC: - self.stop_publishing(topic) - return False - - def message_viewed(self, bag, msg_data): - """ - When a message is viewed publish it - :param bag: the bag the message is in, ''rosbag.bag'' - :param msg_data: tuple of the message data and topic info, ''(str, msg)'' - """ - # Don't publish unless the playhead is moving. - if self.timeline.play_speed <= 0.0: - return - - topic, msg, clock = msg_data - - # Create publisher if this is the first message on the topic - if topic not in self._publishers: - self.create_publisher(topic, msg) - - if self._publish_clock: - time_msg = rosgraph_msgs.msg.Clock() - time_msg.clock = clock - if self._resume or self._last_clock.clock < time_msg.clock: - self._resume = False - self._last_clock = time_msg - self._publishers[CLOCK_TOPIC].publish(time_msg) - self._publishers[topic].publish(msg) - - def message_cleared(self): - pass - - def event(self, event): - """ - This function will be called to process events posted by post_event - it will call message_cleared or message_viewed with the relevant data - """ - bag, msg_data = event.data - if msg_data: - self.message_viewed(bag, msg_data) - else: - self.message_cleared() - return True diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py deleted file mode 100644 index 15ed054e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/message_view.py +++ /dev/null @@ -1,95 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from python_qt_binding.QtCore import QObject - - -class MessageView(QObject): - """ - A message details renderer. When registered with rqt_bag, a MessageView is called - whenever the timeline playhead moves. - """ - name = 'Untitled' - - def __init__(self, timeline, topic): - super(MessageView, self).__init__() - self.timeline = timeline - self.topic = topic - - def message_viewed(self, bag, msg_details): - """ - View the message. - - @param bag: the bag file the message is contained in - @type bag: rosbag.Bag - @param msg_details: the details of the message to be viewed - @type msg_details: tuple (topic, msg, time) - @param topic: the message topic - @type topic: str - @param msg: the message - @param t: the message timestamp - @type t: rospy.Time - """ - pass - - def message_cleared(self): - """ - Clear the currently viewed message (if any). - """ - pass - - def timeline_changed(self): - """ - Called when the messages in a timeline change, e.g. if a new message is recorded, or - a bag file is added - """ - pass - - def close(self): - """ - Close the message view, releasing any resources. - """ - pass - -# NOTE: event function should not be changed in subclasses - def event(self, event): - """ - This function will be called to process events posted by post_event - it will call message_cleared or message_viewed with the relevant data - """ - bag, msg_data = event.data - if msg_data: - self.message_viewed(bag, msg_data) - else: - self.message_cleared() - return True diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py deleted file mode 100644 index d1a06183..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/plugin.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class Plugin(object): - - """ - Interface for rqt_bag plugins. - User-defined plugins may either subclass `rqt_bag.plugin.Plugin` or according to duck typing implement only the needed methods. - """ - - def __init__(self): - pass - - def get_view_class(self): - """Return a class which is a child of rqt_bag.plugin.topic_message_view.TopicMessageView.""" - raise NotImplementedError() - - def get_renderer_class(self): - """ - Return a class which is a child of rqt_bag.plugin.timeline_renderer.TimelineRenderer. - To omit the renderer component simply return None. - """ - return None - - def get_message_types(self): - """ - Return alist of message types which this plugin operates on. - To allow your plugin to be run on all message types return ['*']. - """ - return [] diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py deleted file mode 100644 index 210b0fa0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/raw_view.py +++ /dev/null @@ -1,231 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -""" -Defines a raw view: a TopicMessageView that displays the message contents in a tree. -""" -import rospy -import codecs -import math - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QApplication, QAbstractItemView, QSizePolicy, QTreeWidget, QTreeWidgetItem -from .topic_message_view import TopicMessageView - - -class RawView(TopicMessageView): - name = 'Raw' - """ - Plugin to view a message in a treeview window - The message is loaded into a custum treewidget - """ - def __init__(self, timeline, parent, topic): - """ - :param timeline: timeline data object, ''BagTimeline'' - :param parent: widget that will be added to the ros_gui context, ''QWidget'' - """ - super(RawView, self).__init__(timeline, parent, topic) - self.message_tree = MessageTree(parent) - parent.layout().addWidget(self.message_tree) # This will automatically resize the message_tree to the windowsize - - def message_viewed(self, bag, msg_details): - super(RawView, self).message_viewed(bag, msg_details) - _, msg, t = msg_details # topic, msg, t = msg_details - if t is None: - self.message_cleared() - else: - self.message_tree.set_message(msg) - - def message_cleared(self): - TopicMessageView.message_cleared(self) - self.message_tree.set_message(None) - - -class MessageTree(QTreeWidget): - def __init__(self, parent): - super(MessageTree, self).__init__(parent) - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.setHeaderHidden(True) - self.setSelectionMode(QAbstractItemView.ExtendedSelection) - self._msg = None - - self._expanded_paths = None - self.keyPressEvent = self.on_key_press - - @property - def msg(self): - return self._msg - - def set_message(self, msg): - """ - Clears the tree view and displays the new message - :param msg: message object to display in the treeview, ''msg'' - """ - # Remember whether items were expanded or not before deleting - if self._msg: - for item in self.get_all_items(): - path = self.get_item_path(item) - if item.isExpanded(): - self._expanded_paths.add(path) - elif path in self._expanded_paths: - self._expanded_paths.remove(path) - self.clear() - if msg: - # Populate the tree - self._add_msg_object(None, '', '', msg, msg._type) - - if self._expanded_paths is None: - self._expanded_paths = set() - else: - # Expand those that were previously expanded, and collapse any paths that we've seen for the first time - for item in self.get_all_items(): - path = self.get_item_path(item) - if path in self._expanded_paths: - item.setExpanded(True) - else: - item.setExpanded(False) - self._msg = msg - self.update() - - # Keyboard handler - def on_key_press(self, event): - key, ctrl = event.key(), event.modifiers() & Qt.ControlModifier - if ctrl: - if key == ord('C') or key == ord('c'): - # Ctrl-C: copy text from selected items to clipboard - self._copy_text_to_clipboard() - event.accept() - elif key == ord('A') or key == ord('a'): - # Ctrl-A: select all - self._select_all() - - def _select_all(self): - for i in self.get_all_items(): - if not i.isSelected(): - i.setSelected(True) - i.setExpanded(True) - - def _copy_text_to_clipboard(self): - # Get tab indented text for all selected items - def get_distance(item, ancestor, distance=0): - parent = item.parent() - if parent == None: - return distance - else: - return get_distance(parent, ancestor, distance + 1) - text = '' - for i in self.get_all_items(): - if i in self.selectedItems(): - text += ('\t' * (get_distance(i, None))) + i.text(0) + '\n' - # Copy the text to the clipboard - clipboard = QApplication.clipboard() - clipboard.setText(text) - - def get_item_path(self, item): - return item.data(0, Qt.UserRole)[0].replace(' ', '') # remove spaces that may get introduced in indexing, e.g. [ 3] is [3] - - def get_all_items(self): - items = [] - try: - root = self.invisibleRootItem() - self.traverse(root, items.append) - except Exception: - # TODO: very large messages can cause a stack overflow due to recursion - pass - return items - - def traverse(self, root, function): - for i in range(root.childCount()): - child = root.child(i) - function(child) - self.traverse(child, function) - - def _add_msg_object(self, parent, path, name, obj, obj_type): - label = name - - if hasattr(obj, '__slots__'): - subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__] - elif type(obj) in [list, tuple]: - len_obj = len(obj) - if len_obj == 0: - subobjs = [] - else: - w = int(math.ceil(math.log10(len_obj))) - subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)] - else: - subobjs = [] - - if type(obj) in [int, long, float]: - if type(obj) == float: - obj_repr = '%.6f' % obj - else: - obj_repr = str(obj) - - if obj_repr[0] == '-': - label += ': %s' % obj_repr - else: - label += ': %s' % obj_repr - - elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]: - # Ignore any binary data - obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0] - - # Truncate long representations - if len(obj_repr) >= 50: - obj_repr = obj_repr[:50] + '...' - - label += ': ' + obj_repr - item = QTreeWidgetItem([label]) - if name == '': - pass - elif path.find('.') == -1 and path.find('[') == -1: - self.addTopLevelItem(item) - else: - parent.addChild(item) - item.setData(0, Qt.UserRole, (path, obj_type)) - - for subobj_name, subobj in subobjs: - if subobj is None: - continue - - if path == '': - subpath = subobj_name # root field - elif subobj_name.startswith('['): - subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple - else: - subpath = '%s.%s' % (path, subobj_name) # attribute (prefix with '.') - - if hasattr(subobj, '_type'): - subobj_type = subobj._type - else: - subobj_type = type(subobj).__name__ - - self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py deleted file mode 100644 index b6cd91e3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/timeline_renderer.py +++ /dev/null @@ -1,79 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QObject - - -class TimelineRenderer(QObject): - """ - A custom renderer for interval of time of a topic on the timeline. - - @param msg_combine_px: don't draw discrete messages if they're less than this many pixels separated [default: 1.5] - @type msg_combine_px: float - """ - def __init__(self, timeline, msg_combine_px=1.5): - self.timeline = timeline - self.msg_combine_px = msg_combine_px - - def get_segment_height(self, topic): - """ - Get the height of the topic segment on the timeline. - - @param topic: topic name to draw - @type topic: str - @return: height in pixels of the topic segment. If none, the timeline default is used. - @rtype: int or None - """ - return None - - def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): - """ - Draw the timeline segment. - - @param painter: QPainter context to render into - @param topic: topic name - @param stamp_start: start of the interval on the timeline - @param stamp_end: start of the interval on the timeline - @param x: x coordinate of the timeline interval - @param y: y coordinate of the timeline interval - @param width: width in pixels of the timeline interval - @param height: height in pixels of the timeline interval - @return: whether the interval was renderered - @rtype: bool - """ - return False - - def close(self): - """ - Close the renderer, releasing any resources. - """ - pass diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py deleted file mode 100644 index 4fd62cf2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/plugins/topic_message_view.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -from .message_view import MessageView - -from python_qt_binding.QtGui import QAction, QIcon, QToolBar - - -class TopicMessageView(MessageView): - """ - A message view with a toolbar for navigating messages in a single topic. - """ - def __init__(self, timeline, parent, topic): - MessageView.__init__(self, timeline, topic) - - self._parent = parent - self._stamp = None - self._name = parent.objectName() - - self.toolbar = QToolBar() - self._first_action = QAction(QIcon.fromTheme('go-first'), '', self.toolbar) - self._first_action.triggered.connect(self.navigate_first) - self.toolbar.addAction(self._first_action) - self._prev_action = QAction(QIcon.fromTheme('go-previous'), '', self.toolbar) - self._prev_action.triggered.connect(self.navigate_previous) - self.toolbar.addAction(self._prev_action) - self._next_action = QAction(QIcon.fromTheme('go-next'), '', self.toolbar) - self._next_action.triggered.connect(self.navigate_next) - self.toolbar.addAction(self._next_action) - self._last_action = QAction(QIcon.fromTheme('go-last'), '', self.toolbar) - self._last_action.triggered.connect(self.navigate_last) - self.toolbar.addAction(self._last_action) - parent.layout().addWidget(self.toolbar) - - @property - def parent(self): - return self._parent - - @property - def stamp(self): - return self._stamp - - # MessageView implementation - - def message_viewed(self, bag, msg_details): - _, _, self._stamp = msg_details[:3] - - # Events - def navigate_first(self): - for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region): - self.timeline._timeline_frame.playhead = entry.time - break - - def navigate_previous(self): - last_entry = None - for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.start_stamp, self.timeline._timeline_frame.playhead): - if entry.time < self.timeline._timeline_frame.playhead: - last_entry = entry - - if last_entry: - self.timeline._timeline_frame.playhead = last_entry.time - - def navigate_next(self): - for entry in self.timeline.get_entries([self.topic], self.timeline._timeline_frame.playhead, self.timeline._timeline_frame.end_stamp): - if entry.time > self.timeline._timeline_frame.playhead: - self.timeline._timeline_frame.playhead = entry.time - break - - def navigate_last(self): - last_entry = None - for entry in self.timeline.get_entries([self.topic], *self.timeline._timeline_frame.play_region): - last_entry = entry - - if last_entry: - self.timeline._timeline_frame.playhead = last_entry.time diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py deleted file mode 100644 index 213db578..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/recorder.py +++ /dev/null @@ -1,247 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -""" -Recorder subscribes to ROS messages and writes them to a bag file. -""" - -import Queue -import re -import threading -import time - -import rosbag -import rosgraph -import roslib -import rospy - -import sys - - -class Recorder(object): - def __init__(self, filename, bag_lock=None, all=True, topics=[], regex=False, limit=0, master_check_interval=1.0): - """ - Subscribe to ROS messages and record them to a bag file. - - @param filename: filename of bag to write to - @type filename: str - @param all: all topics are to be recorded [default: True] - @type all: bool - @param topics: topics (or regexes if regex is True) to record [default: empty list] - @type topics: list of str - @param regex: topics should be considered as regular expressions [default: False] - @type regex: bool - @param limit: record only this number of messages on each topic (if non-positive, then unlimited) [default: 0] - @type limit: int - @param master_check_interval: period (in seconds) to check master for new topic publications [default: 1] - @type master_check_interval: float - """ - self._all = all - self._topics = topics - self._regex = regex - self._limit = limit - self._master_check_interval = master_check_interval - - self._bag = rosbag.Bag(filename, 'w') - self._bag_lock = bag_lock if bag_lock else threading.Lock() - self._listeners = [] - self._subscriber_helpers = {} - self._limited_topics = set() - self._failed_topics = set() - self._last_update = time.time() - self._write_queue = Queue.Queue() - self._paused = False - self._stop_condition = threading.Condition() - self._stop_flag = False - - # Compile regular expressions - if self._regex: - self._regexes = [re.compile(t) for t in self._topics] - else: - self._regexes = None - - self._message_count = {} # topic -> int (track number of messages recorded on each topic) - - self._master_check_thread = threading.Thread(target=self._run_master_check) - self._write_thread = threading.Thread(target=self._run_write) - - @property - def bag(self): - return self._bag - - def add_listener(self, listener): - """ - Add a listener which gets called whenever a message is recorded. - @param listener: function to call - @type listener: function taking (topic, message, time) - """ - self._listeners.append(listener) - - def start(self): - """ - Start subscribing and recording messages to bag. - """ - self._master_check_thread.start() - self._write_thread.start() - - @property - def paused(self): - return self._paused - - def pause(self): - self._paused = True - - def unpause(self): - self._paused = False - - def toggle_paused(self): - self._paused = not self._paused - - def stop(self): - """ - Stop recording. - """ - with self._stop_condition: - self._stop_flag = True - self._stop_condition.notify_all() - - self._write_queue.put(self) - - ## Implementation - - def _run_master_check(self): - master = rosgraph.Master('rqt_bag_recorder') - - try: - while not self._stop_flag: - # Check for new topics - for topic, datatype in master.getPublishedTopics(''): - # Check if: - # the topic is already subscribed to, or - # we've failed to subscribe to it already, or - # we've already reached the message limit, or - # we don't want to subscribe - if topic in self._subscriber_helpers or topic in self._failed_topics or topic in self._limited_topics or not self._should_subscribe_to(topic): - continue - - try: - pytype = roslib.message.get_message_class(datatype) - - self._message_count[topic] = 0 - - self._subscriber_helpers[topic] = _SubscriberHelper(self, topic, pytype) - except Exception, ex: - print >> sys.stderr, 'Error subscribing to %s (ignoring): %s' % (topic, str(ex)) - self._failed_topics.add(topic) - - # Wait a while - self._stop_condition.acquire() - self._stop_condition.wait(self._master_check_interval) - - except Exception, ex: - print >> sys.stderr, 'Error recording to bag: %s' % str(ex) - - # Unsubscribe from all topics - for topic in list(self._subscriber_helpers.keys()): - self._unsubscribe(topic) - - # Close the bag file so that the index gets written - try: - self._bag.close() - except Exception, ex: - print >> sys.stderr, 'Error closing bag [%s]: %s' % (self._bag.filename, str(ex)) - - def _should_subscribe_to(self, topic): - if self._all: - return True - - if not self._regex: - return topic in self._topics - - for regex in self._regexes: - if regex.match(topic): - return True - - return False - - def _unsubscribe(self, topic): - try: - self._subscriber_helpers[topic].subscriber.unregister() - except Exception: - return - - del self._subscriber_helpers[topic] - - def _record(self, topic, m): - if self._paused: - return - - if self._limit and self._message_count[topic] >= self._limit: - self._limited_topics.add(topic) - self._unsubscribe(topic) - return - - self._write_queue.put((topic, m, rospy.get_rostime())) - self._message_count[topic] += 1 - - def _run_write(self): - try: - while not self._stop_flag: - # Wait for a message - item = self._write_queue.get() - - if item == self: - continue - - topic, m, t = item - - # Write to the bag - with self._bag_lock: - self._bag.write(topic, m, t) - - # Notify listeners that a message has been recorded - for listener in self._listeners: - listener(topic, m, t) - - except Exception, ex: - print >> sys.stderr, 'Error write to bag: %s' % str(ex) - - -class _SubscriberHelper(object): - def __init__(self, recorder, topic, pytype): - self.recorder = recorder - self.topic = topic - - self.subscriber = rospy.Subscriber(self.topic, pytype, self.callback) - - def callback(self, m): - self.recorder._record(self.topic, m) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py deleted file mode 100644 index 169afec5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_cache.py +++ /dev/null @@ -1,177 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -import bisect -import Queue -import threading -import time - - -class TimelineCache(threading.Thread): - """ - Caches items for timeline renderers - """ - def __init__(self, loader, listener=None, max_cache_size=100): - threading.Thread.__init__(self) - - self.loader = loader - self.listener = listener - self.stop_flag = False - self.lock = threading.RLock() - self.items = {} # topic -> [(timestamp, items), ...] - self.last_accessed = {} # topic -> [(access time, timestamp), ...] - self.item_access = {} # topic -> timestamp -> access time - self.max_cache_size = max_cache_size # max number of items to cache (per topic) - self.queue = Queue.Queue() - self.setDaemon(True) - self.start() - - def run(self): - while not self.stop_flag: - # Get next item to load - entry = self.queue.get() - # self used to signal a change in stop_flag - if entry == self: - continue - # Check we haven't already cached it - topic, stamp, time_threshold, item_details = entry - - if not self.get_item(topic, stamp, time_threshold): - # Load the item - msg_stamp, item = self.loader(topic, stamp, item_details) - if item: - # Store in the cache - self.cache_item(topic, msg_stamp, item) - - if self.listener: - self.listener(topic, msg_stamp, item) -# else: -# try: -# qWarning('Failed to load:%s' % entry) -# except: -# qWarning('Failed to load cache item') - self.queue.task_done() - - def enqueue(self, entry): - self.queue.put(entry) - - def cache_item(self, topic, t, item): - with self.lock: - if topic not in self.items: - self.items[topic] = [] - topic_cache = self.items[topic] - - cache_entry = (t.to_sec(), item) - cache_index = bisect.bisect_right(topic_cache, cache_entry) - topic_cache.insert(cache_index, cache_entry) - - self._update_last_accessed(topic, t.to_sec()) - - self._limit_cache() - - def get_item(self, topic, stamp, time_threshold): - with self.lock: - # Attempt to get a item from the cache that's within time_threshold secs from stamp - topic_cache = self.items.get(topic) - if topic_cache: - cache_index = max(0, bisect.bisect_right(topic_cache, (stamp, None)) - 1) - - if cache_index <= len(topic_cache) - 1: - # Get cache entry before (or at) timestamp, and entry after - (cache_before_stamp, cache_before_item) = topic_cache[cache_index] - if cache_index < len(topic_cache) - 1: - cache_after_stamp, cache_after_item = topic_cache[cache_index + 1] - else: - cache_after_stamp = None - - # Find closest entry - cache_before_dist = abs(stamp - cache_before_stamp) - if cache_after_stamp: - cache_after_dist = abs(cache_after_stamp - stamp) - - if cache_after_stamp and cache_after_dist < cache_before_dist: - cache_dist, cache_stamp, cache_item = cache_after_dist, cache_after_stamp, cache_after_item - else: - cache_dist, cache_stamp, cache_item = cache_before_dist, cache_before_stamp, cache_before_item - - # Check entry is close enough - if cache_dist <= time_threshold: - self._update_last_accessed(topic, cache_stamp) - return cache_item - return None - - def _update_last_accessed(self, topic, stamp): - """ - Maintains a sorted list of cache accesses by timestamp for each topic. - """ - with self.lock: - access_time = time.time() - - if topic not in self.last_accessed: - self.last_accessed[topic] = [(access_time, stamp)] - self.item_access[topic] = {stamp: access_time} - return - - topic_last_accessed = self.last_accessed[topic] - topic_item_access = self.item_access[topic] - - if stamp in topic_item_access: - last_access = topic_item_access[stamp] - - index = bisect.bisect_left(topic_last_accessed, (last_access, None)) - assert(topic_last_accessed[index][1] == stamp) - - del topic_last_accessed[index] - - topic_last_accessed.append((access_time, stamp)) - topic_item_access[stamp] = access_time - - def _limit_cache(self): - """ - Removes LRU's from cache until size of each topic's cache is <= max_cache_size. - """ - with self.lock: - for topic, topic_cache in self.items.items(): - while len(topic_cache) > self.max_cache_size: - lru_stamp = self.last_accessed[topic][0][1] - - cache_index = bisect.bisect_left(topic_cache, (lru_stamp, None)) - assert(topic_cache[cache_index][0] == lru_stamp) - - del topic_cache[cache_index] - del self.last_accessed[topic][0] - del self.item_access[topic][lru_stamp] - - def stop(self): - self.stop_flag = True - self.queue.put(self) diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py deleted file mode 100644 index 9d090166..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_frame.py +++ /dev/null @@ -1,1175 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from python_qt_binding.QtCore import qDebug, QPointF, QRectF, Qt, qWarning, Signal -from python_qt_binding.QtGui import QBrush, QCursor, QColor, QFont, \ - QFontMetrics, QGraphicsItem, QPen, \ - QPolygonF -import rospy - -import bisect -import threading - -from .index_cache_thread import IndexCacheThread -from .plugins.raw_view import RawView - - -class _SelectionMode(object): - """ - SelectionMode states consolidated for readability - NONE = no region marked or started - LEFT_MARKED = one end of the region has been marked - MARKED = both ends of the region have been marked - SHIFTING = region is marked; currently dragging the region - MOVE_LEFT = region is marked; currently changing the left boundry of the selected region - MOVE_RIGHT = region is marked; currently changing the right boundry of the selected region - """ - NONE = 'none' - LEFT_MARKED = 'left marked' - MARKED = 'marked' - SHIFTING = 'shifting' - MOVE_LEFT = 'move left' - MOVE_RIGHT = 'move right' - - -class TimelineFrame(QGraphicsItem): - """ - TimelineFrame Draws the framing elements for the bag messages - (time delimiters, labels, topic names and backgrounds). - Also handles mouse callbacks since they interact closely with the drawn elements - """ - - def __init__(self, bag_timeline): - super(TimelineFrame, self).__init__() - self._bag_timeline = bag_timeline - self._clicked_pos = None - self._dragged_pos = None - - # Timeline boundries - self._start_stamp = None # earliest of all stamps - self._end_stamp = None # latest of all stamps - self._stamp_left = None # earliest currently visible timestamp on the timeline - self._stamp_right = None # latest currently visible timestamp on the timeline - self._history_top = 30 - self._history_left = 0 - self._history_width = 0 - self._history_bottom = 0 - self._history_bounds = {} - self._margin_left = 4 - self._margin_right = 20 - self._margin_bottom = 20 - self._history_top = 30 - - # Background Rendering - self._bag_end_color = QColor(0, 0, 0, 25) # color of background of timeline before first message and after last - self._history_background_color_alternate = QColor(179, 179, 179, 25) - self._history_background_color = QColor(204, 204, 204, 102) - - # Timeline Division Rendering - # Possible time intervals used between divisions - # 1ms, 5ms, 10ms, 50ms, 100ms, 500ms - # 1s, 5s, 15s, 30s - # 1m, 2m, 5m, 10m, 15m, 30m - # 1h, 2h, 3h, 6h, 12h - # 1d, 7d - self._sec_divisions = [0.001, 0.005, 0.01, 0.05, 0.1, 0.5, - 1, 5, 15, 30, - 1 * 60, 2 * 60, 5 * 60, 10 * 60, 15 * 60, 30 * 60, - 1 * 60 * 60, 2 * 60 * 60, 3 * 60 * 60, 6 * 60 * 60, 12 * 60 * 60, - 1 * 60 * 60 * 24, 7 * 60 * 60 * 24] - self._minor_spacing = 15 - self._major_spacing = 50 - self._major_divisions_label_indent = 3 # padding in px between line and label - self._major_division_pen = QPen(QBrush(Qt.black), 0, Qt.DashLine) - self._minor_division_pen = QPen(QBrush(QColor(153, 153, 153, 128)), 0, Qt.DashLine) - self._minor_division_tick_pen = QPen(QBrush(QColor(128, 128, 128, 128)), 0) - - # Topic Rendering - self.topics = [] - self._topics_by_datatype = {} - self._topic_font_height = None - self._topic_name_sizes = None - self._topic_name_spacing = 3 # minimum pixels between end of topic name and start of history - self._topic_font_size = 10.0 - self._topic_font = QFont("cairo") - self._topic_font.setPointSize(self._topic_font_size) - self._topic_font.setBold(False) - self._topic_vertical_padding = 4 - self._topic_name_max_percent = 25.0 # percentage of the horiz space that can be used for topic display - - # Time Rendering - self._time_tick_height = 5 - self._time_font_height = None - self._time_font_size = 10.0 - self._time_font = QFont("cairo") - self._time_font.setPointSize(self._time_font_size) - self._time_font.setBold(False) - - # Defaults - self._default_brush = QBrush(Qt.black, Qt.SolidPattern) - self._default_pen = QPen(Qt.black) - self._default_datatype_color = QColor(0, 0, 102, 204) - self._datatype_colors = { - 'sensor_msgs/CameraInfo': QColor(0, 0, 77, 204), - 'sensor_msgs/Image': QColor(0, 77, 77, 204), - 'sensor_msgs/LaserScan': QColor(153, 0, 0, 204), - 'pr2_msgs/LaserScannerSignal': QColor(153, 0, 0, 204), - 'pr2_mechanism_msgs/MechanismState': QColor(0, 153, 0, 204), - 'tf/tfMessage': QColor(0, 153, 0, 204), - } - self._default_msg_combine_px = 1.0 # minimum number of pixels allowed between two bag messages before they are combined - self._active_message_line_width = 3 - - # Selected Region Rendering - self._selected_region_color = QColor(0, 179, 0, 21) - self._selected_region_outline_top_color = QColor(0.0, 77, 0.0, 51) - self._selected_region_outline_ends_color = QColor(0.0, 77, 0.0, 102) - self._selecting_mode = _SelectionMode.NONE - self._selected_left = None - self._selected_right = None - self._selection_handle_width = 3.0 - - # Playhead Rendering - self._playhead = None # timestamp of the playhead - self._paused = False - self._playhead_pointer_size = (6, 6) - self._playhead_line_width = 1 - self._playhead_color = QColor(255, 0, 0, 191) - - # Zoom - self._zoom_sensitivity = 0.005 - self._min_zoom_speed = 0.5 - self._max_zoom_speed = 2.0 - self._min_zoom = 0.0001 # max zoom out (in px/s) - self._max_zoom = 50000.0 # max zoom in (in px/s) - - # Plugin management - self._viewer_types = {} - self._timeline_renderers = {} - self._rendered_topics = set() - self.load_plugins() - - # Bag indexer for rendering the default message views on the timeline - self.index_cache_cv = threading.Condition() - self.index_cache = {} - self.invalidated_caches = set() - self._index_cache_thread = IndexCacheThread(self) - - # TODO the API interface should exist entirely at the bag_timeline level. Add a "get_draw_parameters()" at the bag_timeline level to access these - # Properties, work in progress API for plugins: - - # property: playhead - def _get_playhead(self): - return self._playhead - - def _set_playhead(self, playhead): - """ - Sets the playhead to the new position, notifies the threads and updates the scene so it will redraw - :signal: emits status_bar_changed_signal if the playhead is successfully set - :param playhead: Time to set the playhead to, ''rospy.Time()'' - """ - with self.scene()._playhead_lock: - if playhead == self._playhead: - return - - self._playhead = playhead - if self._playhead != self._end_stamp: - self.scene().stick_to_end = False - - playhead_secs = playhead.to_sec() - if playhead_secs > self._stamp_right: - dstamp = playhead_secs - self._stamp_right + (self._stamp_right - self._stamp_left) * 0.75 - if dstamp > self._end_stamp.to_sec() - self._stamp_right: - dstamp = self._end_stamp.to_sec() - self._stamp_right - self.translate_timeline(dstamp) - - elif playhead_secs < self._stamp_left: - dstamp = self._stamp_left - playhead_secs + (self._stamp_right - self._stamp_left) * 0.75 - if dstamp > self._stamp_left - self._start_stamp.to_sec(): - dstamp = self._stamp_left - self._start_stamp.to_sec() - self.translate_timeline(-dstamp) - - # Update the playhead positions - for topic in self.topics: - bag, entry = self.scene().get_entry(self._playhead, topic) - if entry: - if topic in self.scene()._playhead_positions and self.scene()._playhead_positions[topic] == (bag, entry.position): - continue - new_playhead_position = (bag, entry.position) - else: - new_playhead_position = (None, None) - with self.scene()._playhead_positions_cvs[topic]: - self.scene()._playhead_positions[topic] = new_playhead_position - self.scene()._playhead_positions_cvs[topic].notify_all() # notify all message loaders that a new message needs to be loaded - self.scene().update() - self.scene().status_bar_changed_signal.emit() - - playhead = property(_get_playhead, _set_playhead) - - # TODO add more api variables here to allow plugin access - @property - def _history_right(self): - return self._history_left + self._history_width - - @property - def has_selected_region(self): - return self._selected_left is not None and self._selected_right is not None - - @property - def play_region(self): - if self.has_selected_region: - return (rospy.Time.from_sec(self._selected_left), rospy.Time.from_sec(self._selected_right)) - else: - return (self._start_stamp, self._end_stamp) - - def emit_play_region(self): - play_region = self.play_region - if(play_region[0] is not None and play_region[1] is not None): - self.scene().selected_region_changed.emit(*play_region) - - @property - def start_stamp(self): - return self._start_stamp - - @property - def end_stamp(self): - return self._end_stamp - - # QGraphicsItem implementation - def boundingRect(self): - return QRectF(0, 0, self._history_left + self._history_width + self._margin_right, self._history_bottom + self._margin_bottom) - - def paint(self, painter, option, widget): - if self._start_stamp is None: - return - - self._layout() - self._draw_topic_dividers(painter) - self._draw_selected_region(painter) - self._draw_time_divisions(painter) - self._draw_topic_histories(painter) - self._draw_bag_ends(painter) - self._draw_topic_names(painter) - self._draw_history_border(painter) - self._draw_playhead(painter) - # END QGraphicsItem implementation - - # Drawing Functions - - def _qfont_width(self, name): - return QFontMetrics(self._topic_font).width(name) - - def _trimmed_topic_name(self, topic_name): - """ - This function trims the topic name down to a reasonable percentage of the viewable scene area - """ - allowed_width = self._scene_width * (self._topic_name_max_percent / 100.0) - allowed_width = allowed_width - self._topic_name_spacing - self._margin_left - trimmed_return = topic_name - if allowed_width < self._qfont_width(topic_name): - # We need to trim the topic - trimmed = '' - split_name = topic_name.split('/') - split_name = filter(lambda a: a != '', split_name) - # Save important last element of topic name provided it is small - popped_last = False - if self._qfont_width(split_name[-1]) < .5 * allowed_width: - popped_last = True - last_item = split_name[-1] - split_name = split_name[:-1] - allowed_width = allowed_width - self._qfont_width(last_item) - # Shorten and add remaining items keeping lenths roughly equal - for item in split_name: - if self._qfont_width(item) > allowed_width / float(len(split_name)): - trimmed_item = item[:-3] + '..' - while self._qfont_width(trimmed_item) > allowed_width / float(len(split_name)): - if len(trimmed_item) >= 3: - trimmed_item = trimmed_item[:-3] + '..' - else: - break - trimmed = trimmed + '/' + trimmed_item - else: - trimmed = trimmed + '/' + item - if popped_last: - trimmed = trimmed + '/' + last_item - trimmed = trimmed[1:] - trimmed_return = trimmed - return trimmed_return - - def _layout(self): - """ - Recalculates the layout of the of the timeline to take into account any changes that have occured - """ - # Calculate history left and history width - self._scene_width = self.scene().views()[0].size().width() - - max_topic_name_width = -1 - for topic in self.topics: - topic_width = self._qfont_width(self._trimmed_topic_name(topic)) - if max_topic_name_width <= topic_width: - max_topic_name_width = topic_width - - # Calculate font height for each topic - self._topic_font_height = -1 - for topic in self.topics: - topic_height = QFontMetrics(self._topic_font).height() - if self._topic_font_height <= topic_height: - self._topic_font_height = topic_height - - # Update the timeline boundries - new_history_left = self._margin_left + max_topic_name_width + self._topic_name_spacing - new_history_width = self._scene_width - new_history_left - self._margin_right - self._history_left = new_history_left - self._history_width = new_history_width - - # Calculate the bounds for each topic - self._history_bounds = {} - y = self._history_top - for topic in self.topics: - datatype = self.scene().get_datatype(topic) - - topic_height = None - if topic in self._rendered_topics: - renderer = self._timeline_renderers.get(datatype) - if renderer: - topic_height = renderer.get_segment_height(topic) - if not topic_height: - topic_height = self._topic_font_height + self._topic_vertical_padding - - self._history_bounds[topic] = (self._history_left, y, self._history_width, topic_height) - - y += topic_height - -# new_history_bottom = max([y + h for (x, y, w, h) in self._history_bounds.values()]) - 1 - new_history_bottom = max([y + h for (_, y, _, h) in self._history_bounds.values()]) - 1 - if new_history_bottom != self._history_bottom: - self._history_bottom = new_history_bottom - - def _draw_topic_histories(self, painter): - """ - Draw all topic messages - :param painter: allows access to paint functions,''QPainter'' - """ - for topic in sorted(self._history_bounds.keys()): - self._draw_topic_history(painter, topic) - - def _draw_topic_history(self, painter, topic): - """ - Draw boxes corrisponding to message regions on the timeline. - :param painter: allows access to paint functions,''QPainter'' - :param topic: the topic for which message boxes should be drawn, ''str'' - """ - -# x, y, w, h = self._history_bounds[topic] - _, y, _, h = self._history_bounds[topic] - - msg_y = y + 2 - msg_height = h - 2 - - datatype = self.scene().get_datatype(topic) - - # Get the renderer and the message combine interval - renderer = None - msg_combine_interval = None - if topic in self._rendered_topics: - renderer = self._timeline_renderers.get(datatype) - if not renderer is None: - msg_combine_interval = self.map_dx_to_dstamp(renderer.msg_combine_px) - if msg_combine_interval is None: - msg_combine_interval = self.map_dx_to_dstamp(self._default_msg_combine_px) - - # Get the cache - if topic not in self.index_cache: - return - all_stamps = self.index_cache[topic] - -# start_index = bisect.bisect_left(all_stamps, self._stamp_left) - end_index = bisect.bisect_left(all_stamps, self._stamp_right) - # Set pen based on datatype - datatype_color = self._datatype_colors.get(datatype, self._default_datatype_color) - # Iterate through regions of connected messages - width_interval = self._history_width / (self._stamp_right - self._stamp_left) - - # Draw stamps - for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], self.map_dx_to_dstamp(self._default_msg_combine_px)): - if stamp_end < self._stamp_left: - continue - - region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval - if region_x_start < self._history_left: - region_x_start = self._history_left # Clip the region - region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval - region_width = max(1, region_x_end - region_x_start) - - painter.setBrush(QBrush(datatype_color)) - painter.setPen(QPen(datatype_color, 1)) - painter.drawRect(region_x_start, msg_y, region_width, msg_height) - - # Draw active message - if topic in self.scene()._listeners: - curpen = painter.pen() - oldwidth = curpen.width() - curpen.setWidth(self._active_message_line_width) - painter.setPen(curpen) - playhead_stamp = None - playhead_index = bisect.bisect_right(all_stamps, self.playhead.to_sec()) - 1 - if playhead_index >= 0: - playhead_stamp = all_stamps[playhead_index] - if playhead_stamp > self._stamp_left and playhead_stamp < self._stamp_right: - playhead_x = self._history_left + (all_stamps[playhead_index] - self._stamp_left) * width_interval - painter.drawLine(playhead_x, msg_y, playhead_x, msg_y + msg_height) - curpen.setWidth(oldwidth) - painter.setPen(curpen) - - # Custom renderer - if renderer: - # Iterate through regions of connected messages - for (stamp_start, stamp_end) in self._find_regions(all_stamps[:end_index], msg_combine_interval): - if stamp_end < self._stamp_left: - continue - - region_x_start = self._history_left + (stamp_start - self._stamp_left) * width_interval - region_x_end = self._history_left + (stamp_end - self._stamp_left) * width_interval - region_width = max(1, region_x_end - region_x_start) - renderer.draw_timeline_segment(painter, topic, stamp_start, stamp_end, region_x_start, msg_y, region_width, msg_height) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_bag_ends(self, painter): - """ - Draw markers to indicate the area the bag file represents within the current visible area. - :param painter: allows access to paint functions,''QPainter'' - """ - x_start, x_end = self.map_stamp_to_x(self._start_stamp.to_sec()), self.map_stamp_to_x(self._end_stamp.to_sec()) - painter.setBrush(QBrush(self._bag_end_color)) - painter.drawRect(self._history_left, self._history_top, x_start - self._history_left, self._history_bottom - self._history_top) - painter.drawRect(x_end, self._history_top, self._history_left + self._history_width - x_end, self._history_bottom - self._history_top) - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_topic_dividers(self, painter): - """ - Draws horizontal lines between each topic to visually separate the messages - :param painter: allows access to paint functions,''QPainter'' - """ - clip_left = self._history_left - clip_right = self._history_left + self._history_width - - row = 0 - for topic in self.topics: - (x, y, w, h) = self._history_bounds[topic] - - if row % 2 == 0: - painter.setPen(Qt.lightGray) - painter.setBrush(QBrush(self._history_background_color_alternate)) - else: - painter.setPen(Qt.lightGray) - painter.setBrush(QBrush(self._history_background_color)) - left = max(clip_left, x) - painter.drawRect(left, y, min(clip_right - left, w), h) - row += 1 - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_selected_region(self, painter): - """ - Draws a box around the selected region - :param painter: allows access to paint functions,''QPainter'' - """ - if self._selected_left is None: - return - - x_left = self.map_stamp_to_x(self._selected_left) - if self._selected_right is not None: - x_right = self.map_stamp_to_x(self._selected_right) - else: - x_right = self.map_stamp_to_x(self.playhead.to_sec()) - - left = x_left - top = self._history_top - self._playhead_pointer_size[1] - 5 - self._time_font_size - 4 - width = x_right - x_left - height = self._history_top - top - - painter.setPen(self._selected_region_color) - painter.setBrush(QBrush(self._selected_region_color)) - painter.drawRect(left, top, width, height) - - painter.setPen(self._selected_region_outline_ends_color) - painter.setBrush(Qt.NoBrush) - painter.drawLine(left, top, left, top + height) - painter.drawLine(left + width, top, left + width, top + height) - - painter.setPen(self._selected_region_outline_top_color) - painter.setBrush(Qt.NoBrush) - painter.drawLine(left, top, left + width, top) - - painter.setPen(self._selected_region_outline_top_color) - painter.drawLine(left, self._history_top, left, self._history_bottom) - painter.drawLine(left + width, self._history_top, left + width, self._history_bottom) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_playhead(self, painter): - """ - Draw a line and 2 triangles to denote the current position being viewed - :param painter: ,''QPainter'' - """ - px = self.map_stamp_to_x(self.playhead.to_sec()) - pw, ph = self._playhead_pointer_size - - # Line - painter.setPen(QPen(self._playhead_color)) - painter.setBrush(QBrush(self._playhead_color)) - painter.drawLine(px, self._history_top - 1, px, self._history_bottom + 2) - - # Upper triangle - py = self._history_top - ph - painter.drawPolygon(QPolygonF([QPointF(px, py + ph), QPointF(px + pw, py), QPointF(px - pw, py)])) - - # Lower triangle - py = self._history_bottom + 1 - painter.drawPolygon(QPolygonF([QPointF(px, py), QPointF(px + pw, py + ph), QPointF(px - pw, py + ph)])) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_history_border(self, painter): - """ - Draw a simple black rectangle frame around the timeline view area - :param painter: ,''QPainter'' - """ - bounds_width = min(self._history_width, self.scene().width()) - x, y, w, h = self._history_left, self._history_top, bounds_width, self._history_bottom - self._history_top - - painter.setBrush(Qt.NoBrush) - painter.setPen(Qt.black) - painter.drawRect(x, y, w, h) - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_topic_names(self, painter): - """ - Calculate positions of existing topic names and draw them on the left, one for each row - :param painter: ,''QPainter'' - """ - topics = self._history_bounds.keys() - coords = [(self._margin_left, y + (h / 2) + (self._topic_font_height / 2)) for (_, y, _, h) in self._history_bounds.values()] - - for text, coords in zip([t.lstrip('/') for t in topics], coords): - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - painter.setFont(self._topic_font) - painter.drawText(coords[0], coords[1], self._trimmed_topic_name(text)) - - def _draw_time_divisions(self, painter): - """ - Draw vertical grid-lines showing major and minor time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - x_per_sec = self.map_dstamp_to_dx(1.0) - major_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._major_spacing] - if len(major_divisions) == 0: - major_division = max(self._sec_divisions) - else: - major_division = min(major_divisions) - - minor_divisions = [s for s in self._sec_divisions if x_per_sec * s >= self._minor_spacing and major_division % s == 0] - if len(minor_divisions) > 0: - minor_division = min(minor_divisions) - else: - minor_division = None - - start_stamp = self._start_stamp.to_sec() - - major_stamps = list(self._get_stamps(start_stamp, major_division)) - self._draw_major_divisions(painter, major_stamps, start_stamp, major_division) - - if minor_division: - minor_stamps = [s for s in self._get_stamps(start_stamp, minor_division) if s not in major_stamps] - self._draw_minor_divisions(painter, minor_stamps, start_stamp, minor_division) - - def _draw_major_divisions(self, painter, stamps, start_stamp, division): - """ - Draw black hashed vertical grid-lines showing major time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - label_y = self._history_top - self._playhead_pointer_size[1] - 5 - for stamp in stamps: - x = self.map_stamp_to_x(stamp, False) - - label = self._get_label(division, stamp - start_stamp) - label_x = x + self._major_divisions_label_indent - if label_x + self._qfont_width(label) < self.scene().width(): - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - painter.setFont(self._time_font) - painter.drawText(label_x, label_y, label) - - painter.setPen(self._major_division_pen) - painter.drawLine(x, label_y - self._time_tick_height - self._time_font_size, x, self._history_bottom) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - def _draw_minor_divisions(self, painter, stamps, start_stamp, division): - """ - Draw grey hashed vertical grid-lines showing minor time divisions. - :param painter: allows access to paint functions,''QPainter'' - """ - xs = [self.map_stamp_to_x(stamp) for stamp in stamps] - painter.setPen(self._minor_division_pen) - for x in xs: - painter.drawLine(x, self._history_top, x, self._history_bottom) - - painter.setPen(self._minor_division_tick_pen) - for x in xs: - painter.drawLine(x, self._history_top - self._time_tick_height, x, self._history_top) - - painter.setBrush(self._default_brush) - painter.setPen(self._default_pen) - - # Close function - - def handle_close(self): - for renderer in self._timeline_renderers.values(): - renderer.close() - self._index_cache_thread.stop() - - # Plugin interaction functions - - def get_viewer_types(self, datatype): - return [RawView] + self._viewer_types.get('*', []) + self._viewer_types.get(datatype, []) - - def load_plugins(self): - from rqt_gui.rospkg_plugin_provider import RospkgPluginProvider - self.plugin_provider = RospkgPluginProvider('rqt_bag', 'rqt_bag::Plugin') - - plugin_descriptors = self.plugin_provider.discover(None) - for plugin_descriptor in plugin_descriptors: - try: - plugin = self.plugin_provider.load(plugin_descriptor.plugin_id(), plugin_context=None) - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to load plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - continue - try: - view = plugin.get_view_class() - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get view from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - continue - - timeline_renderer = None - try: - timeline_renderer = plugin.get_renderer_class() - except AttributeError: - pass - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get renderer from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - - msg_types = [] - try: - msg_types = plugin.get_message_types() - except AttributeError: - pass - except Exception as e: - qWarning('rqt_bag.TimelineFrame.load_plugins() failed to get message types from plugin "%s":\n%s' % (plugin_descriptor.plugin_id(), e)) - finally: - if not msg_types: - qWarning('rqt_bag.TimelineFrame.load_plugins() plugin "%s" declares no message types:\n%s' % (plugin_descriptor.plugin_id(), e)) - - for msg_type in msg_types: - self._viewer_types.setdefault(msg_type, []).append(view) - if timeline_renderer: - self._timeline_renderers[msg_type] = timeline_renderer(self) - - qDebug('rqt_bag.TimelineFrame.load_plugins() loaded plugin "%s"' % plugin_descriptor.plugin_id()) - - # Timeline renderer interaction functions - - def get_renderers(self): - """ - :returns: a list of the currently loaded renderers for the plugins - """ - renderers = [] - - for topic in self.topics: - datatype = self.scene().get_datatype(topic) - renderer = self._timeline_renderers.get(datatype) - if renderer is not None: - renderers.append((topic, renderer)) - return renderers - - def is_renderer_active(self, topic): - return topic in self._rendered_topics - - def toggle_renderers(self): - idle_renderers = len(self._rendered_topics) < len(self.topics) - - self.set_renderers_active(idle_renderers) - - def set_renderers_active(self, active): - if active: - for topic in self.topics: - self._rendered_topics.add(topic) - else: - self._rendered_topics.clear() - self.scene().update() - - def set_renderer_active(self, topic, active): - if active: - if topic in self._rendered_topics: - return - self._rendered_topics.add(topic) - else: - if not topic in self._rendered_topics: - return - self._rendered_topics.remove(topic) - self.scene().update() - - # Index Caching functions - - def _update_index_cache(self, topic): - """ - Updates the cache of message timestamps for the given topic. - :return: number of messages added to the index cache - """ - if self._start_stamp is None or self._end_stamp is None: - return 0 - - if topic not in self.index_cache: - # Don't have any cache of messages in this topic - start_time = self._start_stamp - topic_cache = [] - self.index_cache[topic] = topic_cache - else: - topic_cache = self.index_cache[topic] - - # Check if the cache has been invalidated - if topic not in self.invalidated_caches: - return 0 - - if len(topic_cache) == 0: - start_time = self._start_stamp - else: - start_time = rospy.Time.from_sec(max(0.0, topic_cache[-1])) - - end_time = self._end_stamp - - topic_cache_len = len(topic_cache) - - for entry in self.scene().get_entries(topic, start_time, end_time): - topic_cache.append(entry.time.to_sec()) - - if topic in self.invalidated_caches: - self.invalidated_caches.remove(topic) - - return len(topic_cache) - topic_cache_len - - def _find_regions(self, stamps, max_interval): - """ - Group timestamps into regions connected by timestamps less than max_interval secs apart - :param start_stamp: a list of stamps, ''list'' - :param stamp_step: seconds between each division, ''int'' - """ - region_start, prev_stamp = None, None - for stamp in stamps: - if prev_stamp: - if stamp - prev_stamp > max_interval: - region_end = prev_stamp - yield (region_start, region_end) - region_start = stamp - else: - region_start = stamp - - prev_stamp = stamp - - if region_start and prev_stamp: - yield (region_start, prev_stamp) - - def _get_stamps(self, start_stamp, stamp_step): - """ - Generate visible stamps every stamp_step - :param start_stamp: beginning of timeline stamp, ''int'' - :param stamp_step: seconds between each division, ''int'' - """ - if start_stamp >= self._stamp_left: - stamp = start_stamp - else: - stamp = start_stamp + int((self._stamp_left - start_stamp) / stamp_step) * stamp_step + stamp_step - - while stamp < self._stamp_right: - yield stamp - stamp += stamp_step - - def _get_label(self, division, elapsed): - """ - :param division: number of seconds in a division, ''int'' - :param elapsed: seconds from the beginning, ''int'' - :returns: relevent time elapsed string, ''str'' - """ - secs = int(elapsed) % 60 - - mins = int(elapsed) / 60 - hrs = mins / 60 - days = hrs / 24 - weeks = days / 7 - - if division >= 7 * 24 * 60 * 60: # >1wk divisions: show weeks - return '%dw' % weeks - elif division >= 24 * 60 * 60: # >24h divisions: show days - return '%dd' % days - elif division >= 60 * 60: # >1h divisions: show hours - return '%dh' % hrs - elif division >= 5 * 60: # >5m divisions: show minutes - return '%dm' % mins - elif division >= 1: # >1s divisions: show minutes:seconds - return '%dm%02ds' % (mins, secs) - elif division >= 0.1: # >0.1s divisions: show seconds.0 - return '%d.%ss' % (secs, str(int(10.0 * (elapsed - int(elapsed))))) - elif division >= 0.01: # >0.1s divisions: show seconds.0 - return '%d.%02ds' % (secs, int(100.0 * (elapsed - int(elapsed)))) - else: # show seconds.00 - return '%d.%03ds' % (secs, int(1000.0 * (elapsed - int(elapsed)))) - - # Pixel location/time conversion functions - def map_x_to_stamp(self, x, clamp_to_visible=True): - """ - converts a pixel x value to a stamp - :param x: pixel value to be converted, ''int'' - :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool'' - :returns: timestamp, ''int'' - """ - fraction = float(x - self._history_left) / self._history_width - - if clamp_to_visible: - if fraction <= 0.0: - return self._stamp_left - elif fraction >= 1.0: - return self._stamp_right - - return self._stamp_left + fraction * (self._stamp_right - self._stamp_left) - - def map_dx_to_dstamp(self, dx): - """ - converts a distance in pixel space to a distance in stamp space - :param dx: distance in pixel space to be converted, ''int'' - :returns: distance in stamp space, ''float'' - """ - return float(dx) * (self._stamp_right - self._stamp_left) / self._history_width - - def map_stamp_to_x(self, stamp, clamp_to_visible=True): - """ - converts a timestamp to the x value where that stamp exists in the timeline - :param stamp: timestamp to be converted, ''int'' - :param clamp_to_visible: disallow values that are greater than the current timeline bounds,''bool'' - :returns: # of pixels from the left boarder, ''int'' - """ - if self._stamp_left is None: - return None - fraction = (stamp - self._stamp_left) / (self._stamp_right - self._stamp_left) - - if clamp_to_visible: - fraction = min(1.0, max(0.0, fraction)) - - return self._history_left + fraction * self._history_width - - def map_dstamp_to_dx(self, dstamp): - return (float(dstamp) * self._history_width) / (self._stamp_right - self._stamp_left) - - def map_y_to_topic(self, y): - for topic in self._history_bounds: - x, topic_y, w, topic_h = self._history_bounds[topic] - if y > topic_y and y <= topic_y + topic_h: - return topic - return None - - # View port manipulation functions - def reset_timeline(self): - self.reset_zoom() - - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.NONE - - self.emit_play_region() - - if self._stamp_left is not None: - self.playhead = rospy.Time.from_sec(self._stamp_left) - - def set_timeline_view(self, stamp_left, stamp_right): - self._stamp_left = stamp_left - self._stamp_right = stamp_right - - def translate_timeline(self, dstamp): - self.set_timeline_view(self._stamp_left + dstamp, self._stamp_right + dstamp) - self.scene().update() - - def translate_timeline_left(self): - self.translate_timeline((self._stamp_right - self._stamp_left) * -0.05) - - def translate_timeline_right(self): - self.translate_timeline((self._stamp_right - self._stamp_left) * 0.05) - - # Zoom functions - def reset_zoom(self): - start_stamp, end_stamp = self._start_stamp, self._end_stamp - if start_stamp is None: - return - - if (end_stamp - start_stamp) < rospy.Duration.from_sec(5.0): - end_stamp = start_stamp + rospy.Duration.from_sec(5.0) - - self.set_timeline_view(start_stamp.to_sec(), end_stamp.to_sec()) - self.scene().update() - - def zoom_in(self): - self.zoom_timeline(0.5) - - def zoom_out(self): - self.zoom_timeline(2.0) - - def can_zoom_in(self): - return self.can_zoom(0.5) - - def can_zoom_out(self): - return self.can_zoom(2.0) - - def can_zoom(self, desired_zoom): - if not self._stamp_left or not self.playhead: - return False - - new_interval = self.get_zoom_interval(desired_zoom) - if not new_interval: - return False - - new_range = new_interval[1] - new_interval[0] - curr_range = self._stamp_right - self._stamp_left - actual_zoom = new_range / curr_range - - if desired_zoom < 1.0: - return actual_zoom < 0.95 - else: - return actual_zoom > 1.05 - - def zoom_timeline(self, zoom, center=None): - interval = self.get_zoom_interval(zoom, center) - if not interval: - return - - self._stamp_left, self._stamp_right = interval - - self.scene().update() - - def get_zoom_interval(self, zoom, center=None): - """ - @rtype: tuple - @requires: left & right zoom interval sizes. - """ - if self._stamp_left is None: - return None - - stamp_interval = self._stamp_right - self._stamp_left - if center is None: - center = self.playhead.to_sec() - center_frac = (center - self._stamp_left) / stamp_interval - - new_stamp_interval = zoom * stamp_interval - if new_stamp_interval == 0: - return None - # Enforce zoom limits - px_per_sec = self._history_width / new_stamp_interval - if px_per_sec < self._min_zoom: - new_stamp_interval = self._history_width / self._min_zoom - elif px_per_sec > self._max_zoom: - new_stamp_interval = self._history_width / self._max_zoom - - left = center - center_frac * new_stamp_interval - right = left + new_stamp_interval - - return (left, right) - - def pause(self): - self._paused = True - - def resume(self): - self._paused = False - self._bag_timeline.resume() - - # Mouse event handlers - def on_middle_down(self, event): - self._clicked_pos = self._dragged_pos = event.pos() - self.pause() - - def on_left_down(self, event): - if self.playhead == None: - return - - self._clicked_pos = self._dragged_pos = event.pos() - - self.pause() - - if event.modifiers() == Qt.ShiftModifier: - return - - x = self._clicked_pos.x() - y = self._clicked_pos.y() - if x >= self._history_left and x <= self._history_right: - if y >= self._history_top and y <= self._history_bottom: - # Clicked within timeline - set playhead - playhead_secs = self.map_x_to_stamp(x) - if playhead_secs <= 0.0: - self.playhead = rospy.Time(0, 1) - else: - self.playhead = rospy.Time.from_sec(playhead_secs) - self.scene().update() - - elif y <= self._history_top: - # Clicked above timeline - if self._selecting_mode == _SelectionMode.NONE: - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.LEFT_MARKED - self.scene().update() - self.emit_play_region() - - elif self._selecting_mode == _SelectionMode.MARKED: - left_x = self.map_stamp_to_x(self._selected_left) - right_x = self.map_stamp_to_x(self._selected_right) - if x < left_x - self._selection_handle_width or x > right_x + self._selection_handle_width: - self._selected_left = None - self._selected_right = None - self._selecting_mode = _SelectionMode.LEFT_MARKED - self.scene().update() - self.emit_play_region() - elif self._selecting_mode == _SelectionMode.SHIFTING: - self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor)) - - def on_mouse_up(self, event): - self.resume() - - if self._selecting_mode in [_SelectionMode.LEFT_MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]: - if self._selected_left is None: - self._selecting_mode = _SelectionMode.NONE - else: - self._selecting_mode = _SelectionMode.MARKED - self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor)) - self.scene().update() - - def on_mousewheel(self, event): - dz = event.delta() / 120.0 - self.zoom_timeline(1.0 - dz * 0.2) - - def on_mouse_move(self, event): - if not self._history_left: # TODO: need a better notion of initialized - return - - x = event.pos().x() - y = event.pos().y() - - if event.buttons() == Qt.NoButton: - # Mouse moving - if self._selecting_mode in [_SelectionMode.MARKED, _SelectionMode.MOVE_LEFT, _SelectionMode.MOVE_RIGHT, _SelectionMode.SHIFTING]: - if y <= self._history_top and self._selected_left is not None: - left_x = self.map_stamp_to_x(self._selected_left) - right_x = self.map_stamp_to_x(self._selected_right) - - if abs(x - left_x) <= self._selection_handle_width: - self._selecting_mode = _SelectionMode.MOVE_LEFT - self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor)) - return - elif abs(x - right_x) <= self._selection_handle_width: - self._selecting_mode = _SelectionMode.MOVE_RIGHT - self.scene().views()[0].setCursor(QCursor(Qt.SizeHorCursor)) - return - elif x > left_x and x < right_x: - self._selecting_mode = _SelectionMode.SHIFTING - self.scene().views()[0].setCursor(QCursor(Qt.OpenHandCursor)) - return - else: - self._selecting_mode = _SelectionMode.MARKED - self.scene().views()[0].setCursor(QCursor(Qt.ArrowCursor)) - else: - # Mouse dragging - if event.buttons() == Qt.MidButton or event.modifiers() == Qt.ShiftModifier: - # Middle or shift: zoom and pan - dx_drag, dy_drag = x - self._dragged_pos.x(), y - self._dragged_pos.y() - - if dx_drag != 0: - self.translate_timeline(-self.map_dx_to_dstamp(dx_drag)) - if (dx_drag == 0 and abs(dy_drag) > 0) or (dx_drag != 0 and abs(float(dy_drag) / dx_drag) > 0.2 and abs(dy_drag) > 1): - zoom = min(self._max_zoom_speed, max(self._min_zoom_speed, 1.0 + self._zoom_sensitivity * dy_drag)) - self.zoom_timeline(zoom, self.map_x_to_stamp(x)) - - self.scene().views()[0].setCursor(QCursor(Qt.ClosedHandCursor)) - elif event.buttons() == Qt.LeftButton: - # Left: move selected region and move selected region boundry - clicked_x = self._clicked_pos.x() - clicked_y = self._clicked_pos.y() - - x_stamp = self.map_x_to_stamp(x) - - if y <= self._history_top: - if self._selecting_mode == _SelectionMode.LEFT_MARKED: - # Left and selecting: change selection region - clicked_x_stamp = self.map_x_to_stamp(clicked_x) - - self._selected_left = min(clicked_x_stamp, x_stamp) - self._selected_right = max(clicked_x_stamp, x_stamp) - self.scene().update() - - elif self._selecting_mode == _SelectionMode.MOVE_LEFT: - self._selected_left = x_stamp - self.scene().update() - - elif self._selecting_mode == _SelectionMode.MOVE_RIGHT: - self._selected_right = x_stamp - self.scene().update() - - elif self._selecting_mode == _SelectionMode.SHIFTING: - dx_drag = x - self._dragged_pos.x() - dstamp = self.map_dx_to_dstamp(dx_drag) - - self._selected_left = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_left + dstamp)) - self._selected_right = max(self._start_stamp.to_sec(), min(self._end_stamp.to_sec(), self._selected_right + dstamp)) - self.scene().update() - self.emit_play_region() - - elif clicked_x >= self._history_left and clicked_x <= self._history_right and clicked_y >= self._history_top and clicked_y <= self._history_bottom: - # Left and clicked within timeline: change playhead - if x_stamp <= 0.0: - self.playhead = rospy.Time(0, 1) - else: - self.playhead = rospy.Time.from_sec(x_stamp) - self.scene().update() - self._dragged_pos = event.pos() diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py deleted file mode 100644 index 4078f463..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/timeline_menu.py +++ /dev/null @@ -1,275 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QVBoxLayout, QMenu, QWidget, QDockWidget - -class TopicPopupWidget(QWidget): - def __init__(self, popup_name, timeline, viewer_type, topic): - super(TopicPopupWidget, self).__init__() - self.setObjectName(popup_name) - self.setWindowTitle(popup_name) - - layout = QVBoxLayout() - self.setLayout(layout) - - self._timeline = timeline - self._viewer_type = viewer_type - self._topic = topic - self._viewer = None - self._is_listening = False - - def hideEvent(self, event): - if self._is_listening: - self._timeline.remove_listener(self._topic, self._viewer) - self._is_listening = False - super(TopicPopupWidget, self).hideEvent(event) - - def showEvent(self, event): - if not self._is_listening: - self._timeline.add_listener(self._topic, self._viewer) - self._is_listening = True - super(TopicPopupWidget, self).showEvent(event) - - def show(self, context): - """ - Make this topic popup visible, if necessary. This includes setting up - the proper close button hacks - """ - if not self.parent(): - context.add_widget(self) - # make the dock widget closable, even if it normally isn't - dock_features = self.parent().features() - dock_features |= QDockWidget.DockWidgetClosable - self.parent().setFeatures(dock_features) - - # remove old listener - if self._viewer: - self._timeline.remove_listener(self._topic, self._viewer) - self._viewer = None - - # clean out the layout - while self.layout().count() > 0: - item = self.layout().itemAt(0) - self.layout().removeItem(item) - - # create a new viewer - self._viewer = self._viewer_type(self._timeline, self, self._topic) - if not self._is_listening: - self._timeline.add_listener(self._topic, self._viewer) - self._is_listening = True - - super(TopicPopupWidget, self).show() - -class TimelinePopupMenu(QMenu): - """ - Custom popup menu displayed on rightclick from timeline - """ - def __init__(self, timeline, event, menu_topic): - super(TimelinePopupMenu, self).__init__() - - self.parent = timeline - self.timeline = timeline - - - if menu_topic is not None: - self.setTitle(menu_topic) - self._menu_topic = menu_topic - else: - self._menu_topic = None - - self._reset_timeline = self.addAction('Reset Timeline') - - self._play_all = self.addAction('Play All Messages') - self._play_all.setCheckable(True) - self._play_all.setChecked(self.timeline.play_all) - - self.addSeparator() - - self._renderers = self.timeline._timeline_frame.get_renderers() - self._thumbnail_actions = [] - - # create thumbnail menu items - if menu_topic is None: - submenu = self.addMenu('Thumbnails...') - self._thumbnail_show_action = submenu.addAction('Show All') - self._thumbnail_hide_action = submenu.addAction('Hide All') - submenu.addSeparator() - - for topic, renderer in self._renderers: - self._thumbnail_actions.append(submenu.addAction(topic)) - self._thumbnail_actions[-1].setCheckable(True) - self._thumbnail_actions[-1].setChecked(self.timeline._timeline_frame.is_renderer_active(topic)) - else: - self._thumbnail_show_action = None - self._thumbnail_hide_action = None - for topic, renderer in self._renderers: - if menu_topic == topic: - self._thumbnail_actions.append(self.addAction("Thumbnail")) - self._thumbnail_actions[-1].setCheckable(True) - self._thumbnail_actions[-1].setChecked(self.timeline._timeline_frame.is_renderer_active(topic)) - - # create view menu items - self._topic_actions = [] - self._type_actions = [] - if menu_topic is None: - self._topics = self.timeline._timeline_frame.topics - view_topics_menu = self.addMenu('View (by Topic)') - for topic in self._topics: - datatype = self.timeline.get_datatype(topic) - - # View... / topic - topic_menu = QMenu(topic, self) - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - - # View... / topic / Viewer - for viewer_type in viewer_types: - tempaction = topic_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - view_topics_menu.addMenu(topic_menu) - - view_type_menu = self.addMenu('View (by Type)') - self._topics_by_type = self.timeline._timeline_frame._topics_by_datatype - for datatype in self._topics_by_type: - # View... / datatype - datatype_menu = QMenu(datatype, self) - datatype_topics = self._topics_by_type[datatype] - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - for topic in [t for t in self._topics if t in datatype_topics]: # use timeline ordering - topic_menu = QMenu(topic, datatype_menu) - # View... / datatype / topic / Viewer - for viewer_type in viewer_types: - tempaction = topic_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - datatype_menu.addMenu(topic_menu) - view_type_menu.addMenu(datatype_menu) - else: - view_menu = self.addMenu("View") - datatype = self.timeline.get_datatype(menu_topic) - - viewer_types = self.timeline._timeline_frame.get_viewer_types(datatype) - for viewer_type in viewer_types: - tempaction = view_menu.addAction(viewer_type.name) - tempaction.setData(viewer_type) - self._topic_actions.append(tempaction) - - self.addSeparator() - - # create publish menu items - self._publish_actions = [] - if menu_topic is None: - submenu = self.addMenu('Publish...') - - self._publish_all = submenu.addAction('Publish All') - self._publish_none = submenu.addAction('Publish None') - - submenu.addSeparator() - - for topic in self._topics: - self._publish_actions.append(submenu.addAction(topic)) - self._publish_actions[-1].setCheckable(True) - self._publish_actions[-1].setChecked(self.timeline.is_publishing(topic)) - else: - self._publish_actions.append(self.addAction("Publish")) - self._publish_actions[-1].setCheckable(True) - self._publish_actions[-1].setChecked(self.timeline.is_publishing(menu_topic)) - self._publish_all = None - self._publish_none = None - - - - action = self.exec_(event.globalPos()) - if action is not None and action != 0: - self.process(action) - - def process(self, action): - """ - :param action: action to execute, ''QAction'' - :raises: when it doesn't recognice the action passed in, ''Exception'' - """ - if action == self._reset_timeline: - self.timeline._timeline_frame.reset_timeline() - elif action == self._play_all: - self.timeline.toggle_play_all() - elif action == self._publish_all: - for topic in self.timeline._timeline_frame.topics: - if not self.timeline.start_publishing(topic): - break - elif action == self._publish_none: - for topic in self.timeline._timeline_frame.topics: - if not self.timeline.stop_publishing(topic): - break - elif action == self._thumbnail_show_action: - self.timeline._timeline_frame.set_renderers_active(True) - elif action == self._thumbnail_hide_action: - self.timeline._timeline_frame.set_renderers_active(False) - elif action in self._thumbnail_actions: - if self._menu_topic is None: - topic = action.text() - else: - topic = self._menu_topic - - if self.timeline._timeline_frame.is_renderer_active(topic): - self.timeline._timeline_frame.set_renderer_active(topic, False) - else: - self.timeline._timeline_frame.set_renderer_active(topic, True) - elif action in self._topic_actions + self._type_actions: - if self._menu_topic is None: - topic = action.parentWidget().title() - else: - topic = self._menu_topic - - popup_name = topic + '__' + action.text() - if popup_name not in self.timeline.popups: - frame = TopicPopupWidget(popup_name, self.timeline, - action.data(), str(topic)) - - self.timeline.add_view(topic, frame) - self.timeline.popups[popup_name] = frame - - # make popup visible - frame = self.timeline.popups[popup_name] - frame.show(self.timeline.get_context()) - - elif action in self._publish_actions: - if self._menu_topic is None: - topic = action.text() - else: - topic = self._menu_topic - - if self.timeline.is_publishing(topic): - self.timeline.stop_publishing(topic) - else: - self.timeline.start_publishing(topic) - else: - raise Exception('Unknown action in TimelinePopupMenu.process') diff --git a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py b/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py deleted file mode 100644 index 75d189a8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag/src/rqt_bag/topic_selection.py +++ /dev/null @@ -1,112 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2014, Surya Ambrose, Aldebaran -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosgraph - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QWidget, QHBoxLayout, QVBoxLayout, QCheckBox, QScrollArea, QPushButton - -class TopicSelection(QWidget): - - recordSettingsSelected = Signal(bool, list) - - def __init__(self): - super(TopicSelection, self).__init__() - master = rosgraph.Master('rqt_bag_recorder') - self.setWindowTitle("Select the topics you want to record") - self.resize(500, 700) - - self.topic_list = [] - self.selected_topics = [] - self.items_list = [] - - self.area = QScrollArea(self) - self.main_widget = QWidget(self.area) - self.ok_button = QPushButton("Record", self) - self.ok_button.clicked.connect(self.onButtonClicked) - self.ok_button.setEnabled(False) - - self.main_vlayout = QVBoxLayout(self) - self.main_vlayout.addWidget(self.area) - self.main_vlayout.addWidget(self.ok_button) - self.setLayout(self.main_vlayout) - - self.selection_vlayout = QVBoxLayout(self) - self.item_all = QCheckBox("All", self) - self.item_all.stateChanged.connect(self.updateList) - self.selection_vlayout.addWidget(self.item_all) - topic_data_list = master.getPublishedTopics('') - topic_data_list.sort() - for topic, datatype in topic_data_list: - self.addCheckBox(topic) - - self.main_widget.setLayout(self.selection_vlayout) - - self.area.setWidget(self.main_widget) - self.show() - - def addCheckBox(self, topic): - self.topic_list.append(topic) - item = QCheckBox(topic, self) - item.stateChanged.connect(lambda x: self.updateList(x,topic)) - self.items_list.append(item) - self.selection_vlayout.addWidget(item) - - - def updateList(self, state, topic = None): - if topic is None: # The "All" checkbox was checked / unchecked - if state == Qt.Checked: - self.item_all.setTristate(False) - for item in self.items_list: - if item.checkState() == Qt.Unchecked: - item.setCheckState(Qt.Checked) - elif state == Qt.Unchecked: - self.item_all.setTristate(False) - for item in self.items_list: - if item.checkState() == Qt.Checked: - item.setCheckState(Qt.Unchecked) - else: - if state == Qt.Checked: - self.selected_topics.append(topic) - else: - self.selected_topics.remove(topic) - if self.item_all.checkState()==Qt.Checked: - self.item_all.setCheckState(Qt.PartiallyChecked) - - if self.selected_topics == []: - self.ok_button.setEnabled(False) - else: - self.ok_button.setEnabled(True) - - def onButtonClicked(self): - self.close() - self.recordSettingsSelected.emit((self.item_all.checkState() == Qt.Checked), self.selected_topics) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst deleted file mode 100644 index 8128b90f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CHANGELOG.rst +++ /dev/null @@ -1,109 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_bag_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* add missing dependency on rqt_plot (`#316 `_) -* work around Pillow segfault if PyQt5 is installed (`#289 `_, `#290 `_) - -0.3.10 (2014-10-01) -------------------- -* add displaying of depth image thumbnails - -0.3.9 (2014-08-18) ------------------- -* add missing dependency on python-cairo (`#269 `_) - -0.3.8 (2014-07-15) ------------------- -* fix missing installation of resource subfolder - -0.3.7 (2014-07-11) ------------------- -* add plotting plugin (`#239 `_) -* fix rqt_bag to plot array members (`#253 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* fix PIL/Pillow error (`#224 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt deleted file mode 100644 index 6561ffff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_bag_plugins) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox deleted file mode 100644 index 6b5645f7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_bag_plugins - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml b/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml deleted file mode 100644 index 43113cfd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/package.xml +++ /dev/null @@ -1,36 +0,0 @@ - - rqt_bag_plugins - 0.3.13 - rqt_bag provides a GUI plugin for displaying and replaying ROS bag files. - Aaron Blasdel - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_bag - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - Tim Field - - catkin - - geometry_msgs - python-cairo - python-imaging - rosbag - roslib - rospy - rqt_bag - rqt_gui - rqt_gui_py - rqt_plot - sensor_msgs - std_msgs - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml b/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml deleted file mode 100644 index bac205d5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/plugin.xml +++ /dev/null @@ -1,13 +0,0 @@ - - - - A plugin for rqt_bag for showing images. - - - - - - A plugin for rqt_bag for plotting data. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui b/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui deleted file mode 100644 index bd7e6fdb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/resource/plot.ui +++ /dev/null @@ -1,162 +0,0 @@ - - - Form - - - - 0 - 0 - 488 - 329 - - - - - 0 - 0 - - - - Form - - - Qt::LeftToRight - - - - - - - 0 - 0 - - - - Qt::LeftToRight - - - Qt::Horizontal - - - - - 200 - 0 - - - - - 200 - 0 - - - - - 0 - 0 - - - - - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - - - - - - - - - - - Resolution: - - - - - - - - - - - false - - - - 0 - 0 - - - - - 100 - 16777215 - - - - - - - 100 - - - true - - - - - - - auto - - - true - - - true - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py deleted file mode 100644 index 2b2255fb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_bag_plugins'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/__init__.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py deleted file mode 100755 index ce3d5d23..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_helper.py +++ /dev/null @@ -1,102 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import array -from cStringIO import StringIO -import sys - -from PIL import Image -from PIL import ImageOps -import cairo - - -def imgmsg_to_pil(img_msg, rgba=True): - try: - if img_msg._type == 'sensor_msgs/CompressedImage': - pil_img = Image.open(StringIO(img_msg.data)) - if pil_img.mode != 'L': - pil_img = pil_bgr2rgb(pil_img) - else: - alpha = False - pil_mode = 'RGB' - if img_msg.encoding == 'mono8': - mode = 'L' - elif img_msg.encoding == 'rgb8': - mode = 'RGB' - elif img_msg.encoding == 'bgr8': - mode = 'BGR' - elif img_msg.encoding in ['bayer_rggb8', 'bayer_bggr8', 'bayer_gbrg8', 'bayer_grbg8']: - mode = 'L' - elif img_msg.encoding == 'mono16' or img_msg.encoding == '16UC1': - pil_mode = 'F' - if img_msg.is_bigendian: - mode = 'F;16B' - else: - mode = 'F;16' - elif img_msg.encoding == 'rgba8': - mode = 'BGR' - alpha = True - elif img_msg.encoding == 'bgra8': - mode = 'RGB' - alpha = True - else: - raise Exception("Unsupported image format: %s" % img_msg.encoding) - pil_img = Image.frombuffer(pil_mode, (img_msg.width, img_msg.height), img_msg.data, 'raw', mode, 0, 1) - - # 16 bits conversion to 8 bits - if pil_img.mode == 'F': - pil_img = pil_img.point(lambda i: i*(1./256.)).convert('L') - pil_img = ImageOps.autocontrast(pil_img) - - if rgba and pil_img.mode != 'RGBA': - pil_img = pil_img.convert('RGBA') - - return pil_img - - except Exception, ex: - print >> sys.stderr, 'Can\'t convert image: %s' % ex - return None - - -def pil_bgr2rgb(pil_img): - rgb2bgr = (0, 0, 1, 0, - 0, 1, 0, 0, - 1, 0, 0, 0) - return pil_img.convert('RGB', rgb2bgr) - - -def pil_to_cairo(pil_img): - w, h = pil_img.size - data = array.array('c') - data.fromstring(pil_img.tostring()) - - return cairo.ImageSurface.create_for_data(data, cairo.FORMAT_ARGB32, w, h) diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py deleted file mode 100755 index 0a9bf63c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_plugin.py +++ /dev/null @@ -1,51 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_bag.plugins.plugin import Plugin - -from .image_timeline_renderer import ImageTimelineRenderer -from .image_view import ImageView - - -class ImagePlugin(Plugin): - - def __init__(self): - pass - - def get_view_class(self): - return ImageView - - def get_renderer_class(self): - return ImageTimelineRenderer - - def get_message_types(self): - return ['sensor_msgs/Image', 'sensor_msgs/CompressedImage'] diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py deleted file mode 100755 index cbbd9628..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_timeline_renderer.py +++ /dev/null @@ -1,181 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy - -# HACK workaround for upstream pillow issue python-pillow/Pillow#400 -import sys -if 'PyQt5' in sys.modules: - sys.modules['PyQt5'] = None -from PIL import Image -from PIL.ImageQt import ImageQt - -from rqt_bag import TimelineCache, TimelineRenderer - -import image_helper - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QBrush, QPen, QPixmap - - -class ImageTimelineRenderer(TimelineRenderer): - """ - Draws thumbnails of sensor_msgs/Image or sensor_msgs/CompressedImage in the timeline. - """ - def __init__(self, timeline, thumbnail_height=160): - super(ImageTimelineRenderer, self).__init__(timeline, msg_combine_px=40.0) - - self.thumbnail_height = thumbnail_height - - self.thumbnail_combine_px = 20.0 # use cached thumbnail if it's less than this many pixels away - self.min_thumbnail_width = 8 # don't display thumbnails if less than this many pixels across - self.quality = Image.NEAREST # quality hint for thumbnail scaling - - self.thumbnail_cache = TimelineCache(self._load_thumbnail, lambda topic, msg_stamp, thumbnail: self.timeline.scene().update()) - # TimelineRenderer implementation - - def get_segment_height(self, topic): - return self.thumbnail_height - - def draw_timeline_segment(self, painter, topic, stamp_start, stamp_end, x, y, width, height): - """ - draws a stream of images for the topic - :param painter: painter object, ''QPainter'' - :param topic: topic to draw, ''str'' - :param stamp_start: stamp to start drawing, ''rospy.Time'' - :param stamp_end: stamp to end drawing, ''rospy.Time'' - :param x: x to draw images at, ''int'' - :param y: y to draw images at, ''int'' - :param width: width in pixels of the timeline area, ''int'' - :param height: height in pixels of the timeline area, ''int'' - """ - if x < self.timeline._history_left: - width += x - self.timeline._history_left - x = self.timeline._history_left - max_interval_thumbnail = self.timeline.map_dx_to_dstamp(self.thumbnail_combine_px) - max_interval_thumbnail = max(0.1, max_interval_thumbnail) - thumbnail_gap = 6 - thumbnail_x, thumbnail_y, thumbnail_height = x + 1, y + 1, height - 2 - thumbnail_gap # leave 1px border - - # set color to white draw rectangle over messages - painter.setBrush(QBrush(Qt.white)) - painter.drawRect(x, y, width, height - thumbnail_gap) - thumbnail_width = None - - while True: - available_width = (x + width) - thumbnail_x - - # Check for enough remaining to draw thumbnail - if width > 1 and available_width < self.min_thumbnail_width: - break - - # Try to display the thumbnail, if its right edge is to the right of the timeline's left side - if not thumbnail_width or thumbnail_x + thumbnail_width >= self.timeline._history_left: - stamp = self.timeline.map_x_to_stamp(thumbnail_x, clamp_to_visible=False) - thumbnail_bitmap = self.thumbnail_cache.get_item(topic, stamp, max_interval_thumbnail) - - # Cache miss - if not thumbnail_bitmap: - thumbnail_details = (thumbnail_height,) - self.thumbnail_cache.enqueue((topic, stamp, max_interval_thumbnail, thumbnail_details)) - if not thumbnail_width: - break - else: - thumbnail_width, _ = thumbnail_bitmap.size - - if width > 1: - if available_width < thumbnail_width: - thumbnail_width = available_width - 1 - QtImage = ImageQt(thumbnail_bitmap) - pixmap = QPixmap.fromImage(QtImage) - painter.drawPixmap(thumbnail_x, thumbnail_y, thumbnail_width, thumbnail_height, pixmap) - thumbnail_x += thumbnail_width - - if width == 1: - break - - painter.setPen(QPen(QBrush(Qt.black))) - painter.setBrush(QBrush(Qt.transparent)) - if width == 1: - painter.drawRect(x, y, thumbnail_x - x, height - thumbnail_gap - 1) - else: - painter.drawRect(x, y, width, height - thumbnail_gap - 1) - return True - - def close(self): - if self.thumbnail_cache: - self.thumbnail_cache.stop() - self.thumbnail_cache.join() - - def _load_thumbnail(self, topic, stamp, thumbnail_details): - """ - Loads the thumbnail from the bag - """ - (thumbnail_height,) = thumbnail_details - - # Find position of stamp using index - t = rospy.Time.from_sec(stamp) - bag, entry = self.timeline.scene().get_entry(t, topic) - if not entry: - return None, None - pos = entry.position - - # Not in the cache; load from the bag file - - with self.timeline.scene()._bag_lock: - msg_topic, msg, msg_stamp = bag._read_message(pos) - - # Convert from ROS image to PIL image - try: - pil_image = image_helper.imgmsg_to_pil(msg) - except Exception, ex: - print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) - pil_image = None - - if not pil_image: - print >> sys.stderr, 'Disabling renderer on %s' % topic - self.timeline.set_renderer_active(topic, False) - return None, None - - # Calculate width to maintain aspect ratio - try: - pil_image_size = pil_image.size - thumbnail_width = int(round(thumbnail_height * (float(pil_image_size[0]) / pil_image_size[1]))) - # Scale to thumbnail size - thumbnail = pil_image.resize((thumbnail_width, thumbnail_height), self.quality) - - return msg_stamp, thumbnail - - except Exception, ex: - print >> sys.stderr, 'Error loading image on topic %s: %s' % (topic, str(ex)) - raise - return None, None diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py deleted file mode 100755 index e056abb9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/image_view.py +++ /dev/null @@ -1,111 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from PIL import Image - -# HACK workaround for upstream pillow issue python-pillow/Pillow#400 -import sys -if 'PyQt5' in sys.modules: - sys.modules['PyQt5'] = None -from PIL.ImageQt import ImageQt - -from rqt_bag import TopicMessageView -import image_helper - -from python_qt_binding.QtGui import QGraphicsScene, QGraphicsView, QPixmap - - -class ImageView(TopicMessageView): - """ - Popup image viewer - """ - name = 'Image' - - def __init__(self, timeline, parent, topic): - super(ImageView, self).__init__(timeline, parent, topic) - - self._image = None - self._image_topic = None - self._image_stamp = None - self.quality = Image.NEAREST # quality hint for scaling - - # TODO put the image_topic and image_stamp on the picture or display them in some fashion - self._overlay_font_size = 14.0 - self._overlay_indent = (4, 4) - self._overlay_color = (0.2, 0.2, 1.0) - - self._image_view = QGraphicsView(parent) - self._image_view.resizeEvent = self._resizeEvent - self._scene = QGraphicsScene() - self._image_view.setScene(self._scene) - parent.layout().addWidget(self._image_view) - - # MessageView implementation - def _resizeEvent(self, event): - # TODO make this smarter. currently there will be no scrollbar even if the timeline extends beyond the viewable area - self._scene.setSceneRect(0, 0, self._image_view.size().width() - 2, self._image_view.size().height() - 2) - self.put_image_into_scene() - - def message_viewed(self, bag, msg_details): - """ - refreshes the image - """ - TopicMessageView.message_viewed(self, bag, msg_details) - topic, msg, t = msg_details[:3] - if not msg: - self.set_image(None, topic, 'no message') - else: - self.set_image(msg, topic, msg.header.stamp) - - def message_cleared(self): - TopicMessageView.message_cleared(self) - self.set_image(None, None, None) - - # End MessageView implementation - def put_image_into_scene(self): - if self._image: - resized_image = self._image.resize((self._image_view.size().width() - 2, self._image_view.size().height() - 2), self.quality) - - QtImage = ImageQt(resized_image) - pixmap = QPixmap.fromImage(QtImage) - self._scene.clear() - self._scene.addPixmap(pixmap) - - def set_image(self, image_msg, image_topic, image_stamp): - self._image_msg = image_msg - if image_msg: - self._image = image_helper.imgmsg_to_pil(image_msg) - else: - self._image = None - self._image_topic = image_topic - self._image_stamp = image_stamp - self.put_image_into_scene() diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py deleted file mode 100755 index eddfb087..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_plugin.py +++ /dev/null @@ -1,50 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_bag.plugins.plugin import Plugin - -from .plot_view import PlotView - - -class PlotPlugin(Plugin): - - def __init__(self): - pass - - def get_view_class(self): - return PlotView - - def get_renderer_class(self): - return None - - def get_message_types(self): - return ['*'] diff --git a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py b/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py deleted file mode 100755 index a50b7eb8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_bag_plugins/src/rqt_bag_plugins/plot_view.py +++ /dev/null @@ -1,495 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2014, Austin Hendrix, Stanford University -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Design notes: -# -# The original rxbag plot widget displays the plot -# -# It has a settings menu which allows the user to add subplots -# and assign arbitrary fields to subplots. possibly many fields in a single -# subplot, or one field per subplot, or any othe rcombination -# in particular, it is possible to add the same field to multiple subplots -# It doesn't appear able to add fields from different topics to the same plot -# Since rqt_plot can do this, and it's useful for our application, it's worth -# thinking about. If it makes the UI too cluttered, it may not be worth it -# If it isn't possible due to the architecture of rqt_bag, it may not be -# worth it -# -# Thoughts on new design: -# a plottable field is anything which is either numeric, or contains -# at least one plottable subfield (this is useful for enabling/disabling all -# of the fields of a complex type) -# -# for simple messages, we could display a tree view of the message fields -# on top of the plot, along with the color for each plottable field. This gets -# unweildy for large messages, because they'll use up too much screen space -# displaying the topic list -# -# It may be better to display the topic list for a plot as a dockable widget, -# and be able to dismiss it when it isn't actively in use, similar to the -# existing rxbag plot config window -# -# The plot should be dockable and viewable. If it's a separate window, it -# doesn't make sense to make it align with the timeline. This could be done -# if someone wanted to implement a separate timeline view - -import os -import math -import codecs -import threading -import rospkg -from rqt_bag import MessageView - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QWidget, QPushButton, QTreeWidget, QTreeWidgetItem, QSizePolicy, QDoubleValidator, QIcon - -from rqt_plot.data_plot import DataPlot - -# rospy used for Time and Duration objects, for interacting with rosbag -import rospy - -class PlotView(MessageView): - """ - Popup plot viewer - """ - name = 'Plot' - - def __init__(self, timeline, parent, topic): - super(PlotView, self).__init__(timeline, topic) - - self.plot_widget = PlotWidget(timeline, parent, topic) - - parent.layout().addWidget(self.plot_widget) - - def message_viewed(self, bag, msg_details): - """ - refreshes the plot - """ - _, msg, t = msg_details[:3] - - if t is None: - self.message_cleared() - else: - self.plot_widget.message_tree.set_message(msg) - self.plot_widget.set_cursor((t-self.plot_widget.start_stamp).to_sec()) - - def message_cleared(self): - pass - -class PlotWidget(QWidget): - - def __init__(self, timeline, parent, topic): - super(PlotWidget, self).__init__(parent) - self.setObjectName('PlotWidget') - - self.timeline = timeline - msg_type = self.timeline.get_datatype(topic) - self.msgtopic = topic - self.start_stamp = self.timeline._get_start_stamp() - self.end_stamp = self.timeline._get_end_stamp() - - # the current region-of-interest for our bag file - # all resampling and plotting is done with these limits - self.limits = [0,(self.end_stamp-self.start_stamp).to_sec()] - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_bag_plugins'), 'resource', 'plot.ui') - loadUi(ui_file, self) - self.message_tree = MessageTree(msg_type, self) - self.data_tree_layout.addWidget(self.message_tree) - # TODO: make this a dropdown with choices for "Auto", "Full" and - # "Custom" - # I continue to want a "Full" option here - self.auto_res.stateChanged.connect(self.autoChanged) - - self.resolution.editingFinished.connect(self.settingsChanged) - self.resolution.setValidator(QDoubleValidator(0.0,1000.0,6,self.resolution)) - - - self.timeline.selected_region_changed.connect(self.region_changed) - - self.recompute_timestep() - - self.plot = DataPlot(self) - self.plot.set_autoscale(x=False) - self.plot.set_autoscale(y=DataPlot.SCALE_VISIBLE) - self.plot.autoscroll(False) - self.plot.set_xlim(self.limits) - self.data_plot_layout.addWidget(self.plot) - - self._home_button = QPushButton() - self._home_button.setToolTip("Reset View") - self._home_button.setIcon(QIcon.fromTheme('go-home')) - self._home_button.clicked.connect(self.home) - self.plot_toolbar_layout.addWidget(self._home_button) - - self._config_button = QPushButton("Configure Plot") - self._config_button.clicked.connect(self.plot.doSettingsDialog) - self.plot_toolbar_layout.addWidget(self._config_button) - - self.set_cursor(0) - - self.paths_on = set() - self._lines = None - - # get bag from timeline - bag = None - start_time = self.start_stamp - while bag is None: - bag,entry = self.timeline.get_entry(start_time, topic) - if bag is None: - start_time = self.timeline.get_entry_after(start_time)[1].time - - self.bag = bag - # get first message from bag - msg = bag._read_message(entry.position) - self.message_tree.set_message(msg[1]) - - # state used by threaded resampling - self.resampling_active = False - self.resample_thread = None - self.resample_fields = set() - - def set_cursor(self, position): - self.plot.vline(position, color=DataPlot.RED) - self.plot.redraw() - - def add_plot(self, path): - self.resample_data([path]) - - def update_plot(self): - if len(self.paths_on)>0: - self.resample_data(self.paths_on) - - def remove_plot(self, path): - self.plot.remove_curve(path) - self.paths_on.remove(path) - self.plot.redraw() - - def load_data(self): - """get a generator for the specified time range on our bag""" - return self.bag.read_messages(self.msgtopic, - self.start_stamp+rospy.Duration.from_sec(self.limits[0]), - self.start_stamp+rospy.Duration.from_sec(self.limits[1])) - - def resample_data(self, fields): - if self.resample_thread: - # cancel existing thread and join - self.resampling_active = False - self.resample_thread.join() - - for f in fields: - self.resample_fields.add(f) - - # start resampling thread - self.resampling_active = True - self.resample_thread = threading.Thread(target=self._resample_thread) - # explicitly mark our resampling thread as a daemon, because we don't - # want to block program exit on a long resampling operation - self.resample_thread.setDaemon(True) - self.resample_thread.start() - - def _resample_thread(self): - # TODO: - # * look into doing partial display updates for long resampling - # operations - # * add a progress bar for resampling operations - x = {} - y = {} - for path in self.resample_fields: - x[path] = [] - y[path] = [] - - msgdata = self.load_data() - - for entry in msgdata: - # detect if we're cancelled and return early - if not self.resampling_active: - return - - for path in self.resample_fields: - # this resampling method is very unstable, because it picks - # representative points rather than explicitly representing - # the minimum and maximum values present within a sample - # If the data has spikes, this is particularly bad because they - # will be missed entirely at some resolutions and offsets - if x[path]==[] or (entry[2]-self.start_stamp).to_sec()-x[path][-1] >= self.timestep: - y_value = entry[1] - for field in path.split('.'): - index = None - if field.endswith(']'): - field = field[:-1] - field, _, index = field.rpartition('[') - y_value = getattr(y_value, field) - if index: - index = int(index) - y_value = y_value[index] - y[path].append(y_value) - x[path].append((entry[2]-self.start_stamp).to_sec()) - - # TODO: incremental plot updates would go here... - # we should probably do incremental updates based on time; - # that is, push new data to the plot maybe every .5 or .1 - # seconds - # time is a more useful metric than, say, messages loaded or - # percentage, because it will give a reasonable refresh rate - # without overloading the computer - # if we had a progress bar, we could emit a signal to update it here - - # update the plot with final resampled data - for path in self.resample_fields: - if len(x[path]) < 1: - qWarning("Resampling resulted in 0 data points for %s" % path) - else: - if path in self.paths_on: - self.plot.clear_values(path) - self.plot.update_values(path, x[path], y[path]) - else: - self.plot.add_curve(path, path, x[path], y[path]) - self.paths_on.add(path) - - self.plot.redraw() - - self.resample_fields.clear() - self.resampling_active = False - - def recompute_timestep(self): - # this is only called if we think the timestep has changed; either - # by changing the limits or by editing the resolution - limits = self.limits - if self.auto_res.isChecked(): - timestep = round((limits[1]-limits[0])/200.0,5) - else: - timestep = float(self.resolution.text()) - self.resolution.setText(str(timestep)) - self.timestep = timestep - - def region_changed(self, start, end): - # this is the only place where self.limits is set - limits = [ (start - self.start_stamp).to_sec(), - (end - self.start_stamp).to_sec() ] - - # cap the limits to the start and end of our bag file - if limits[0]<0: - limits = [0.0,limits[1]] - if limits[1]>(self.end_stamp-self.start_stamp).to_sec(): - limits = [limits[0],(self.end_stamp-self.start_stamp).to_sec()] - - self.limits = limits - - self.recompute_timestep() - self.plot.set_xlim(limits) - self.plot.redraw() - self.update_plot() - - def settingsChanged(self): - # resolution changed. recompute the timestep and resample - self.recompute_timestep() - self.update_plot() - - def autoChanged(self, state): - if state==2: - # auto mode enabled. recompute the timestep and resample - self.resolution.setDisabled(True) - self.recompute_timestep() - self.update_plot() - else: - # auto mode disabled. enable the resolution text box - # no change to resolution yet, so no need to redraw - self.resolution.setDisabled(False) - - def home(self): - # TODO: re-add the button for this. It's useful for restoring the - # X and Y limits so that we can see all of the data - # effectively a "zoom all" button - - # reset the plot to our current limits - self.plot.set_xlim(self.limits) - # redraw the plot; this forces a Y autoscaling - self.plot.redraw() - - - -class MessageTree(QTreeWidget): - def __init__(self, msg_type, parent): - super(MessageTree, self).__init__(parent) - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.setHeaderHidden(True) - self.itemChanged.connect(self.handleChanged) - self._msg_type = msg_type - self._msg = None - - self._expanded_paths = None - self._checked_states = set() - self.plot_list = set() - - # populate the tree from the message type - - - @property - def msg(self): - return self._msg - - def set_message(self, msg): - # Remember whether items were expanded or not before deleting - if self._msg: - for item in self.get_all_items(): - path = self.get_item_path(item) - if item.isExpanded(): - self._expanded_paths.add(path) - elif path in self._expanded_paths: - self._expanded_paths.remove(path) - if item.checkState(0)==Qt.Checked: - self._checked_states.add(path) - elif path in self._checked_states: - self._checked_states.remove(path) - self.clear() - if msg: - # Populate the tree - self._add_msg_object(None, '', '', msg, msg._type) - - if self._expanded_paths is None: - self._expanded_paths = set() - else: - # Expand those that were previously expanded, and collapse any paths that we've seen for the first time - for item in self.get_all_items(): - path = self.get_item_path(item) - if path in self._expanded_paths: - item.setExpanded(True) - else: - item.setExpanded(False) - self._msg = msg - self.update() - - def get_item_path(self, item): - return item.data(0, Qt.UserRole)[0].replace(' ', '') # remove spaces that may get introduced in indexing, e.g. [ 3] is [3] - - def get_all_items(self): - items = [] - try: - root = self.invisibleRootItem() - self.traverse(root, items.append) - except Exception: - # TODO: very large messages can cause a stack overflow due to recursion - pass - return items - - def traverse(self, root, function): - for i in range(root.childCount()): - child = root.child(i) - function(child) - self.traverse(child, function) - - def _add_msg_object(self, parent, path, name, obj, obj_type): - label = name - - if hasattr(obj, '__slots__'): - subobjs = [(slot, getattr(obj, slot)) for slot in obj.__slots__] - elif type(obj) in [list, tuple]: - len_obj = len(obj) - if len_obj == 0: - subobjs = [] - else: - w = int(math.ceil(math.log10(len_obj))) - subobjs = [('[%*d]' % (w, i), subobj) for (i, subobj) in enumerate(obj)] - else: - subobjs = [] - - plotitem=False - if type(obj) in [int, long, float]: - plotitem=True - if type(obj) == float: - obj_repr = '%.6f' % obj - else: - obj_repr = str(obj) - - if obj_repr[0] == '-': - label += ': %s' % obj_repr - else: - label += ': %s' % obj_repr - - elif type(obj) in [str, bool, int, long, float, complex, rospy.Time]: - # Ignore any binary data - obj_repr = codecs.utf_8_decode(str(obj), 'ignore')[0] - - # Truncate long representations - if len(obj_repr) >= 50: - obj_repr = obj_repr[:50] + '...' - - label += ': ' + obj_repr - item = QTreeWidgetItem([label]) - if name == '': - pass - elif path.find('.') == -1 and path.find('[') == -1: - self.addTopLevelItem(item) - else: - parent.addChild(item) - if plotitem == True: - if path.replace(' ', '') in self._checked_states: - item.setCheckState (0, Qt.Checked) - else: - item.setCheckState (0, Qt.Unchecked) - item.setData(0, Qt.UserRole, (path, obj_type)) - - - for subobj_name, subobj in subobjs: - if subobj is None: - continue - - if path == '': - subpath = subobj_name # root field - elif subobj_name.startswith('['): - subpath = '%s%s' % (path, subobj_name) # list, dict, or tuple - else: - subpath = '%s.%s' % (path, subobj_name) # attribute (prefix with '.') - - if hasattr(subobj, '_type'): - subobj_type = subobj._type - else: - subobj_type = type(subobj).__name__ - - self._add_msg_object(item, subpath, subobj_name, subobj, subobj_type) - - def handleChanged(self, item, column): - if item.data(0, Qt.UserRole)==None: - pass - else: - path = self.get_item_path(item) - if item.checkState(column) == Qt.Checked: - if path not in self.plot_list: - self.plot_list.add(path) - self.parent().parent().parent().add_plot(path) - if item.checkState(column) == Qt.Unchecked: - if path in self.plot_list: - self.plot_list.remove(path) - self.parent().parent().parent().remove_plot(path) diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst deleted file mode 100644 index fd6cdec4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/CHANGELOG.rst +++ /dev/null @@ -1,83 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_common_plugins -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- -* New pkgs added (rqt_action, rqt_launch) - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt deleted file mode 100644 index 3a2016cd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/CMakeLists.txt +++ /dev/null @@ -1,4 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_common_plugins) -find_package(catkin REQUIRED) -catkin_metapackage() diff --git a/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml b/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml deleted file mode 100644 index 130d425f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_common_plugins/package.xml +++ /dev/null @@ -1,58 +0,0 @@ - - rqt_common_plugins - 0.3.13 - - rqt_common_plugins metapackage provides ROS backend graphical tools suite that can be used on/off of robot runtime.
-
- To run any rqt plugins, just type in a single command "rqt", then select any plugins you want from the GUI that launches afterwards.
-
- rqt consists of three following metapackages:
-
    -
  • rqt - core modules of rqt (ROS GUI) framework. rqt plugin developers barely needs to pay attention to this metapackage.
    • -
  • rqt_common_plugins (you're here!)
    • -
  • rqt_robot_plugins - rqt plugins that are particularly used with robots during their runtime.

    • -
-
- - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - Dorian Scholz - Thibault Kruse - Aaron Blasdel - Isaac Saito - - catkin - rqt_action - rqt_bag - rqt_bag_plugins - rqt_console - rqt_dep - rqt_graph - rqt_image_view - rqt_launch - rqt_logger_level - - rqt_msg - rqt_plot - rqt_publisher - rqt_py_common - rqt_py_console - rqt_reconfigure - rqt_service_caller - rqt_shell - rqt_srv - rqt_top - rqt_topic - rqt_web - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst deleted file mode 100644 index 0e78a7db..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_console -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* use icons instead of text when available, refactor pause/resume button - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* rewrite of rqt_console to drastically improve performance (`#186 `_) - -0.3.0 (2013-08-28) ------------------- -* pause button no more saves state (`#125 `_) -* persist message limit (`#138 `_) -* add ability to set logger level (`#117 `_) -* add tooltips to table cells (`#143 `_) -* improve labels for filters (`#146 `_) -* fix time column when loading data from file (`#160 `_) -* fix applying message limit on change (`#133 `_) -* fix clear button to remove all messages (`#141 `_) -* fix sorting to use row index to decide order between equal values (except for time column) (`#124 `_) -* fix locking of message queue -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* added missing word in status tip - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- -* Fix; can't add filters when using pyside (`#36 `_) - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt deleted file mode 100644 index d7a78392..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_console) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_console - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox deleted file mode 100644 index 085267a8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_console provides a GUI plugin for displaying and filtering ROS messages. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_console/package.xml b/workspace/src/rqt_common_plugins/rqt_console/package.xml deleted file mode 100644 index f8ccce80..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_console - 0.3.13 - rqt_console provides a GUI plugin for displaying and filtering ROS messages. - Aaron Blasdel - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_console - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - catkin - - python-rospkg - roslib - rospy - rqt_gui - rqt_gui_py - rqt_logger_level - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/plugin.xml b/workspace/src/rqt_common_plugins/rqt_console/plugin.xml deleted file mode 100644 index df26f202..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for displaying and filtering ROS messages. - - - - - folder - Plugins related to logging. - - - mail-message-new - A Python GUI plugin for displaying and filtering ROS log messages. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui deleted file mode 100644 index 684e9ca3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/console_settings_dialog.ui +++ /dev/null @@ -1,120 +0,0 @@ - - - ConsoleSettingsDialog - - - Qt::ApplicationModal - - - - 0 - 0 - 368 - 123 - - - - Setup - - - true - - - - - - - - - 94 - 16777215 - - - - Rosout Topic - - - - - - - - - - - - - - - 94 - 16777215 - - - - Buffer Size - - - - - - - 100000 - - - 1000 - - - 20000 - - - - - - - - - - - - QDialogButtonBox::Cancel|QDialogButtonBox::Ok - - - - - - - - - buttonBox - accepted() - ConsoleSettingsDialog - accept() - - - 248 - 254 - - - 157 - 274 - - - - - buttonBox - rejected() - ConsoleSettingsDialog - reject() - - - 316 - 260 - - - 286 - 274 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui deleted file mode 100644 index 8b727160..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/console_widget.ui +++ /dev/null @@ -1,340 +0,0 @@ - - - ConsoleWidget - - - - 0 - 0 - 838 - 675 - - - - Console - - - - 4 - - - 5 - - - - - QLayout::SetMinAndMaxSize - - - - - Load a Message file - - - Load - - - - 16 - 16 - - - - - - - - Save currently displayed messages to a file - - - Save - - - - 16 - 16 - - - - - - - - Pause the message stream - - - Pause - - - - 16 - 16 - - - - - - - - Resume the message stream - - - Resume - - - - 16 - 16 - - - - - - - - Displaying 0 messages - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Clear - - - - - - - Resizes message table columns to fit their contents exactly - - - Fit Columns - - - - - - - - - - - Qt::Vertical - - - 9 - - - true - - - - - - - true - - - QAbstractItemView::SelectRows - - - - 16 - 16 - - - - QAbstractItemView::ScrollPerPixel - - - true - - - false - - - false - - - - - - - - Qt::Vertical - - - 9 - - - true - - - - Messages matching ANY of these rules will NOT be displayed - - - Exclude Messages... - - - - - - true - - - QAbstractItemView::NoSelection - - - 1 - - - false - - - true - - - false - - - - - - - - - 27 - 16777215 - - - - Add new filter - - - - - - - 16 - 16 - - - - - - - - - Messages matching ANY of these rules will be highlighted - - - Highlight Messages... - - - - - - - - - true - - - QAbstractItemView::NoSelection - - - 1 - - - false - - - true - - - false - - - - - - - - - - - 27 - 16777215 - - - - Show only highlighted rows - - - - - - - 16 - 16 - - - - true - - - false - - - - - - - - 27 - 16777215 - - - - Add new filter - - - - - - - 16 - 16 - - - - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui deleted file mode 100644 index 0aa39b27..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/custom_filter_widget.ui +++ /dev/null @@ -1,188 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 125 - - - - Form - - - - 0 - - - - - 0 - - - - - 0 - - - - - Message: - - - - - - - Enter a Filter string - - - - - - - Toggle Regular Expressions - - - Regex - - - - - - - - - 0 - - - - - Severity: - - - - - - - - 16777215 - 45 - - - - Click a Severity to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - 20 - - - - - - - Topic - - - - - - - - 16777215 - 45 - - - - Click a Topic to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - 0 - - - - - Node - - - - - - - - 16777215 - 45 - - - - Click a Node to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui deleted file mode 100644 index b45a2855..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/filter_wrapper_widget.ui +++ /dev/null @@ -1,73 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - Toggle Filter Enabled - - - true - - - - - - - Name - - - - - - - - - - - 27 - 16777215 - - - - Delete Filter - - - - - - - 16 - 16 - - - - true - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui deleted file mode 100644 index 79db7845..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/list_filter_widget.ui +++ /dev/null @@ -1,56 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 47 - - - - Form - - - - 0 - - - - - - - - 16777215 - 45 - - - - Click an Item to select it - - - QAbstractItemView::NoEditTriggers - - - true - - - QAbstractItemView::MultiSelection - - - QListView::LeftToRight - - - 1 - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui deleted file mode 100644 index f9c2a2f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/text_filter_widget.ui +++ /dev/null @@ -1,41 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - - - - Toggle Regular Expressions - - - Regex - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui deleted file mode 100644 index 9b3fb67a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/filters/time_filter_widget.ui +++ /dev/null @@ -1,74 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - Start Time - - - hh:mm:ss.zzz (yyyy-MM-dd) - - - - - - - Disable End Time - - - - - - true - - - - - - - End Time - - - hh:mm:ss.zzz (yyyy-MM-dd) - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui b/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui deleted file mode 100644 index 5f463253..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/resource/text_browse_dialog.ui +++ /dev/null @@ -1,67 +0,0 @@ - - - TextBrowseDialog - - - - 0 - 0 - 585 - 562 - - - - Message Viewer - - - - - - - - - Qt::Horizontal - - - QDialogButtonBox::Close - - - - - - - - - button_box - accepted() - TextBrowseDialog - accept() - - - 248 - 254 - - - 157 - 274 - - - - - button_box - rejected() - TextBrowseDialog - reject() - - - 316 - 260 - - - 286 - 274 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console b/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console deleted file mode 100755 index 47ce376e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/scripts/rqt_console +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_console.console.Console')) diff --git a/workspace/src/rqt_common_plugins/rqt_console/setup.py b/workspace/src/rqt_common_plugins/rqt_console/setup.py deleted file mode 100644 index c9f19532..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_console', 'rqt_console.filters'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_console'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/__init__.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py deleted file mode 100644 index 4104f58a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console.py +++ /dev/null @@ -1,139 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rosgraph_msgs.msg import Log -import rospkg -import rospy - -from python_qt_binding.QtCore import QMutex, QMutexLocker, QTimer - -from qt_gui.plugin import Plugin - -from .console_settings_dialog import ConsoleSettingsDialog -from .console_widget import ConsoleWidget -from .message import Message -from .message_data_model import MessageDataModel -from .message_proxy_model import MessageProxyModel - - -class Console(Plugin): - """ - rqt_console plugin's main class. Handles communication with ros_gui and contains - callbacks to handle incoming message - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Console, self).__init__(context) - self.setObjectName('Console') - - self._rospack = rospkg.RosPack() - - self._model = MessageDataModel() - self._proxy_model = MessageProxyModel() - self._proxy_model.setSourceModel(self._model) - - self._widget = ConsoleWidget(self._proxy_model, self._rospack) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - # queue to store incoming data which get flushed periodically to the model - # required since QSortProxyModel can not handle a high insert rate - self._message_queue = [] - self._mutex = QMutex() - self._timer = QTimer() - self._timer.timeout.connect(self.insert_messages) - self._timer.start(100) - - self._subscriber = None - self._topic = '/rosout_agg' - self._subscribe(self._topic) - - def queue_message(self, log_msg): - """ - Callback for adding an incomming message to the queue. - """ - if not self._widget._paused: - msg = Console.convert_rosgraph_log_message(log_msg) - with QMutexLocker(self._mutex): - self._message_queue.append(msg) - - @staticmethod - def convert_rosgraph_log_message(log_msg): - msg = Message() - msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') - msg.message = log_msg.msg - msg.severity = log_msg.level - msg.node = log_msg.name - msg.stamp = (log_msg.header.stamp.secs, log_msg.header.stamp.nsecs) - msg.topics = sorted(log_msg.topics) - msg.location = log_msg.file + ':' + log_msg.function + ':' + str(log_msg.line) - return msg - - def insert_messages(self): - """ - Callback for flushing incoming Log messages from the queue to the model. - """ - with QMutexLocker(self._mutex): - msgs = self._message_queue - self._message_queue = [] - if msgs: - self._model.insert_rows(msgs) - - def shutdown_plugin(self): - self._subscriber.unregister() - self._timer.stop() - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - topics = [t for t in rospy.get_published_topics() if t[1] == 'rosgraph_msgs/Log'] - topics.sort(key=lambda tup: tup[0]) - dialog = ConsoleSettingsDialog(topics, self._rospack) - (topic, message_limit) = dialog.query(self._topic, self._model.get_message_limit()) - if topic != self._topic: - self._subscribe(topic) - if message_limit != self._model.get_message_limit(): - self._model.set_message_limit(message_limit) - - def _subscribe(self, topic): - if self._subscriber: - self._subscriber.unregister() - self._subscriber = rospy.Subscriber(topic, Log, self.queue_message) - self._currenttopic = topic diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py deleted file mode 100644 index 14c72394..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_settings_dialog.py +++ /dev/null @@ -1,73 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QDialog - -from rqt_logger_level.logger_level_widget import LoggerLevelWidget -from rqt_logger_level.logger_level_service_caller import LoggerLevelServiceCaller - - -class ConsoleSettingsDialog(QDialog): - """ - Dialog to change the subscribed topic and alter the message buffer size. - """ - def __init__(self, topics, rospack): - """ - :param topics: list of topics to allow switching, ''list of string'' - :param limit: displayed in the message buffer size spin box, ''int'' - """ - super(ConsoleSettingsDialog, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource', 'console_settings_dialog.ui') - loadUi(ui_file, self) - for topic in topics: - self.topic_combo.addItem(topic[0] + ' (' + topic[1] + ')', topic[0]) - - self._service_caller = LoggerLevelServiceCaller() - self._logger_widget = LoggerLevelWidget(self._service_caller) - self.levelsLayout.addWidget(self._logger_widget) - self.adjustSize() - - def query(self, topic, buffer_size): - index = self.topic_combo.findData(topic) - if index != -1: - self.topic_combo.setCurrentIndex(index) - self.buffer_size_spin.setValue(buffer_size) - ok = self.exec_() - if ok == 1: - index = self.topic_combo.currentIndex() - if index != -1: - topic = self.topic_combo.itemData(index) - buffer_size = self.buffer_size_spin.value() - return topic, buffer_size diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py deleted file mode 100644 index f96ceb73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/console_widget.py +++ /dev/null @@ -1,754 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QApplication, QCursor, QFileDialog, QHeaderView,QIcon, QMenu, QMessageBox, QTableView, QWidget -from python_qt_binding.QtCore import QRegExp, Qt, qWarning - -import time -import datetime - -from rqt_py_common.ini_helper import pack, unpack - -from .filters.custom_filter import CustomFilter -from .filters.location_filter import LocationFilter -from .filters.message_filter import MessageFilter -from .filters.node_filter import NodeFilter -from .filters.severity_filter import SeverityFilter -from .filters.time_filter import TimeFilter -from .filters.topic_filter import TopicFilter - -from .filters.custom_filter_widget import CustomFilterWidget -from .filters.filter_wrapper_widget import FilterWrapperWidget -from .filters.list_filter_widget import ListFilterWidget -from .filters.text_filter_widget import TextFilterWidget -from .filters.time_filter_widget import TimeFilterWidget - -from .message import Message -from .message_data_model import MessageDataModel - -from .text_browse_dialog import TextBrowseDialog - - -class ConsoleWidget(QWidget): - """ - Primary widget for the rqt_console plugin. - """ - def __init__(self, proxy_model, rospack, minimal=False): - """ - :param proxymodel: the proxy model to display in the widget,''QSortFilterProxyModel'' - :param minimal: if true the load, save and column buttons will be hidden as well as the filter splitter, ''bool'' - """ - super(ConsoleWidget, self).__init__() - self._proxy_model = proxy_model - self._model = self._proxy_model.sourceModel() - self._paused = False - self._rospack = rospack - - # These are lists of Tuples = (,) - self._exclude_filters = [] - self._highlight_filters = [] - - ui_file = os.path.join(self._rospack.get_path('rqt_console'), 'resource', 'console_widget.ui') - loadUi(ui_file, self) - - if minimal: - self.load_button.hide() - self.save_button.hide() - self.column_resize_button.hide() - self.setObjectName('ConsoleWidget') - self.table_view.setModel(proxy_model) - - self._columnwidth = (60, 100, 70, 100, 100, 100, 100) - for idx, width in enumerate(self._columnwidth): - self.table_view.horizontalHeader().resizeSection(idx, width) - - def update_sort_indicator(logical_index, order): - if logical_index == 0: - self._proxy_model.sort(-1) - self.table_view.horizontalHeader().setSortIndicatorShown(logical_index != 0) - self.table_view.horizontalHeader().sortIndicatorChanged.connect(update_sort_indicator) - - self.add_exclude_button.setIcon(QIcon.fromTheme('list-add')) - self.add_highlight_button.setIcon(QIcon.fromTheme('list-add')) - self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) - if not self.pause_button.icon().isNull(): - self.pause_button.setText('') - self.record_button.setIcon(QIcon.fromTheme('media-record')) - if not self.record_button.icon().isNull(): - self.record_button.setText('') - self.load_button.setIcon(QIcon.fromTheme('document-open')) - if not self.load_button.icon().isNull(): - self.load_button.setText('') - self.save_button.setIcon(QIcon.fromTheme('document-save')) - if not self.save_button.icon().isNull(): - self.save_button.setText('') - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - if not self.clear_button.icon().isNull(): - self.clear_button.setText('') - self.highlight_exclude_button.setIcon(QIcon.fromTheme('format-text-strikethrough')) - - self.pause_button.clicked[bool].connect(self._handle_pause_clicked) - self.record_button.clicked[bool].connect(self._handle_record_clicked) - self.load_button.clicked[bool].connect(self._handle_load_clicked) - self.save_button.clicked[bool].connect(self._handle_save_clicked) - self.column_resize_button.clicked[bool].connect(self._handle_column_resize_clicked) - self.clear_button.clicked[bool].connect(self._handle_clear_button_clicked) - - self.table_view.mouseDoubleClickEvent = self._handle_mouse_double_click - self.table_view.mousePressEvent = self._handle_mouse_press - self.table_view.keyPressEvent = self._handle_custom_keypress - - self.highlight_exclude_button.clicked[bool].connect(self._proxy_model.set_show_highlighted_only) - - self.add_highlight_button.clicked.connect(self._add_highlight_filter) - self.add_exclude_button.clicked.connect(self._add_exclude_filter) - - # Filter factory dictionary: - # index 0 is a label describing the widget, index 1 is the class that provides filtering logic - # index 2 is the widget that sets the data in the filter class, index 3 are the arguments for the widget class constructor - self._filter_factory_order = ['message', 'severity', 'node', 'time', 'topic', 'location', 'custom'] - self.filter_factory = {'message': (self.tr('...containing'), MessageFilter, TextFilterWidget), - 'severity': (self.tr('...with severities'), SeverityFilter, ListFilterWidget, self._model.get_severity_dict), - 'node': (self.tr('...from node'), NodeFilter, ListFilterWidget, self._model.get_unique_nodes), - 'time': (self.tr('...from time range'), TimeFilter, TimeFilterWidget, self.get_time_range_from_selection), - 'topic': (self.tr('...from topic'), TopicFilter, ListFilterWidget, self._model.get_unique_topics), - 'location': (self.tr('...from location'), LocationFilter, TextFilterWidget), - 'custom': (self.tr('Custom'), CustomFilter, CustomFilterWidget, [self._model.get_severity_dict, self._model.get_unique_nodes, self._model.get_unique_topics])} - - self._model.rowsInserted.connect(self.update_status) - self._model.rowsRemoved.connect(self.update_status) - self._proxy_model.rowsInserted.connect(self.update_status) - self._proxy_model.rowsRemoved.connect(self.update_status) - - # list of TextBrowserDialogs to close when cleaning up - self._browsers = [] - - # This defaults the filters panel to start by taking 50% of the available space - if minimal: - self.table_splitter.setSizes([1, 0]) - else: - self.table_splitter.setSizes([1, 1]) - self.exclude_table.resizeColumnsToContents() - self.highlight_table.resizeColumnsToContents() - - def get_message_summary(self, start_time_offset=None, end_time_offset=None): - """ - :param start_time: number of seconds before now to start, ''int'' (optional) - :param end_time: number of seconds before now to end, ''int'' (optional) - :returns: summary of message numbers within time - """ - current_time = time.mktime(datetime.datetime.now().timetuple()) - if start_time_offset is None: - start_time = current_time - 240 - else: - start_time = current_time - start_time_offset - if end_time_offset is not None: - end_time = current_time - end_time_offset - else: - end_time = None - - message_subset = self._model.get_message_between(start_time, end_time) - - class Message_Summary(object): - __slots__ = 'fatal', 'error', 'warn', 'info', 'debug' - - def __init__(self, messages): - self.fatal = 0 - self.error = 0 - self.warn = 0 - self.info = 0 - self.debug = 0 - for message in messages: - if message.severity == Message.DEBUG: - self.debug += 1 - elif message.severity == Message.INFO: - self.info += 1 - elif message.severity == Message.WARN: - self.warn += 1 - elif message.severity == Message.ERROR: - self.error += 1 - elif message.severity == Message.FATAL: - self.fatal += 1 - else: - assert False, "Unknown severity type '%s'" % str(message.severity) - - return Message_Summary(message_subset) - - def get_time_range_from_selection(self): - """ - :returns: the range of time of messages in the current table selection (min, max), ''tuple(str,str)'' - """ - rowlist = [] - indexes = self.table_view.selectionModel().selectedIndexes() - - if indexes: - rowlist = [self._proxy_model.mapToSource(current).row() for current in indexes] - rowlist = sorted(list(set(rowlist))) - - mintime, maxtime = self._model.get_time_range(rowlist) - return (mintime, maxtime) - return (-1, -1) - - def _delete_highlight_filter(self): - """ - Deletes any highlight filters which have a checked delete button - """ - for index, item in enumerate(self._highlight_filters): - if item[1].delete_button.isChecked(): - self._proxy_model.delete_highlight_filter(index) - self.highlight_table.removeCellWidget(index, 0) - self.highlight_table.removeRow(index) - item[0].filter_changed_signal.disconnect(self._proxy_model.handle_highlight_filters_changed) - item[1].delete_button.clicked.disconnect(self._delete_highlight_filter) - del self._highlight_filters[index] - - def _delete_exclude_filter(self): - """ - Deletes any exclude filters which have a checked delete button - """ - for index, item in enumerate(self._exclude_filters): - if item[1].delete_button.isChecked(): - self._proxy_model.delete_exclude_filter(index) - self.exclude_table.removeCellWidget(index, 0) - self.exclude_table.removeRow(index) - item[0].filter_changed_signal.disconnect(self._proxy_model.handle_exclude_filters_changed) - item[1].delete_button.clicked.disconnect(self._delete_exclude_filter) - del self._exclude_filters[index] - - def _add_highlight_filter(self, filter_index=False): - """ - :param filter_index: if false then this function shows a QMenu to allow the user to choose a type of message filter. ''bool'' - OR - :param filter_index: the index of the filter to be added, ''int'' - :return: if a filter was added then the index is returned, ''int'' - OR - :return: if no filter was added then None is returned, ''NoneType'' - """ - if filter_index is False: - filter_index = -1 - filter_select_menu = QMenu() - for index in self._filter_factory_order: - # flattens the _highlight filters list and only adds the item if it doesn't already exist - if index in ['message', 'location'] or not self.filter_factory[index][1] in [type(item) for sublist in self._highlight_filters for item in sublist]: - filter_select_menu.addAction(self.filter_factory[index][0]) - action = filter_select_menu.exec_(QCursor.pos()) - if action is None: - return - for index in self._filter_factory_order: - if self.filter_factory[index][0] == action.text(): - filter_index = index - if filter_index == -1: - return - - index = len(self._highlight_filters) - newfilter = self.filter_factory[filter_index][1]() - if len(self.filter_factory[filter_index]) >= 4: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack, self.filter_factory[filter_index][3]) - else: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack) - - # pack the new filter tuple onto the filter list - self._highlight_filters.append((newfilter, FilterWrapperWidget(newwidget, self.filter_factory[filter_index][0]), filter_index)) - self._proxy_model.add_highlight_filter(newfilter) - newfilter.filter_changed_signal.connect(self._proxy_model.handle_highlight_filters_changed) - self._highlight_filters[index][1].delete_button.clicked.connect(self._delete_highlight_filter) - self._model.rowsInserted.connect(self._highlight_filters[index][1].repopulate) - - # place the widget in the proper location - self.highlight_table.insertRow(index) - self.highlight_table.setCellWidget(index, 0, self._highlight_filters[index][1]) - self.highlight_table.resizeColumnsToContents() - self.highlight_table.resizeRowsToContents() - newfilter.filter_changed_signal.emit() - return index - - def _add_exclude_filter(self, filter_index=False): - """ - :param filter_index: if false then this function shows a QMenu to allow the user to choose a type of message filter. ''bool'' - OR - :param filter_index: the index of the filter to be added, ''int'' - :return: if a filter was added then the index is returned, ''int'' - OR - :return: if no filter was added then None is returned, ''NoneType'' - """ - if filter_index is False: - filter_index = -1 - filter_select_menu = QMenu() - for index in self._filter_factory_order: - # flattens the _exclude filters list and only adds the item if it doesn't already exist - if index in ['message', 'location'] or not self.filter_factory[index][1] in [type(item) for sublist in self._exclude_filters for item in sublist]: - filter_select_menu.addAction(self.filter_factory[index][0]) - action = filter_select_menu.exec_(QCursor.pos()) - if action is None: - return None - for index in self._filter_factory_order: - if self.filter_factory[index][0] == action.text(): - filter_index = index - if filter_index == -1: - return None - - index = len(self._exclude_filters) - newfilter = self.filter_factory[filter_index][1]() - if len(self.filter_factory[filter_index]) >= 4: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack, self.filter_factory[filter_index][3]) - else: - newwidget = self.filter_factory[filter_index][2](newfilter, self._rospack) - - # pack the new filter tuple onto the filter list - self._exclude_filters.append((newfilter, FilterWrapperWidget(newwidget, self.filter_factory[filter_index][0]), filter_index)) - self._proxy_model.add_exclude_filter(newfilter) - newfilter.filter_changed_signal.connect(self._proxy_model.handle_exclude_filters_changed) - self._exclude_filters[index][1].delete_button.clicked.connect(self._delete_exclude_filter) - self._model.rowsInserted.connect(self._exclude_filters[index][1].repopulate) - - # place the widget in the proper location - self.exclude_table.insertRow(index) - self.exclude_table.setCellWidget(index, 0, self._exclude_filters[index][1]) - self.exclude_table.resizeColumnsToContents() - self.exclude_table.resizeRowsToContents() - newfilter.filter_changed_signal.emit() - return index - - def _process_highlight_exclude_filter(self, selection, selectiontype, exclude=False): - """ - Modifies the relevant filters (based on selectiontype) to remove (exclude=True) - or highlight (exclude=False) the selection from the dataset in the tableview. - :param selection: the actual selection, ''str'' - :param selectiontype: the type of selection, ''str'' - :param exclude: If True process as an exclude filter, False process as an highlight filter, ''bool'' - """ - types = {self.tr('Node'): 2, self.tr('Topic'): 4, self.tr('Severity'): 1, self.tr('Message'): 0} - try: - col = types[selectiontype] - except: - raise RuntimeError("Bad Column name in ConsoleWidget._process_highlight_exclude_filter()") - - if col == 0: - unique_messages = set() - selected_indexes = self.table_view.selectionModel().selectedIndexes() - num_selected = len(selected_indexes) / 6 - for index in range(num_selected): - unique_messages.add(selected_indexes[num_selected * col + index].data()) - unique_messages = list(unique_messages) - for message in unique_messages: - message = message.replace('\\', '\\\\') - message = message.replace('.', '\\.') - if exclude: - filter_index = self._add_exclude_filter(selectiontype.lower()) - filter_widget = self._exclude_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - else: - filter_index = self._add_highlight_filter(col) - filter_widget = self._highlight_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.set_regex(True) - filter_widget.set_text('^' + message + '$') - - else: - if exclude: - # Test if the filter we are adding already exists if it does use the existing filter - if self.filter_factory[selectiontype.lower()][1] not in [type(item) for sublist in self._exclude_filters for item in sublist]: - filter_index = self._add_exclude_filter(selectiontype.lower()) - else: - for index, item in enumerate(self._exclude_filters): - if type(item[0]) == self.filter_factory[selectiontype.lower()][1]: - filter_index = index - else: - # Test if the filter we are adding already exists if it does use the existing filter - if self.filter_factory[selectiontype.lower()][1] not in [type(item) for sublist in self._highlight_filters for item in sublist]: - filter_index = self._add_highlight_filter(col) - else: - for index, item in enumerate(self._highlight_filters): - if type(item[0]) == self.filter_factory[selectiontype.lower()][1]: - filter_index = index - - if exclude: - filter_widget = self._exclude_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.select_item(selection) - else: - filter_widget = self._highlight_filters[filter_index][1].findChildren(QWidget, QRegExp('.*FilterWidget.*'))[0] - filter_widget.select_item(selection) - - def _rightclick_menu(self, event): - """ - Dynamically builds the rightclick menu based on the unique column data - from the passed in datamodel and then launches it modally - :param event: the mouse event object, ''QMouseEvent'' - """ - severities = {} - for severity, label in Message.SEVERITY_LABELS.items(): - if severity in self._model.get_unique_severities(): - severities[severity] = label - nodes = sorted(self._model.get_unique_nodes()) - topics = sorted(self._model.get_unique_topics()) - - # menutext entries turned into - menutext = [] - menutext.append([self.tr('Exclude'), [[self.tr('Severity'), severities], [self.tr('Node'), nodes], [self.tr('Topic'), topics], [self.tr('Selected Message(s)')]]]) - menutext.append([self.tr('Highlight'), [[self.tr('Severity'), severities], [self.tr('Node'), nodes], [self.tr('Topic'), topics], [self.tr('Selected Message(s)')]]]) - menutext.append([self.tr('Copy Selected')]) - menutext.append([self.tr('Browse Selected')]) - - menu = QMenu() - submenus = [] - subsubmenus = [] - for item in menutext: - if len(item) > 1: - submenus.append(QMenu(item[0], menu)) - for subitem in item[1]: - if len(subitem) > 1: - subsubmenus.append(QMenu(subitem[0], submenus[-1])) - if isinstance(subitem[1], dict): - for key in sorted(subitem[1].keys()): - action = subsubmenus[-1].addAction(subitem[1][key]) - action.setData(key) - else: - for subsubitem in subitem[1]: - subsubmenus[-1].addAction(subsubitem) - submenus[-1].addMenu(subsubmenus[-1]) - else: - submenus[-1].addAction(subitem[0]) - menu.addMenu(submenus[-1]) - else: - menu.addAction(item[0]) - action = menu.exec_(event.globalPos()) - - if action is None or action == 0: - return - elif action.text() == self.tr('Browse Selected'): - self._show_browsers() - elif action.text() == self.tr('Copy Selected'): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - copytext = self._model.get_selected_text(rowlist) - if copytext is not None: - clipboard = QApplication.clipboard() - clipboard.setText(copytext) - elif action.text() == self.tr('Selected Message(s)'): - if action.parentWidget().title() == self.tr('Highlight'): - self._process_highlight_exclude_filter(action.text(), 'Message', False) - elif action.parentWidget().title() == self.tr('Exclude'): - self._process_highlight_exclude_filter(action.text(), 'Message', True) - else: - raise RuntimeError("Menu format corruption in ConsoleWidget._rightclick_menu()") - else: - # This processes the dynamic list entries (severity, node and topic) - try: - roottitle = action.parentWidget().parentWidget().title() - except: - raise RuntimeError("Menu format corruption in ConsoleWidget._rightclick_menu()") - - if roottitle == self.tr('Highlight'): - self._process_highlight_exclude_filter(action.text(), action.parentWidget().title(), False) - elif roottitle == self.tr('Exclude'): - self._process_highlight_exclude_filter(action.text(), action.parentWidget().title(), True) - else: - raise RuntimeError("Unknown Root Action %s selected in ConsoleWidget._rightclick_menu()" % roottitle) - - def update_status(self): - """ - Sets the message display label to the current value - """ - if self._model.rowCount() == self._proxy_model.rowCount(): - tip = self.tr('Displaying %d messages') % (self._model.rowCount()) - else: - tip = self.tr('Displaying %d of %d messages') % (self._proxy_model.rowCount(), self._model.rowCount()) - self.messages_label.setText(tip) - - def cleanup_browsers_on_close(self): - for browser in self._browsers: - browser.close() - - def _show_browsers(self): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - browsetext = self._model.get_selected_text(rowlist) - if browsetext is not None: - self._browsers.append(TextBrowseDialog(browsetext, self._rospack)) - self._browsers[-1].show() - - def _handle_clear_button_clicked(self, checked): - self._model.remove_rows([]) - Message._next_id = 1 - - def _handle_load_clicked(self, checked): - filename = QFileDialog.getOpenFileName(self, self.tr('Load from File'), '.', self.tr('rqt_console message file {.csv} (*.csv)')) - if filename[0] != '': - try: - with open(filename[0], 'r') as h: - lines = h.read().splitlines() - except IOError as e: - qWarning(str(e)) - return False - - # extract column header - columns = lines[0].split(';') - if len(lines) < 2: - return True - - # join wrapped lines - rows = [] - last_wrapped = False - for line in lines[1:]: - # ignore empty lines - if not line: - continue - # check for quotes and remove them - if line == '"': - has_prefix = not last_wrapped - has_suffix = last_wrapped - line = '' - else: - has_prefix = line[0] == '"' - if has_prefix: - line = line[1:] - has_suffix = line[-1] == '"' - if has_suffix: - line = line[:-1] - - # ignore line without prefix if previous line was not wrapped - if not has_prefix and not last_wrapped: - continue - # remove wrapped line which is not continued on the next line - if last_wrapped and has_prefix: - rows.pop() - - # add/append lines - if last_wrapped: - rows[-1] += line - else: - # add line without quote prefix - rows.append(line) - - last_wrapped = not has_suffix - - # generate message for each row - messages = [] - skipped = [] - for row in rows: - data = row.split('";"') - msg = Message() - msg.set_stamp_format('hh:mm:ss.ZZZ (yyyy-MM-dd)') - for i, column in enumerate(columns): - value = data[i] - if column == 'message': - msg.message = value.replace('\\"', '"') - elif column == 'severity': - msg.severity = int(value) - if msg.severity not in Message.SEVERITY_LABELS: - skipped.append('Unknown severity value: %s' % value) - msg = None - break - elif column == 'stamp': - parts = value.split('.') - if len(parts) != 2: - skipped.append('Unknown timestamp format: %s' % value) - msg = None - break - msg.stamp = (int(parts[0]), int(parts[1])) - elif column == 'topics': - msg.topics = value.split(',') - elif column == 'node': - msg.node = value - elif column == 'location': - msg.location = value - else: - skipped.append('Unknown column: %s' % column) - msg = None - break - if msg: - messages.append(msg) - if skipped: - qWarning('Skipped %d rows since they do not appear to be in rqt_console message file format:\n- %s' % (len(skipped), '\n- '.join(skipped))) - - if messages: - self._model.insert_rows(messages) - - self._handle_pause_clicked(True) - - return True - - else: - qWarning('File does not appear to be a rqt_console message file: missing file header.') - return False - - def _handle_save_clicked(self, checked): - filename = QFileDialog.getSaveFileName(self, 'Save to File', '.', self.tr('rqt_console msg file {.csv} (*.csv)')) - if filename[0] != '': - filename = filename[0] - if filename[-4:] != '.csv': - filename += '.csv' - try: - handle = open(filename, 'w') - except IOError as e: - qWarning(str(e)) - return - try: - handle.write(';'.join(MessageDataModel.columns) + '\n') - for index in range(self._proxy_model.rowCount()): - row = self._proxy_model.mapToSource(self._proxy_model.index(index, 0)).row() - msg = self._model._messages[row] - data = {} - data['message'] = msg.message.replace('"', '\\"') - data['severity'] = str(msg.severity) - data['node'] = msg.node - data['stamp'] = str(msg.stamp[0]) + '.' + str(msg.stamp[1]).zfill(9) - data['topics'] = ','.join(msg.topics) - data['location'] = msg.location - line = [] - for column in MessageDataModel.columns: - line.append('"%s"' % data[column]) - handle.write(';'.join(line) + '\n') - except Exception as e: - qWarning('File save failed: %s' % str(e)) - return False - finally: - handle.close() - return True - - def _handle_pause_clicked(self): - self._paused = True - self.pause_button.setVisible(False) - self.record_button.setVisible(True) - - def _handle_record_clicked(self): - self._paused = False - self.pause_button.setVisible(True) - self.record_button.setVisible(False) - - def _handle_column_resize_clicked(self): - self.table_view.resizeColumnsToContents() - - def _delete_selected_rows(self): - rowlist = [] - for current in self.table_view.selectionModel().selectedIndexes(): - rowlist.append(self._proxy_model.mapToSource(current).row()) - rowlist = list(set(rowlist)) - return self._model.remove_rows(rowlist) - - def _handle_custom_keypress(self, event, old_keyPressEvent=QTableView.keyPressEvent): - """ - Handles the delete key. - The delete key removes the tableview's selected rows from the datamodel - """ - if event.key() == Qt.Key_Delete and len(self._model._messages) > 0: - delete = QMessageBox.Yes - if len(self.table_view.selectionModel().selectedIndexes()) == 0: - delete = QMessageBox.question(self, self.tr('Message'), self.tr("Are you sure you want to delete all messages?"), QMessageBox.Yes | QMessageBox.No, QMessageBox.No) - if delete == QMessageBox.Yes and event.key() == Qt.Key_Delete and event.modifiers() == Qt.NoModifier: - if self._delete_selected_rows(): - event.accept() - return old_keyPressEvent(self.table_view, event) - - def _handle_mouse_double_click(self, event, old_doubleclickevent=QTableView.mouseDoubleClickEvent): - if event.buttons() & Qt.LeftButton and event.modifiers() == Qt.NoModifier: - self._show_browsers() - event.accept() - return old_doubleclickevent(self.table_view, event) - - def _handle_mouse_press(self, event, old_pressEvent=QTableView.mousePressEvent): - if event.buttons() & Qt.RightButton and event.modifiers() == Qt.NoModifier: - self._rightclick_menu(event) - event.accept() - return old_pressEvent(self.table_view, event) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('settings_exist', True) - - instance_settings.set_value('table_splitter', self.table_splitter.saveState()) - instance_settings.set_value('filter_splitter', self.filter_splitter.saveState()) - - instance_settings.set_value('paused', self._paused) - instance_settings.set_value('show_highlighted_only', self.highlight_exclude_button.isChecked()) - - exclude_filters = [] - for index, item in enumerate(self._exclude_filters): - exclude_filters.append(item[2]) - filter_settings = instance_settings.get_settings('exclude_filter_' + str(index)) - item[1].save_settings(filter_settings) - instance_settings.set_value('exclude_filters', pack(exclude_filters)) - - highlight_filters = [] - for index, item in enumerate(self._highlight_filters): - highlight_filters.append(item[2]) - filter_settings = instance_settings.get_settings('highlight_filter_' + str(index)) - item[1].save_settings(filter_settings) - instance_settings.set_value('highlight_filters', pack(highlight_filters)) - instance_settings.set_value('message_limit', self._model.get_message_limit()) - - def restore_settings(self, pluggin_settings, instance_settings): - if instance_settings.contains('table_splitter'): - self.table_splitter.restoreState(instance_settings.value('table_splitter')) - if instance_settings.contains('filter_splitter'): - self.filter_splitter.restoreState(instance_settings.value('filter_splitter')) - else: - self.filter_splitter.setSizes([1, 1]) - - paused = instance_settings.value('paused') in [True, 'true'] - if paused: - self._handle_pause_clicked() - else: - self._handle_record_clicked() - self.highlight_exclude_button.setChecked(instance_settings.value('show_highlighted_only') in [True, 'true']) - self._proxy_model.set_show_highlighted_only(self.highlight_exclude_button.isChecked()) - - for item in self._exclude_filters: - item[1].delete_button.setChecked(True) - self._delete_exclude_filter() - if instance_settings.contains('exclude_filters'): - exclude_filters = unpack(instance_settings.value('exclude_filters')) - if exclude_filters is not None: - for index, item in enumerate(exclude_filters): - self._add_exclude_filter(item) - filter_settings = instance_settings.get_settings('exclude_filter_' + str(index)) - self._exclude_filters[-1][1].restore_settings(filter_settings) - else: - self._add_exclude_filter('severity') - - for item in self._highlight_filters: - item[1].delete_button.setChecked(True) - self._delete_highlight_filter() - if instance_settings.contains('highlight_filters'): - highlight_filters = unpack(instance_settings.value('highlight_filters')) - if highlight_filters is not None: - for index, item in enumerate(highlight_filters): - self._add_highlight_filter(item) - filter_settings = instance_settings.get_settings('highlight_filter_' + str(index)) - self._highlight_filters[-1][1].restore_settings(filter_settings) - else: - self._add_highlight_filter('message') - - if instance_settings.contains('message_limit'): - self._model.set_message_limit(int(instance_settings.value('message_limit'))) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/__init__.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py deleted file mode 100644 index 8dda1006..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/base_filter.py +++ /dev/null @@ -1,77 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QObject, QTimer, Signal - - -class BaseFilter(QObject): - """ - Contains basic functions common to all filters. - Handles enabled logic and change notification. - """ - filter_changed_signal = Signal() - - def __init__(self): - super(BaseFilter, self).__init__() - self._enabled = True - - self._timer = QTimer(self) - self._timer.setSingleShot(True) - self._timer.timeout.connect(self.filter_changed_signal.emit) - - def start_emit_timer(self, msec=None): - """ - Starts a timer to emit a signal to refresh the filters after the filter is changed - :param msec: number of msecs to wait before emitting the signal to change the filter ''int'' - """ - if msec is None: - self._timer.start(10) - else: - self._timer.start(msec) - - def is_enabled(self): - return self._enabled - - def set_enabled(self, checked): - """ - Setter for _enabled - :param checked: boolean flag to set ''bool'' - :emits filter_changed_signal: Always - """ - self._enabled = checked - self.start_emit_timer(200) - - def has_filter(self): - raise NotImplementedError() - - def test_message(self, message): - raise NotImplementedError() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py deleted file mode 100644 index 2e42e473..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter.py +++ /dev/null @@ -1,93 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter -from .message_filter import MessageFilter -from .node_filter import NodeFilter -from .severity_filter import SeverityFilter -from .topic_filter import TopicFilter - - -class CustomFilter(BaseFilter): - """ - Contains filter logic for the custom filter which allows message, severity, - node and topic filtering simultaniously. All of these filters must match - together (if they are used) or the custom filter does not match. - """ - - def __init__(self): - super(CustomFilter, self).__init__() - - self._message = MessageFilter() - self._severity = SeverityFilter() - self._node = NodeFilter() - self._topic = TopicFilter() - - self._all_filters = [self._message, self._severity, self._node, self._topic] - for f in self._all_filters: - f.filter_changed_signal.connect(self._relay_signal) - - def set_enabled(self, checked): - """ - :signal: emits filter_changed_signal - :param checked: enables the filters if checked is True''bool'' - """ - for f in self._all_filters: - f.set_enabled(checked) - super(CustomFilter, self).set_enabled(checked) - - def _relay_signal(self): - """ - Passes any signals emitted by the child filters along - """ - self.start_emit_timer(1) - - def has_filter(self): - for f in self._all_filters: - if f.has_filter(): - return True - return False - - def test_message(self, message): - """ - Tests if the message matches the filter. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches all child filters, ''bool'' - """ - if not self.is_enabled(): - return False - # if non of the subfilters contains any input the custom filter does not match - if not self.has_filter(): - return False - # the custom filter matches when all subfilters which contain input match - all_filters = [not f.has_filter() or f.test_message(message) for f in self._all_filters] - return False not in all_filters diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py deleted file mode 100644 index 22b6d57b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/custom_filter_widget.py +++ /dev/null @@ -1,180 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QPalette, QWidget - -from rqt_py_common.ini_helper import pack, unpack - - -class CustomFilterWidget(QWidget): - def __init__(self, parentfilter, rospack, item_providers): - super(CustomFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'custom_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('CustomFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - # keep color for highlighted items even when not active - for list_widget in [self.severity_list, self.node_list, self.topic_list]: - active_color = list_widget.palette().brush(QPalette.Active, QPalette.Highlight).color().name() - list_widget.setStyleSheet('QListWidget:item:selected:!active { background: %s; }' % active_color) - - # Text Filter Initialization - self.text_edit.textChanged.connect(self.handle_text_changed) - self.regex_check_box.clicked[bool].connect(self.handle_regex_clicked) - self.handle_text_changed() - - # Severity Filter Initialization - self.severity_list.itemSelectionChanged.connect(self.handle_severity_item_changed) - new_items = item_providers[0]() - for key in sorted(new_items.keys()): - item = new_items[key] - self.severity_list.addItem(item) - self.severity_list.item(self.severity_list.count() - 1).setData(Qt.UserRole, key) - - # Node Filter Initialization - self._node_list_populate_function = item_providers[1] - self.node_list.itemSelectionChanged.connect(self.handle_node_item_changed) - - # Topic Filter Initialization - self._topic_list_populate_function = item_providers[2] - self.topic_list.itemSelectionChanged.connect(self.handle_topic_item_changed) - - self.repopulate() - - def handle_node_item_changed(self): - self._parentfilter._node.set_selected_items(self.node_list.selectedItems()) - - def handle_topic_item_changed(self): - self._parentfilter._topic.set_selected_items(self.topic_list.selectedItems()) - - def handle_severity_item_changed(self): - self._parentfilter._severity.set_selected_items(self.severity_list.selectedItems()) - - def handle_text_changed(self): - self._parentfilter._message.set_text(self.text_edit.text()) - - def handle_regex_clicked(self, clicked): - self._parentfilter._message.set_regex(clicked) - - def repopulate(self): - """ - Repopulates the display widgets based on the function arguments passed - in during initialization - """ - newset = self._topic_list_populate_function() - for item in newset: - if len(self.topic_list.findItems(item, Qt.MatchExactly)) == 0: - self._add_item(self.topic_list, item) - - newset = self._node_list_populate_function() - for item in newset: - if len(self.node_list.findItems(item, Qt.MatchExactly)) == 0: - self._add_item(self.node_list, item) - - def _add_item(self, list_widget, item): - """ - Insert item in alphabetical order. - """ - for i in range(list_widget.count()): - if item <= list_widget.item(i).text(): - list_widget.insertItem(i, item) - return - list_widget.addItem(item) - - def save_settings(self, settings): - """ - Saves the settings for this filter to an ini file. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('text', self._parentfilter._message._text) - settings.set_value('regex', self._parentfilter._message._regex) - - settings.set_value('severityitemlist', pack(self._parentfilter._severity._selected_items)) - - settings.set_value('topicitemlist', pack(self._parentfilter._topic._selected_items)) - - settings.set_value('nodeitemlist', pack(self._parentfilter._node._selected_items)) - - return - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - text = settings.value('text', '') - self.text_edit.setText(text) - self.handle_text_changed() - - regex = settings.value('regex') in [True, 'true'] - self.regex_check_box.setChecked(regex) - self.handle_regex_clicked(regex) - - #Restore Severities - for index in range(self.severity_list.count()): - self.severity_list.item(index).setSelected(False) - severity_item_list = unpack(settings.value('severityitemlist')) - for item in severity_item_list: - items = self.severity_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_severity_item_changed() - - #Restore Topics - for index in range(self.topic_list.count()): - self.topic_list.item(index).setSelected(False) - topic_item_list = unpack(settings.value('topicitemlist')) - for item in topic_item_list: - if len(self.topic_list.findItems(item, Qt.MatchExactly)) == 0: - self.topic_list.addItem(item) - items = self.topic_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_topic_item_changed() - - #Restore Nodes - for index in range(self.node_list.count()): - self.node_list.item(index).setSelected(False) - node_item_list = unpack(settings.value('nodeitemlist')) - for item in node_item_list: - if len(self.node_list.findItems(item, Qt.MatchExactly)) == 0: - self.node_list.addItem(item) - items = self.node_list.findItems(item, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_node_item_changed() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py deleted file mode 100644 index 841be238..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_collection.py +++ /dev/null @@ -1,68 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class FilterCollection: - """ - Collection of filters to test messages against. - """ - def __init__(self): - self._filters = [] - - def test_message(self, message, default=False): - """ - Test if the message matches any filter. - :param message: message to be tested against the filters, ''Message'' - :param default: return value when there is no active filter, ''bool'' - :returns: True if the message matches any filter, ''bool'' - """ - match = default - for f in self._filters: - if f.is_enabled() and f.has_filter(): - if f.test_message(message): - return True - else: - match = False - return match - - def append(self, filter): - self._filters.append(filter) - - def count_enabled_filters(self): - enabled = [f for f in self._filters if f.is_enabled()] - return len(enabled) - - def __len__(self): - return len(self._filters) - - def __delitem__(self, index): - del self._filters[index] diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py deleted file mode 100644 index 5d7a1025..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/filter_wrapper_widget.py +++ /dev/null @@ -1,94 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QIcon, QWidget - - -class FilterWrapperWidget(QWidget): - """ - Wraps the other filter widgets to provide enable check box, delete button control and uniform labeling - """ - def __init__(self, widget, filter_name): - """ - :param widget: the Qwidget to wrap ''Qwidget'' - :param filter_name: the name to be placed on the label ''str'' - """ - super(FilterWrapperWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_console'), 'resource/filters', 'filter_wrapper_widget.ui') - loadUi(ui_file, self) - self.setObjectName('FilterWrapperWidget') - self.delete_button.setIcon(QIcon.fromTheme('list-remove')) - self._widget = widget - - # Replace the placeholder QWidget with the passed in object - stretch = self.layout_frame.stretch(2) - self.layout_frame.insertWidget(2, widget) - self.layout_frame.setStretch(2, stretch) - - # This line sets the place holder to 0 width so it can no longer be seen - # It is a hack since removing it causes other widgets not to function properly - self.layout_frame.setStretch(3, 0) - - self.enabled_checkbox.clicked[bool].connect(self.enabled_callback) - self.filter_name_label.setText(filter_name + ':') - - def enabled_callback(self, checked): - self._widget._parentfilter.set_enabled(checked) - self._widget.setEnabled(checked) - - def repopulate(self): - self._widget.repopulate() - - def save_settings(self, settings): - """ - Handles writing the enabled flag to the ini file and then passes the - settings object to the wrapped widget - - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('enabled', self._widget._parentfilter._enabled) - self._widget.save_settings(settings) - - def restore_settings(self, settings): - """ - Handles reading the enabled flag from the ini file. - :param settings: used to read the settings to an ini file ''qt_gui.settings.Settings'' - """ - checked = settings.value('enabled') in [True, 'true'] - self.enabled_callback(checked) - self.enabled_checkbox.setChecked(checked) - self._widget.restore_settings(settings) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py deleted file mode 100644 index 46c9ff3a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/list_filter_widget.py +++ /dev/null @@ -1,133 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QPalette, QWidget - -from rqt_py_common.ini_helper import pack, unpack - - -class ListFilterWidget(QWidget): - """ - Generic List widget to be used when implementing filters that require - limited dynamic selections - """ - def __init__(self, parentfilter, rospack, item_provider): - """ - :param parentfilter: The filter object, must implement set_list and - contain _list ''QObject'' - :param item_provider: a function designed to provide a list or dict - """ - super(ListFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'list_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('ListFilterWidget') - self._parentfilter = parentfilter # When data is changed we need to store it in the parent filter - - # keep color for highlighted items even when not active - active_color = self.palette().brush(QPalette.Active, QPalette.Highlight).color().name() - self.setStyleSheet('QListWidget:item:selected:!active { background: %s; }' % active_color) - - self._list_populate_function = item_provider - self.list_widget.itemSelectionChanged.connect(self.handle_item_changed) - self._display_list = [] - self.repopulate() - - def select_item(self, text): - """ - All items matching text will be selected in the list_widget - :param item: a string to be matched against the list ''str'' - """ - items = self.list_widget.findItems(text, Qt.MatchExactly) - for item in items: - item.setSelected(True) - self.handle_item_changed() - - def handle_item_changed(self): - self._parentfilter.set_selected_items(self.list_widget.selectedItems()) - - def repopulate(self): - """ - Repopulates the display widgets based on the function arguments passed - in during initialization - """ - new_items = self._list_populate_function() - - new_set = set(new_items.values() if isinstance(new_items, dict) else new_items) - - if len(new_items) != len(self._display_list): - if isinstance(new_items, dict): - # for dictionaries store the key as user role data - for key in sorted(new_items.keys()): - item = new_items[key] - if item not in self._display_list: - self.list_widget.addItem(item) - self.list_widget.item(self.list_widget.count() - 1).setData(Qt.UserRole, key) - else: - for item in new_items: - if item not in self._display_list: - self._add_item(item) - self._display_list = sorted(set(new_set) | set(self._display_list)) - - def _add_item(self, item): - """ - Insert item in alphabetical order. - """ - for i in range(self.list_widget.count()): - if item <= self.list_widget.item(i).text(): - self.list_widget.insertItem(i, item) - return - self.list_widget.addItem(item) - - def save_settings(self, settings): - """ - Saves the settings for this filter. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('itemlist', pack(self._parentfilter._selected_items)) - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - for index in range(self.list_widget.count()): - self.list_widget.item(index).setSelected(False) - item_list = unpack(settings.value('itemlist')) - for item in item_list: - if len(self.list_widget.findItems(item, Qt.MatchExactly)) == 0: - self.list_widget.addItem(item) - self.select_item(item) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py deleted file mode 100644 index 7d3acb5f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/location_filter.py +++ /dev/null @@ -1,45 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .message_filter import MessageFilter - - -class LocationFilter(MessageFilter): - """ - Contains filter logic for a location filter. - """ - - def __init__(self): - super(LocationFilter, self).__init__() - - def test_message(self, message): - return self._test_message(message.location) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py deleted file mode 100644 index 21513910..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/message_filter.py +++ /dev/null @@ -1,104 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QRegExp -from .base_filter import BaseFilter - - -class MessageFilter(BaseFilter): - """ - Contains filter logic for a message filter. If the regex flag is False - simple 'is this in that' text matching is used on _text. If the regex flag is True - _text is treated as a regular expression with one exception. If it does not - start with a ^ a .* is appended, and if it does not end with a $ then a .* - is added to the end. - The filter_changed signal should be connected to a slot which notifies the - overall filtering system that it needs to reevaluate all entries. - """ - - def __init__(self): - super(MessageFilter, self).__init__() - self._text = '' - self._regex = False - - def set_text(self, text): - """ - Setter for _text - :param text: text to set ''str'' - :emits filter_changed_signal: If _enabled is true - """ - self._text = text - if self.is_enabled(): - self.start_emit_timer(500) - - def set_regex(self, checked): - """ - Setter for _regex - :param checked: boolean flag to set ''bool'' - :emits filter_changed_signal: If _enabled is true - """ - self._regex = checked - if self.is_enabled(): - self.start_emit_timer(500) - - def has_filter(self): - return self._text != '' - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the regex flag is False simple 'is this in that' text matching is used - on _text. If the regex flag is True _text is treated as a regular expression - with one exception. If it does not start with a ^ a .* is appended, and if - it does not end with a $ then a .* is added to the end. - - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - return self._test_message(message.message) - - def _test_message(self, value): - if not self.is_enabled(): - return False - if self._text != '': - if self._regex: - temp = self._text - if temp[0] != '^': - temp = '.*' + temp - if temp[-1] != '$': - temp += '.*' - if QRegExp(temp).exactMatch(value): - return True - else: - if value.find(self._text) != -1: - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py deleted file mode 100644 index 7787674f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/node_filter.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - - -class NodeFilter(BaseFilter): - """ - Contains filter logic for a single node filter. - If the message's node text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(NodeFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_" list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's node text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if message.node == item.text(): - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py deleted file mode 100644 index a4bd2530..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/severity_filter.py +++ /dev/null @@ -1,74 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - -from python_qt_binding.QtCore import Qt - - -class SeverityFilter(BaseFilter): - """ - Contains filter logic for a severity filter. - If the message's severity text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(SeverityFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_: list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's severity text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if message.severity == item.data(Qt.UserRole): - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py deleted file mode 100644 index a8b0373b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/text_filter_widget.py +++ /dev/null @@ -1,97 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - - -class TextFilterWidget(QWidget): - def __init__(self, parentfilter, rospack): - """ - Widget for displaying interactive data related to text filtering. - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - """ - super(TextFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'text_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TextFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - self.text_edit.textChanged.connect(self.handle_text_changed) - self.regex_check_box.clicked[bool].connect(self.handle_regex_clicked) - - self.handle_text_changed() - - def set_text(self, text): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.text_edit.setText(text) - - def set_regex(self, checked): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.regex_check_box.setChecked(checked) - self.handle_regex_clicked(checked) - - def handle_text_changed(self): - self._parentfilter.set_text(self.text_edit.text()) - - def handle_regex_clicked(self, clicked): - self._parentfilter.set_regex(clicked) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - settings.set_value('text', self._parentfilter._text) - settings.set_value('regex', self._parentfilter._regex) - - def restore_settings(self, settings): - text = settings.value('text', '') - self.set_text(text) - self.handle_text_changed() - - regex = settings.value('regex') in [True, 'true'] - self.set_regex(regex) - self.handle_regex_clicked(regex) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py deleted file mode 100644 index 37fc08bb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter.py +++ /dev/null @@ -1,100 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QDateTime -from .base_filter import BaseFilter - - -class TimeFilter(BaseFilter): - """ - Contains filter logic for a time filter. - If _stop_time_enabled is true then the message's time value must be between the dates provided - to be considered a match - If _stop_time_enabled is false then the time must simply be after _start_time - """ - - def __init__(self): - super(TimeFilter, self).__init__() - self._start_time = QDateTime() - self._stop_time = QDateTime() - self._stop_time_enabled = True - - def set_start_time(self, time): - """ - Setter for _start_time - :param time" start datetime for filter ''QDateTime'' - :emits filter_changed_signal: If _enabled is true - """ - self._start_time = time - if self.is_enabled(): - self.start_emit_timer() - - def set_stop_time(self, time): - """ - Setter for _stop_time - :param time" stop datetime for filter ''QDateTime'' - :emits filter_changed_signal: If _enabled is true - """ - self._stop_time = time - if self.is_enabled(): - self.start_emit_timer() - - def set_stop_time_enabled(self, checked): - """ - Setter for _stop_time_enabled - :param checked" boolean flag to set ''bool'' - :emits filter_changed_signal: If _enabled is true - """ - self._stop_time_enabled = checked - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return True - - def test_message(self, message): - """ - Tests if the message matches the filter. - If _stop_time_enabled is true then the message's time value must be between the dates provided - to be considered a match - If _stop_time_enabled is false then the time must simply be after _start_time - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - message_time = message.get_stamp_as_qdatetime() - if message_time < self._start_time: - return False - if self._stop_time_enabled and self._stop_time < message_time: - return False - return True diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py deleted file mode 100644 index 858e4a3d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/time_filter_widget.py +++ /dev/null @@ -1,121 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from datetime import datetime -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QDateTime -from python_qt_binding.QtGui import QWidget - - -class TimeFilterWidget(QWidget): - def __init__(self, parentfilter, rospack, time_range_provider): - """ - Widget for displaying interactive data related to time filtering. - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - :param display_list_args: single element list containing one tuple with - the min and max time to be displayed, ''list of tuple'' - """ - super(TimeFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource/filters', 'time_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TimeFilterWidget') - self._parentfilter = parentfilter # When data is changed it is stored in the parent filter - - self.start_datetime.dateTimeChanged[QDateTime].connect(self.handle_start_changed) - self.stop_datetime.dateTimeChanged[QDateTime].connect(self.handle_stop_changed) - self.stop_enabled_check_box.clicked[bool].connect(self.handle_stop_enabled_changed) - - # Times are passed in unixtimestamp '.' decimal fraction of a second - mintime, maxtime = time_range_provider() - if mintime != -1: - mintime = str(mintime).split('.') - maxtime = str(maxtime).split('.') - - time = QDateTime() - time.setTime_t(int(mintime[0])) - mintime = time.addMSecs(int(str(mintime[1]).zfill(9)[:3])) - self.start_datetime.setDateTime(mintime) - time.setTime_t(int(maxtime[0])) - maxtime = time.addMSecs(int(str(maxtime[1]).zfill(9)[:3])) - self.stop_datetime.setDateTime(maxtime) - else: - self.start_datetime.setDateTime(datetime.now()) - self.stop_datetime.setDateTime(datetime.now()) - - def handle_start_changed(self, datetime_): - self._parentfilter.set_start_time(datetime_) - - def handle_stop_changed(self, datetime_): - self._parentfilter.set_stop_time(datetime_) - - def handle_stop_enabled_changed(self, checked): - self._parentfilter.set_stop_time_enabled(checked) - self.stop_datetime.setEnabled(checked) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - """ - Saves the settings for this filter to an ini file. - :param settings: used to write the settings to an ini file ''qt_gui.settings.Settings'' - """ - settings.set_value('start_time', self._parentfilter._start_time.toString('hh:mm:ss.zzz (yyyy-MM-dd)')) - settings.set_value('stop_time', self._parentfilter._stop_time.toString('hh:mm:ss.zzz (yyyy-MM-dd)')) - settings.set_value('stop_time_enabled', self._parentfilter._stop_time_enabled) - - def restore_settings(self, settings): - """ - Restores the settings for this filter from an ini file. - :param settings: used to extract the settings from an ini file ''qt_gui.settings.Settings'' - """ - self.handle_stop_enabled_changed(settings.value('stop_time_enabled') in [True, 'true']) - if settings.contains('start_time'): - self.handle_start_changed(QDateTime.fromString(settings.value('start_time'), 'hh:mm:ss.zzz (yyyy-MM-dd)')) - else: - self.handle_start_changed(QDateTime(datetime.now())) - if settings.contains('stop_time'): - self.handle_stop_changed(QDateTime.fromString(settings.value('stop_time'), 'hh:mm:ss.zzz (yyyy-MM-dd)')) - else: - self.handle_stop_changed(QDateTime(datetime.now())) - - self.stop_datetime.setDateTime(self._parentfilter._stop_time) - self.start_datetime.setDateTime(self._parentfilter._start_time) - self.stop_enabled_check_box.setChecked(self._parentfilter._stop_time_enabled) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py deleted file mode 100644 index 6a06a2c4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/filters/topic_filter.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from .base_filter import BaseFilter - - -class TopicFilter(BaseFilter): - """ - Contains filter logic for a topic filter. - If the message's topic text matches any of the text in the stored list - then it is considered a match. - """ - def __init__(self): - super(TopicFilter, self).__init__() - self._selected_items = [] - - def set_selected_items(self, items): - """ - Setter for selected items. - :param list_" list of items to store for filtering ''list of QListWidgetItem'' - :emits filter_changed_signal: If _enabled is true - """ - self._selected_items = items - if self.is_enabled(): - self.start_emit_timer() - - def has_filter(self): - return len(self._selected_items) > 0 - - def test_message(self, message): - """ - Tests if the message matches the filter. - If the message's topic text matches any of the text in the stored list - then it is considered a match. - :param message: the message to be tested against the filters, ''Message'' - :returns: True if the message matches, ''bool'' - """ - if not self.is_enabled(): - return False - for item in self._selected_items: - if item.text() in message.topics: - return True - return False diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py deleted file mode 100644 index 901ccfd4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message.py +++ /dev/null @@ -1,133 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rosgraph_msgs.msg import Log - -from python_qt_binding.QtCore import QCoreApplication, QDateTime, QObject - - -class Message(QObject): - - DEBUG = 1 - INFO = 2 - WARN = 4 - ERROR = 8 - FATAL = 16 - - SEVERITY_LABELS = { - DEBUG: QCoreApplication.translate('Message', 'Debug'), - INFO: QCoreApplication.translate('Message', 'Info'), - WARN: QCoreApplication.translate('Message', 'Warn'), - ERROR: QCoreApplication.translate('Message', 'Error'), - FATAL: QCoreApplication.translate('Message', 'Fatal'), - } - - _next_id = 1 - - def __init__(self): - super(Message, self).__init__() - self.id = Message._next_id - Message._next_id += 1 - - self.message = None - self.severity = None - self.node = None - self.__stamp = (None, None) - self.topics = [] - self.location = None - - self._stamp_compare = None - self._stamp_qdatetime = None - - self._stamp_format = None - self._stamp_string = None - - self.highlighted = True - - def _get_stamp(self): - return self.__stamp - - def _set_stamp(self, stamp): - """ - Update the string representation of the timestamp. - :param stamp: a tuple containing seconds and nano seconds - """ - assert len(stamp) == 2 - self.__stamp = stamp - if None not in self.__stamp: - # shortest string representation to compare stamps - # floats do not provide enough precision - self._stamp_compare = '%08x%08x' % (self.__stamp[0], self.__stamp[1]) - else: - self._stamp_compare = None - self._stamp_qdatetime = self._get_stamp_as_qdatetime(self.__stamp) - if self._stamp_format: - s = self._stamp_qdatetime.toString(self._stamp_format) - if 'ZZZ' in s: - s = s.replace('ZZZ', str(self.__stamp[1]).zfill(9)) - self._stamp_string = s - - stamp = property(_get_stamp, _set_stamp) - - def get_stamp_for_compare(self): - return self._stamp_compare - - def get_stamp_as_qdatetime(self): - return self._stamp_qdatetime - - def _get_stamp_as_qdatetime(self, stamp): - if None in self.__stamp: - return None - dt = QDateTime() - dt.setTime_t(stamp[0]) - dt.addMSecs(int(float(stamp[1]) / 10**6)) - return dt - - def get_stamp_string(self): - return self._stamp_string - - def set_stamp_format(self, format): - self._stamp_format = format - if None not in self.__stamp: - self.stamp = self.__stamp - - def pretty_print(self): - text = self.tr('Node: ') + self.node + '\n' - text += self.tr('Time: ') + self.get_stamp_string() + '\n' - text += self.tr('Severity: ') + Message.SEVERITY_LABELS[self.severity] + '\n' - text += self.tr('Published Topics: ') + ', '.join(self.topics) + '\n' - text += '\n' + self.message + '\n' - text += '\n' + 'Location:' - text += '\n' + self.location + '\n\n' - text += '-' * 100 + '\n\n' - - return text diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py deleted file mode 100644 index 7b32eab3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_data_model.py +++ /dev/null @@ -1,267 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QAbstractTableModel, QModelIndex, Qt, qWarning -from python_qt_binding.QtGui import QBrush, QIcon - -from .message import Message -from .message_list import MessageList - - -class MessageDataModel(QAbstractTableModel): - - # the column names must match the message attributes - columns = ['message', 'severity', 'node', 'stamp', 'topics', 'location'] - - severity_colors = { - Message.DEBUG: QBrush(Qt.darkCyan), - Message.INFO: QBrush(Qt.darkBlue), - Message.WARN: QBrush(Qt.darkYellow), - Message.ERROR: QBrush(Qt.darkRed), - Message.FATAL: QBrush(Qt.red), - } - - def __init__(self): - super(MessageDataModel, self).__init__() - self._messages = MessageList() - self._message_limit = 20000 - self._info_icon = QIcon.fromTheme('dialog-information') - self._warning_icon = QIcon.fromTheme('dialog-warning') - self._error_icon = QIcon.fromTheme('dialog-error') - - # BEGIN Required implementations of QAbstractTableModel functions - - def rowCount(self, parent=None): - return len(self._messages) - - def columnCount(self, parent=None): - return len(MessageDataModel.columns) + 1 - - def data(self, index, role=None): - if role is None: - role = Qt.DisplayRole - if index.row() >= 0 and index.row() < len(self._messages): - msg = self._messages[index.row()] - if index.column() == 0: - if role == Qt.DisplayRole: - return '#%d' % msg.id - elif index.column() > 0 and index.column() < len(MessageDataModel.columns) + 1: - column = MessageDataModel.columns[index.column() - 1] - if role == Qt.DisplayRole or role == Qt.UserRole: - if column == 'stamp': - if role != Qt.UserRole: - data = msg.get_stamp_string() - else: - data = msg.get_stamp_for_compare() - else: - data = getattr(msg, column) - # map severity enum to label - if role == Qt.DisplayRole and column == 'severity': - data = Message.SEVERITY_LABELS[data] - # implode topic names - if column == 'topics': - data = ', '.join(data) - # append row number to define strict order - if role == Qt.UserRole: - # append row number to define strict order - # shortest string representation to compare stamps - #print(column, data, str(index.row()).zfill(len(str(len(self._messages))))) - data = str(data) + ' %08x' % index.row() - return data - - # decorate message column with severity icon - if role == Qt.DecorationRole and column == 'message': - if msg.severity in [Message.DEBUG, Message.INFO]: - return self._info_icon - elif msg.severity in [Message.WARN]: - return self._warning_icon - elif msg.severity in [Message.ERROR, Message.FATAL]: - return self._error_icon - - # colorize severity label - if role == Qt.ForegroundRole and column =='severity': - assert msg.severity in MessageDataModel.severity_colors, 'Unknown severity type: %s' % msg.severity - return MessageDataModel.severity_colors[msg.severity] - - if role == Qt.ToolTipRole and column != 'severity': - if column == 'stamp': - data = msg.get_stamp_string() - elif column == 'topics': - data = ', '.join(msg.topics) - else: - data = getattr(msg, column) - # tag enables word wrap by forcing rich text - return '' + data + '

' + self.tr('Right click for menu.') + '' - - def headerData(self, section, orientation, role=None): - if role is None: - role = Qt.DisplayRole - if orientation == Qt.Horizontal: - if role == Qt.DisplayRole: - if section == 0: - return self.tr('#') - else: - return MessageDataModel.columns[section - 1].capitalize() - if role == Qt.ToolTipRole: - if section == 0: - return self.tr('Sort the rows by serial number in descendig order') - else: - return self.tr('Sorting the table by a column other then the serial number slows down the interaction especially when recording high frequency data') - - # END Required implementations of QAbstractTableModel functions - - def get_message_limit(self): - return self._message_limit - - def set_message_limit(self, new_limit): - self._message_limit = new_limit - self._enforce_message_limit(self._message_limit) - - def _enforce_message_limit(self, limit): - if len(self._messages) > limit: - self.beginRemoveRows(QModelIndex(), limit, len(self._messages) - 1) - del self._messages[limit:len(self._messages)] - self.endRemoveRows() - - def insert_rows(self, msgs): - # never try to insert more message than the limit - if len(msgs) > self._message_limit: - msgs = msgs[-self._message_limit:] - # reduce model before insert - limit = self._message_limit - len(msgs) - self._enforce_message_limit(limit) - # insert newest messages - self.beginInsertRows(QModelIndex(), 0, len(msgs) - 1) - self._messages.extend(msgs) - self.endInsertRows() - - def remove_rows(self, rowlist): - """ - :param rowlist: list of row indexes, ''list(int)'' - :returns: True if the indexes were removed successfully, ''bool'' - """ - if len(rowlist) == 0: - if len(self._messages) > 0: - try: - self.beginRemoveRows(QModelIndex(), 0, len(self._messages)) - del self._messages[0:len(self._messages)] - self.endRemoveRows() - except: - return False - else: - rowlist = list(set(rowlist)) - rowlist.sort(reverse=True) - dellist = [rowlist[0]] - for row in rowlist[1:]: - if dellist[-1] - 1 > row: - try: - self.beginRemoveRows(QModelIndex(), dellist[-1], dellist[0]) - del self._messages[dellist[-1]:dellist[0] + 1] - self.endRemoveRows() - except: - return False - dellist = [] - dellist.append(row) - if len(dellist) > 0: - try: - self.beginRemoveRows(QModelIndex(), dellist[-1], dellist[0]) - del self._messages[dellist[-1]:dellist[0] + 1] - self.endRemoveRows() - except: - return False - return True - - def get_selected_text(self, rowlist): - """ - Returns an easily readable block of text for the currently selected rows - :param rowlist: list of row indexes, ''list(int)'' - :returns: the text from those indexes, ''str'' - """ - text = None - if len(rowlist) != 0: - text = '' - rowlist = list(set(rowlist)) - for row in rowlist: - text += self._messages[row].pretty_print() - return text - - def get_time_range(self, rowlist): - """ - :param rowlist: a list of row indexes, ''list'' - :returns: a tuple of min and max times in a rowlist in '(unix timestamp).(fraction of second)' format, ''tuple(str,str)'' - """ - min_ = float("inf") - max_ = float("-inf") - for row in rowlist: - item = self._messages[row].time_as_datestamp() - if float(item) > float(max_): - max_ = item - if float(item) < float(min_): - min_ = item - return min_, max_ - - def get_unique_nodes(self): - nodes = set([]) - for message in self._messages: - nodes.add(message.node) - return nodes - - def get_unique_severities(self): - severities = set([]) - for message in self._messages: - severities.add(message.severity) - return severities - - def get_unique_topics(self): - topics = set([]) - for message in self._messages: - for topic in message.topics: - topics.add(topic) - return topics - - def get_severity_dict(self): - return Message.SEVERITY_LABELS - - def get_message_between(self, start_time, end_time=None): - """ - :param start_time: time to start in timestamp form (including decimal - fractions of a second is acceptable, ''unixtimestamp'' - :param end_time: time to end in timestamp form (including decimal - fractions of a second is acceptable, ''unixtimestamp'' (Optional) - :returns: list of messages in the time range ''list[message]'' - """ - msgs = [] - for message in self._messages: - msg_time = message.stamp[0] + float(message.stamp[1]) / 10**9 - if msg_time >= float(start_time) and (end_time is None or msg_time <= float(end_time)): - msgs.append(message) - return msgs diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py deleted file mode 100644 index b0c1c910..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_list.py +++ /dev/null @@ -1,63 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Open Source Robotics Foundation Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -class MessageList(object): - - def __init__(self): - super(MessageList, self).__init__() - self._messages = [] - - def __getitem__(self, key): - return self._messages[len(self._messages) - key - 1] - - def __delitem__(self, key): - if isinstance(key, slice): - assert key.step is None or key.step == 1, 'MessageList.__delitem__ not implemented for slices with step argument different than 1' - del self._messages[len(self._messages) - key.stop:len(self._messages) - key.start] - else: - del self._messages[len(self._messages) - key - 1] - - def __iter__(self): - return reversed(self._messages) - - def __reversed__(self): - return iter(self._messages) - - def __contains__(self, item): - return item in self._messages - - def __len__(self): - return len(self._messages) - - def extend(self, item): - self._messages.extend(item) diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py deleted file mode 100644 index 62554733..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/message_proxy_model.py +++ /dev/null @@ -1,132 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Qt, qWarning -from python_qt_binding.QtGui import QBrush, QColor, QSortFilterProxyModel - -from .filters.filter_collection import FilterCollection -from .message import Message - - -class MessageProxyModel(QSortFilterProxyModel): - """ - Provides sorting and filtering capabilities for the MessageDataModel. - Filtering is based on a collection of exclude and highlight filters. - """ - - def __init__(self): - super(MessageProxyModel, self).__init__() - self.setDynamicSortFilter(True) - self.setFilterRole(Qt.UserRole) - self.setSortCaseSensitivity(Qt.CaseInsensitive) - self.setSortRole(Qt.UserRole) - - self._exclude_filters = FilterCollection() - self._highlight_filters = FilterCollection() - self._show_highlighted_only = False - - # caching source model locally for better performance of filterAcceptsRow() - self._source_model = None - - def setSourceModel(self, source_model): - super(MessageProxyModel, self).setSourceModel(source_model) - self._source_model = self.sourceModel() - - # BEGIN Required implementations of QSortFilterProxyModel functions - - def filterAcceptsRow(self, sourcerow, sourceparent): - """ - returns: True if the row does not match any exclude filter AND (_show_highlighted_only is False OR it matches any highlight filter), ''bool'' - """ - msg = self._source_model._messages[sourcerow] - if self._exclude_filters.test_message(msg): - # hide excluded message - return False - - highlighted = True - if self._highlight_filters.count_enabled_filters() > 0: - highlighted = self._highlight_filters.test_message(msg, default=True) - if self._show_highlighted_only and not highlighted: - # hide messages which are not highlighted when only highlightes messages should be visible - return False - - # update message state - msg.highlighted = highlighted - - return True - - def data(self, proxy_index, role=None): - """ - Set colors of items based on highlight filters. - """ - index = self.mapToSource(proxy_index) - if role == Qt.ForegroundRole: - msg = self._source_model._messages[index.row()] - if not msg.highlighted: - return QBrush(Qt.gray) - return self._source_model.data(index, role) - - # END Required implementations of QSortFilterProxyModel functions - - def handle_exclude_filters_changed(self): - """ - Invalidate filters and trigger refiltering. - """ - self.invalidateFilter() - - def handle_highlight_filters_changed(self): - """ - Invalidate filters and trigger refiltering. - """ - if self._show_highlighted_only: - self.invalidateFilter() - else: - self.invalidateFilter() - self.dataChanged.emit(self.index(0, 0), self.index(self.rowCount() - 1, self.columnCount() - 1)) - - def add_exclude_filter(self, newfilter): - self._exclude_filters.append(newfilter) - - def add_highlight_filter(self, newfilter): - self._highlight_filters.append(newfilter) - - def delete_exclude_filter(self, index): - del self._exclude_filters[index] - self.handle_exclude_filters_changed() - - def delete_highlight_filter(self, index): - del self._highlight_filters[index] - self.handle_highlight_filters_changed() - - def set_show_highlighted_only(self, show_highlighted_only): - self._show_highlighted_only = show_highlighted_only - self.invalidateFilter() diff --git a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py b/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py deleted file mode 100644 index 3449aac7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_console/src/rqt_console/text_browse_dialog.py +++ /dev/null @@ -1,50 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QDialog - - -class TextBrowseDialog(QDialog): - """ - Simple text brower Dialog that sets its text from the passed in text. - """ - def __init__(self, text, rospack): - """ - :param text: value to set the text of the widget to, ''str'' - """ - super(TextBrowseDialog, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_console'), 'resource', 'text_browse_dialog.ui') - loadUi(ui_file, self) - self.text_browser.setText(text) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst deleted file mode 100644 index dacde9f1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/CHANGELOG.rst +++ /dev/null @@ -1,111 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_dep -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* install rqt_dep globally (`#286 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* using CATKIN_ENABLE_TESTING to optionally configure tests (`#220 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* make it a non ros plugin startable without a roscore (`#200 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* add partial support for metapackages -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* fix handling ResourceNotFound for not found packages in graph - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* add combo to filter wet/dry packages - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- -* improve graph generation, get rid of redundant traversal, remove max edge limit - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into Groovy diff --git a/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt deleted file mode 100644 index 0da391bc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_dep) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test/dotcode_pack_test.py) -endif() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_dep - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox deleted file mode 100644 index bba00f3f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_dep provides a GUI plugin for visualizing the ROS dependency graph. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_dep/package.xml b/workspace/src/rqt_common_plugins/rqt_dep/package.xml deleted file mode 100644 index 925469e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_dep - 0.3.13 - rqt_dep provides a GUI plugin for visualizing the ROS dependency graph. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_dep - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Thibault Kruse - - catkin - - python-rospkg - qt_dotgraph - qt_gui - qt_gui_py_common - rqt_graph - rqt_gui_py - - python-mock - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml b/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml deleted file mode 100644 index 63e9ecf7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to visualize a ROS package network. - - - - - folder - Plugins related to introspection. - - - preferences-system-network - A Python GUI plugin for visualizing the ROS package dependencies. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui b/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui deleted file mode 100644 index d06c7164..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/resource/RosPackGraph.ui +++ /dev/null @@ -1,274 +0,0 @@ - - - RosPackGraphWidget - - - true - - - - 0 - 0 - 1144 - 307 - - - - Package Graph - - - - - - - - - Refresh ROS graph - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Choose graph type - - - - - - - Comma separated regex patterns / names, leading dash creates exclusion pattern. Example: gazebo, -test.* - - - (Separate pkgs by comma) - - - - - - - - 0 - 0 - - - - - - - - - 0 - 0 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Load DOT graph from file - - - - 16 - 16 - - - - - - - - Save as DOT graph - - - - 16 - 16 - - - - - - - - Save as SVG - - - - 16 - 16 - - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - - - - - - Mark Selected - - - Mark - - - - - - - Show stacks - - - Show Stacks - - - - - - - Do not show transitive / duplicate dependencies - - - No Transitive deps - - - - - - - Random colors for stacks - - - Colorize - - - true - - - - - - - Qt::Vertical - - - - - - - Highlight incoming and outgoing connections on mouse over - - - Highlight - - - true - - - - - - - Automatically fit graph into view on update - - - Fit - - - true - - - - - - - Fit graph in view - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - QPainter::Antialiasing|QPainter::HighQualityAntialiasing|QPainter::SmoothPixmapTransform|QPainter::TextAntialiasing - - - QGraphicsView::AnchorViewCenter - - - - - - - - InteractiveGraphicsView - QGraphicsView -
rqt_graph.interactive_graphics_view
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep b/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep deleted file mode 100755 index 961a30c0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/scripts/rqt_dep +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_dep.ros_pack_graph.RosPackGraph')) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/setup.py b/workspace/src/rqt_common_plugins/rqt_dep/setup.py deleted file mode 100644 index 893ad5a6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_dep'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_dep'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/__init__.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py deleted file mode 100644 index fae10652..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/dotcode_pack.py +++ /dev/null @@ -1,396 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import with_statement, print_function - -import os -import re - -from rospkg import MANIFEST_FILE -from rospkg.common import ResourceNotFound -from qt_dotgraph.colors import get_color_for_string - - -def matches_any(name, patternlist): - for pattern in patternlist: - if name == pattern: - return True - if re.match("^[a-zA-Z0-9_]+$", pattern) is None: - if re.match(pattern, name) is not None: - return True - return False - - -class RosPackageGraphDotcodeGenerator: - - def __init__(self, rospack, rosstack): - """ - :param rospack: use rospkg.RosPack() - :param rosstack: use rospkg.RosStack() - """ - self.rospack = rospack - self.rosstack = rosstack - self.stacks = {} - self.packages = {} - self.package_types = {} - self.edges = {} - self.traversed_ancestors = {} - self.traversed_descendants = {} - self.last_drawargs = None - self.last_selection = None - - def generate_dotcode(self, - dotcode_factory, - selected_names=[], - excludes=[], - depth=3, - with_stacks=True, - descendants=True, - ancestors=True, - hide_transitives=True, - mark_selected=True, - colortheme=None, - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - force_refresh=False, - hide_wet=False, - hide_dry=False): - """ - - :param hide_transitives: if true, then dependency of children to grandchildren will be hidden if parent has same dependency - """ - - # defaults - selected_names = filter(lambda x: x is not None and x != '', selected_names) - excludes = filter(lambda x: x is not None and x != '', excludes) - if selected_names is None or selected_names == []: - selected_names = ['.*'] - self.depth = 1 - if depth is None: - depth = -1 - - # update arguments - - selection_args = { - "dotcode_factory": dotcode_factory, - "with_stacks": with_stacks, - "depth": depth, - "hide_transitives": hide_transitives, - "selected_names": selected_names, - "excludes": excludes, - "ancestors": ancestors, - "descendants": descendants, - "hide_wet": hide_wet, - "hide_dry": hide_dry - } - - # if selection did not change, we need not build up the graph again - selection_changed = False - if self.last_selection != selection_args: - selection_changed = True - self.last_selection = selection_args - - self.dotcode_factory = dotcode_factory - self.with_stacks = with_stacks - self.depth = depth - self.hide_transitives = hide_transitives - self.selected_names = selected_names - self.excludes = excludes - self.ancestors = ancestors - self.descendants = descendants - self.hide_wet = hide_wet - self.hide_dry = hide_dry - - if force_refresh or selection_changed: - self.stacks = {} - self.packages = {} - self.package_types = {} - self.edges = {} - self.traversed_ancestors = {} - self.traversed_descendants = {} - # update internal graph structure - for name in self.rospack.list(): - if matches_any(name, self.selected_names): - if descendants: - self.add_package_descendants_recursively(name) - if ancestors: - self.add_package_ancestors_recursively(name) - for stackname in self.rosstack.list(): - if matches_any(stackname, self.selected_names): - manifest = self.rosstack.get_manifest(stackname) - if manifest.is_catkin: - if descendants: - self.add_package_descendants_recursively(stackname) - if ancestors: - self.add_package_ancestors_recursively(stackname) - else: - for package_name in self.rosstack.packages_of(stackname): - if descendants: - self.add_package_descendants_recursively(package_name) - if ancestors: - self.add_package_ancestors_recursively(package_name) - - drawing_args = { - 'dotcode_factory': dotcode_factory, - "rank": rank, - "rankdir": rankdir, - "ranksep": ranksep, - "simplify": simplify, - "colortheme": colortheme, - "mark_selected": mark_selected - } - - # if selection and display args did not change, no need to generate dotcode - display_changed = False - if self.last_drawargs != drawing_args: - display_changed = True - self.last_drawargs = drawing_args - - self.dotcode_factory = dotcode_factory - self.rank = rank - self.rankdir = rankdir - self.ranksep = ranksep - self.simplify = simplify - self.colortheme = colortheme - self.dotcode_factory = dotcode_factory - self.mark_selected = mark_selected - - #generate new dotcode - if force_refresh or selection_changed or display_changed: - self.graph = self.generate(self.dotcode_factory) - self.dotcode = dotcode_factory.create_dot(self.graph) - - return self.dotcode - - def generate(self, dotcode_factory): - graph = dotcode_factory.get_graph(rank=self.rank, - rankdir=self.rankdir, - ranksep=self.ranksep, - simplify=self.simplify) - # print("In generate", self.with_stacks, len(self.stacks), len(self.packages), len(self.edges)) - packages_in_stacks = [] - if self.with_stacks and not self.hide_dry: - for stackname in self.stacks: - color = None - if self.mark_selected and not '.*' in self.selected_names and matches_any(stackname, self.selected_names): - color = 'tomato' - else: - color = 'gray' - if self.colortheme is not None: - color = get_color_for_string(stackname) - g = dotcode_factory.add_subgraph_to_graph(graph, - stackname, - color=color, - rank=self.rank, - rankdir=self.rankdir, - ranksep=self.ranksep, - simplify=self.simplify) - - for package_name in self.stacks[stackname]['packages']: - packages_in_stacks.append(package_name) - self._generate_package(dotcode_factory, g, package_name) - - for package_name, attributes in self.packages.iteritems(): - if package_name not in packages_in_stacks: - self._generate_package(dotcode_factory, graph, package_name, attributes) - for name1, name2 in self.edges.keys(): - dotcode_factory.add_edge_to_graph(graph, name1, name2) - return graph - - def _generate_package(self, dotcode_factory, graph, package_name, attributes=None): - if self._hide_package(package_name): - return - color = None - if self.mark_selected and not '.*' in self.selected_names and matches_any(package_name, self.selected_names): - if attributes and attributes['is_catkin']: - color = 'red' - else: - color = 'tomato' - elif attributes and not attributes['is_catkin']: - color = 'gray' - if attributes and 'not_found' in attributes and attributes['not_found']: - color = 'orange' - package_name += ' ?' - dotcode_factory.add_node_to_graph(graph, package_name, color=color) - - def _add_stack(self, stackname): - if stackname is None or stackname in self.stacks: - return - self.stacks[stackname] = {'packages': []} - - def _add_package(self, package_name, parent=None): - """ - adds object based on package_name to self.packages - :param parent: packagename which referenced package_name (for debugging only) - """ - if self._hide_package(package_name): - return - if package_name in self.packages: - return False - - catkin_package = self._is_package_wet(package_name) - if catkin_package is None: - return False - self.packages[package_name] = {'is_catkin': catkin_package} - - if self.with_stacks: - try: - stackname = self.rospack.stack_of(package_name) - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator._add_package(%s), parent %s: ResourceNotFound:' % (package_name, parent), e) - stackname = None - if not stackname is None and stackname != '': - if not stackname in self.stacks: - self._add_stack(stackname) - self.stacks[stackname]['packages'].append(package_name) - return True - - def _hide_package(self, package_name): - if not self.hide_wet and not self.hide_dry: - return False - catkin_package = self._is_package_wet(package_name) - if self.hide_wet and catkin_package: - return True - if self.hide_dry and catkin_package is False: - return True - # if type of package is unknown don't hide it - return False - - def _is_package_wet(self, package_name): - if package_name not in self.package_types: - try: - package_path = self.rospack.get_path(package_name) - manifest_file = os.path.join(package_path, MANIFEST_FILE) - self.package_types[package_name] = not os.path.exists(manifest_file) - except ResourceNotFound: - return None - return self.package_types[package_name] - - def _add_edge(self, name1, name2, attributes=None): - if self._hide_package(name1) or self._hide_package(name2): - return - self.edges[(name1, name2)] = attributes - - def add_package_ancestors_recursively(self, package_name, expanded_up=None, depth=None, implicit=False, parent=None): - """ - :param package_name: the name of package for which to add ancestors - :param expanded_up: names that have already been expanded (to avoid cycles) - :param depth: how many layers to follow - :param implicit: arg to rospack - :param parent: package that referenced package_name for error message only - """ - if package_name in self.traversed_ancestors: - traversed_depth = self.traversed_ancestors[package_name] - if traversed_depth is None: - return - if depth is not None and traversed_depth >= depth: - return - self.traversed_ancestors[package_name] = depth - - if matches_any(package_name, self.excludes): - return False - if (depth == 0): - return False - if (depth == None): - depth = self.depth - self._add_package(package_name, parent=parent) - if expanded_up is None: - expanded_up = [] - expanded_up.append(package_name) - if (depth != 1): - try: - depends_on = self.rospack.get_depends_on(package_name, implicit=implicit) - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator.add_package_ancestors_recursively(%s), parent %s: ResourceNotFound:' % (package_name, parent), e) - depends_on = [] - new_nodes = [] - for dep_on_name in [x for x in depends_on if not matches_any(x, self.excludes)]: - if not self.hide_transitives or not dep_on_name in expanded_up: - new_nodes.append(dep_on_name) - self._add_edge(dep_on_name, package_name) - self._add_package(dep_on_name, parent=package_name) - expanded_up.append(dep_on_name) - for dep_on_name in new_nodes: - self.add_package_ancestors_recursively(package_name=dep_on_name, - expanded_up=expanded_up, - depth=depth - 1, - implicit=implicit, - parent=package_name) - - def add_package_descendants_recursively(self, package_name, expanded=None, depth=None, implicit=False, parent=None): - if package_name in self.traversed_descendants: - traversed_depth = self.traversed_descendants[package_name] - if traversed_depth is None: - return - if depth is not None and traversed_depth >= depth: - return - self.traversed_descendants[package_name] = depth - - if matches_any(package_name, self.excludes): - return - if (depth == 0): - return - if (depth == None): - depth = self.depth - self._add_package(package_name, parent=parent) - if expanded is None: - expanded = [] - expanded.append(package_name) - if (depth != 1): - try: - try: - depends = self.rospack.get_depends(package_name, implicit=implicit) - except ResourceNotFound: - # try falling back to rosstack to find wet metapackages - manifest = self.rosstack.get_manifest(package_name) - if manifest.is_catkin: - depends = [d.name for d in manifest.depends] - else: - raise - except ResourceNotFound as e: - print('RosPackageGraphDotcodeGenerator.add_package_descendants_recursively(%s), parent: %s: ResourceNotFound:' % (package_name, parent), e) - depends = [] - new_nodes = [] - for dep_name in [x for x in depends if not matches_any(x, self.excludes)]: - if not self.hide_transitives or not dep_name in expanded: - new_nodes.append(dep_name) - self._add_edge(package_name, dep_name) - self._add_package(dep_name, parent=package_name) - expanded.append(dep_name) - for dep_name in new_nodes: - self.add_package_descendants_recursively(package_name=dep_name, - expanded=expanded, - depth=depth - 1, - implicit=implicit, - parent=package_name) diff --git a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py b/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py deleted file mode 100644 index f86d424d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/src/rqt_dep/ros_pack_graph.py +++ /dev/null @@ -1,417 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2011, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os -import pickle - -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QFile, QIODevice, Qt, Signal, Slot, QAbstractListModel -from python_qt_binding.QtGui import QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget, QCompleter -from python_qt_binding.QtSvg import QSvgGenerator - -import rosservice -import rostopic - -from .dotcode_pack import RosPackageGraphDotcodeGenerator -from qt_dotgraph.pydotfactory import PydotFactory -# from qt_dotgraph.pygraphvizfactory import PygraphvizFactory -from qt_dotgraph.dot_to_qt import DotToQtGenerator -from qt_gui_py_common.worker_thread import WorkerThread - -from rqt_gui_py.plugin import Plugin -from rqt_graph.interactive_graphics_view import InteractiveGraphicsView - - -class RepeatedWordCompleter(QCompleter): - """A completer that completes multiple times from a list""" - def pathFromIndex(self, index): - path = QCompleter.pathFromIndex(self, index) - lst = str(self.widget().text()).split(',') - if len(lst) > 1: - path = '%s, %s' % (','.join(lst[:-1]), path) - return path - - def splitPath(self, path): - path = str(path.split(',')[-1]).lstrip(' ') - return [path] - - -class StackageCompletionModel(QAbstractListModel): - """Ros package and stacknames""" - def __init__(self, linewidget, rospack, rosstack): - super(StackageCompletionModel, self).__init__(linewidget) - self.allnames = sorted(list(set(rospack.list() + rosstack.list()))) - self.allnames = self.allnames + ['-%s' % name for name in self.allnames] - - def rowCount(self, parent): - return len(self.allnames) - - def data(self, index, role): - # TODO: symbols to distinguish stacks from packages - if index.isValid() and (role == Qt.DisplayRole or role == Qt.EditRole): - return self.allnames[index.row()] - return None - - -class RosPackGraph(Plugin): - - _deferred_fit_in_view = Signal() - - def __init__(self, context): - super(RosPackGraph, self).__init__(context) - self.initialized = False - self._current_dotcode = None - self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) - self._nodes = {} - self._edges = {} - self._options = {} - self._options_serialized = '' - - self.setObjectName('RosPackGraph') - - rospack = rospkg.RosPack() - rosstack = rospkg.RosStack() - - # factory builds generic dotcode items - self.dotcode_factory = PydotFactory() - # self.dotcode_factory = PygraphvizFactory() - # generator builds rosgraph - self.dotcode_generator = RosPackageGraphDotcodeGenerator(rospack, rosstack) - # dot_to_qt transforms into Qt elements using dot layout - self.dot_to_qt = DotToQtGenerator() - - self._widget = QWidget() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_dep'), 'resource', 'RosPackGraph.ui') - loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) - self._widget.setObjectName('RosPackGraphUi') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - self._scene = QGraphicsScene() - self._scene.setBackgroundBrush(Qt.white) - self._widget.graphics_view.setScene(self._scene) - - self._widget.depth_combo_box.insertItem(0, self.tr('infinite'), -1) - self._widget.depth_combo_box.insertItem(1, self.tr('1'), 2) - self._widget.depth_combo_box.insertItem(2, self.tr('2'), 3) - self._widget.depth_combo_box.insertItem(3, self.tr('3'), 4) - self._widget.depth_combo_box.insertItem(4, self.tr('4'), 5) - self._widget.depth_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - self._widget.directions_combo_box.insertItem(0, self.tr('depends'), 0) - self._widget.directions_combo_box.insertItem(1, self.tr('depends_on'), 1) - self._widget.directions_combo_box.insertItem(2, self.tr('both'), 2) - self._widget.directions_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - self._widget.package_type_combo_box.insertItem(0, self.tr('wet & dry'), 3) - self._widget.package_type_combo_box.insertItem(1, self.tr('wet only'), 2) - self._widget.package_type_combo_box.insertItem(2, self.tr('dry only'), 1) - self._widget.package_type_combo_box.currentIndexChanged.connect(self._refresh_rospackgraph) - - completionmodel = StackageCompletionModel(self._widget.filter_line_edit, rospack, rosstack) - completer = RepeatedWordCompleter(completionmodel, self) - completer.setCompletionMode(QCompleter.PopupCompletion) - completer.setWrapAround(True) - - completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.filter_line_edit.editingFinished.connect(self._refresh_rospackgraph) - self._widget.filter_line_edit.setCompleter(completer) - self._widget.filter_line_edit.selectionChanged.connect(self._clear_filter) - - self._widget.with_stacks_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.mark_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.colorize_check_box.clicked.connect(self._refresh_rospackgraph) - self._widget.hide_transitives_check_box.clicked.connect(self._refresh_rospackgraph) - - self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) - self._widget.refresh_graph_push_button.pressed.connect(self._update_rospackgraph) - - self._widget.highlight_connections_check_box.toggled.connect(self._refresh_rospackgraph) - self._widget.auto_fit_graph_check_box.toggled.connect(self._refresh_rospackgraph) - self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) - self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) - - self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) - self._widget.load_dot_push_button.pressed.connect(self._load_dot) - self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_dot_push_button.pressed.connect(self._save_dot) - self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) - self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) - self._widget.save_as_image_push_button.pressed.connect(self._save_image) - - self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) - self._deferred_fit_in_view.emit() - - context.add_widget(self._widget) - - # If in either of following case, this turnes True - # - 1st filtering key is already input by user - # - filtering key is restored - self._filtering_started = False - - def shutdown_plugin(self): - self._update_thread.kill() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('depth_combo_box_index', self._widget.depth_combo_box.currentIndex()) - instance_settings.set_value('directions_combo_box_index', self._widget.directions_combo_box.currentIndex()) - instance_settings.set_value('package_type_combo_box', self._widget.package_type_combo_box.currentIndex()) - instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) - instance_settings.set_value('with_stacks_state', self._widget.with_stacks_check_box.isChecked()) - instance_settings.set_value('hide_transitives_state', self._widget.hide_transitives_check_box.isChecked()) - instance_settings.set_value('mark_state', self._widget.mark_check_box.isChecked()) - instance_settings.set_value('colorize_state', self._widget.colorize_check_box.isChecked()) - instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) - instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) - - def restore_settings(self, plugin_settings, instance_settings): - _str_filter = instance_settings.value('filter_line_edit_text', '') - if (_str_filter == None or _str_filter == '') and \ - not self._filtering_started: - _str_filter = '(Separate pkgs by comma)' - else: - self._filtering_started = True - - self._widget.depth_combo_box.setCurrentIndex(int(instance_settings.value('depth_combo_box_index', 0))) - self._widget.directions_combo_box.setCurrentIndex(int(instance_settings.value('directions_combo_box_index', 0))) - self._widget.package_type_combo_box.setCurrentIndex(int(instance_settings.value('package_type_combo_box', 0))) - self._widget.filter_line_edit.setText(_str_filter) - self._widget.with_stacks_check_box.setChecked(instance_settings.value('with_stacks_state', True) in [True, 'true']) - self._widget.mark_check_box.setChecked(instance_settings.value('mark_state', True) in [True, 'true']) - self._widget.colorize_check_box.setChecked(instance_settings.value('colorize_state', False) in [True, 'true']) - self._widget.hide_transitives_check_box.setChecked(instance_settings.value('hide_transitives_state', False) in [True, 'true']) - self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) - self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) - self.initialized = True - self._refresh_rospackgraph() - - def _update_rospackgraph(self): - # re-enable controls customizing fetched ROS graph - self._widget.depth_combo_box.setEnabled(True) - self._widget.directions_combo_box.setEnabled(True) - self._widget.package_type_combo_box.setEnabled(True) - self._widget.filter_line_edit.setEnabled(True) - self._widget.with_stacks_check_box.setEnabled(True) - self._widget.mark_check_box.setEnabled(True) - self._widget.colorize_check_box.setEnabled(True) - self._widget.hide_transitives_check_box.setEnabled(True) - - self._refresh_rospackgraph(force_update=True) - - def _update_options(self): - self._options['depth'] = self._widget.depth_combo_box.itemData(self._widget.depth_combo_box.currentIndex()) - self._options['directions'] = self._widget.directions_combo_box.itemData(self._widget.directions_combo_box.currentIndex()) - self._options['package_types'] = self._widget.package_type_combo_box.itemData(self._widget.package_type_combo_box.currentIndex()) - self._options['with_stacks'] = self._widget.with_stacks_check_box.isChecked() - self._options['mark_selected'] = self._widget.mark_check_box.isChecked() - self._options['hide_transitives'] = self._widget.hide_transitives_check_box.isChecked() - # TODO: Allow different color themes - self._options['colortheme'] = True if self._widget.colorize_check_box.isChecked() else None - self._options['names'] = self._widget.filter_line_edit.text().split(',') - if self._options['names'] == [u'None']: - self._options['names'] = [] - self._options['highlight_level'] = 3 if self._widget.highlight_connections_check_box.isChecked() else 1 - self._options['auto_fit'] = self._widget.auto_fit_graph_check_box.isChecked() - - def _refresh_rospackgraph(self, force_update=False): - if not self.initialized: - return - - self._update_thread.kill() - - self._update_options() - - # avoid update if options did not change and force_update is not set - new_options_serialized = pickle.dumps(self._options) - if new_options_serialized == self._options_serialized and not force_update: - return - self._options_serialized = pickle.dumps(self._options) - - self._scene.setBackgroundBrush(Qt.lightGray) - - self._update_thread.start() - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_thread_run(self): - self._update_graph(self._generate_dotcode()) - - @Slot() - def _update_finished(self): - self._scene.setBackgroundBrush(Qt.white) - self._redraw_graph_scene() - - # this runs in a non-gui thread, so don't access widgets here directly - def _generate_dotcode(self): - includes = [] - excludes = [] - for name in self._options['names']: - if name.strip().startswith('-'): - excludes.append(name.strip()[1:]) - else: - includes.append(name.strip()) - # orientation = 'LR' - descendants = True - ancestors = True - if self._options['directions'] == 1: - descendants = False - if self._options['directions'] == 0: - ancestors = False - return self.dotcode_generator.generate_dotcode(dotcode_factory=self.dotcode_factory, - selected_names=includes, - excludes=excludes, - depth=self._options['depth'], - with_stacks=self._options['with_stacks'], - descendants=descendants, - ancestors=ancestors, - mark_selected=self._options['mark_selected'], - colortheme=self._options['colortheme'], - hide_transitives=self._options['hide_transitives'], - hide_wet=self._options['package_types'] == 1, - hide_dry=self._options['package_types'] == 2) - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_graph(self, dotcode): - self._current_dotcode = dotcode - self._nodes, self._edges = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, self._options['highlight_level']) - - def _generate_tool_tip(self, url): - if url is not None and ':' in url: - item_type, item_path = url.split(':', 1) - if item_type == 'node': - tool_tip = 'Node:\n %s' % (item_path) - service_names = rosservice.get_service_list(node=item_path) - if service_names: - tool_tip += '\nServices:' - for service_name in service_names: - try: - service_type = rosservice.get_service_type(service_name) - tool_tip += '\n %s [%s]' % (service_name, service_type) - except rosservice.ROSServiceIOException, e: - tool_tip += '\n %s' % (e) - return tool_tip - elif item_type == 'topic': - topic_type, topic_name, _ = rostopic.get_topic_type(item_path) - return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) - return url - - def _redraw_graph_scene(self): - # remove items in order to not garbage nodes which will be continued to be used - for item in self._scene.items(): - self._scene.removeItem(item) - self._scene.clear() - for node_item in self._nodes.itervalues(): - self._scene.addItem(node_item) - for edge_items in self._edges.itervalues(): - for edge_item in edge_items: - edge_item.add_to_scene(self._scene) - - self._scene.setSceneRect(self._scene.itemsBoundingRect()) - if self._options['auto_fit']: - self._fit_in_view() - - def _load_dot(self, file_name=None): - if file_name is None: - file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - try: - fh = open(file_name, 'rb') - dotcode = fh.read() - fh.close() - except IOError: - return - - # disable controls customizing fetched ROS graph - self._widget.depth_combo_box.setEnabled(False) - self._widget.directions_combo_box.setEnabled(False) - self._widget.package_type_combo_box.setEnabled(False) - self._widget.filter_line_edit.setEnabled(False) - self._widget.with_stacks_check_box.setEnabled(False) - self._widget.mark_check_box.setEnabled(False) - self._widget.colorize_check_box.setEnabled(False) - self._widget.hide_transitives_check_box.setEnabled(False) - - self._update_graph(dotcode) - self._redraw_graph_scene() - - @Slot() - def _fit_in_view(self): - self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) - - def _save_dot(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rospackgraph.dot', self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - handle = QFile(file_name) - if not handle.open(QIODevice.WriteOnly | QIODevice.Text): - return - - handle.write(self._current_dotcode) - handle.close() - - def _save_svg(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rospackgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) - if file_name is None or file_name == '': - return - - generator = QSvgGenerator() - generator.setFileName(file_name) - generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) - - painter = QPainter(generator) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - - def _save_image(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rospackgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) - if file_name is None or file_name == '': - return - - img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) - painter = QPainter(img) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - img.save(file_name) - - def _clear_filter(self): - if not self._filtering_started: - self._widget.filter_line_edit.setText('') - self._filtering_started = True diff --git a/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py b/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py deleted file mode 100644 index 361a0154..00000000 --- a/workspace/src/rqt_common_plugins/rqt_dep/test/dotcode_pack_test.py +++ /dev/null @@ -1,66 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest - -# get mock from pypi as 'mock' -from mock import Mock, patch - -from rqt_dep.dotcode_pack import RosPackageGraphDotcodeGenerator - - -PKG = 'rqt_deps' - - -class DotcodeGeneratorTest(unittest.TestCase): - - def test_packages_only(self): - with patch('rospkg.RosPack') as rospack: - with patch('rospkg.RosStack') as rosstack: - factoryMock = Mock() - graphMock = Mock() - rospack.list.return_value = [] - rosstack.list.return_value = [] - factoryMock.get_graph.return_value = graphMock - gen = RosPackageGraphDotcodeGenerator(rospack, rosstack) - graph = gen.generate_dotcode(factoryMock) - - rospack.list.assert_called() - rosstack.list.assert_called() - factoryMock.get_graph.assert_called_with(simplify=True, rank='same', ranksep=0.2, rankdir='TB') - factoryMock.create_dot.assert_called_with(graphMock) - - -if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'test_packages_only', DotcodeGeneratorTest) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst deleted file mode 100644 index 35d79cf4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/CHANGELOG.rst +++ /dev/null @@ -1,114 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_graph -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* Remove repeated prefices from buttons -* Prefix all node and topic names with `n\_` and `t\_` respectively, to allow dot to distinguish them -* Contributors: Eric Wieser - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix duplicate rendering of statistics information (`#283 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix rendering of namespace boxes (`#266 `_) - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use TopicStatistics only if available (`#252 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* add displaying of topic/connection statistics along edges (`#214 `_) -* using CATKIN_ENABLE_TESTING to optionally configure tests (`#220 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* modified zooming method to work better on high-res trackpads like Macbook Pros (`#187 `_) - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- -* Improve checkbox labels and tooltips wording. - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt deleted file mode 100644 index fefb8595..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_graph) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test/dotcode_test.py) -endif() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_graph - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox deleted file mode 100644 index a9f78acd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_graph provides a GUI plugin for visualizing the ROS computation graph. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_graph/package.xml b/workspace/src/rqt_common_plugins/rqt_graph/package.xml deleted file mode 100644 index b8cd2b1f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/package.xml +++ /dev/null @@ -1,41 +0,0 @@ - - rqt_graph - 0.3.13 - rqt_graph provides a GUI plugin for visualizing the ROS - computation graph.
- - Its components are made generic so that other packages - where you want to achieve graph representation can depend upon this pkg - (use rqt_dep to find out - the pkgs that depend. rqt_dep itself depends on rqt_graph too). - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_graph - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - - catkin - - python-rospkg - qt_dotgraph - rosgraph - rosgraph_msgs - roslib - rosnode - rospy - rosservice - rostopic - rqt_gui - rqt_gui_py - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml b/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml deleted file mode 100644 index 600eb9b1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to visualize a ROS computation graph. - - - - - folder - Plugins related to introspection. - - - preferences-system-network - A Python GUI plugin for visualizing the ROS computation graph. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui b/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui deleted file mode 100644 index f64b8305..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/resource/RosGraph.ui +++ /dev/null @@ -1,301 +0,0 @@ - - - RosGraphWidget - - - true - - - - 0 - 0 - 1298 - 307 - - - - Node Graph - - - - - - - - Refresh ROS graph - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Choose graph type - - - - - - - Namespace filter, comma separated list of names/regexp to include or exclude. Example: -/rqt.* - - - / - - - - - - - Topic filter, comma separated list of names/regexp to include or exclude. Example: -/tf - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - Load DOT graph from file - - - - 16 - 16 - - - - - - - - Save as DOT graph - - - - 16 - 16 - - - - - - - - Save as SVG - - - - 16 - 16 - - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - - - - - - - - - Group: - - - - - - - Show namespace boxes - - - Namespaces - - - - - - - Summarize action topics into one virtual topic node (named <action_server>/action_topics) - - - Actions - - - - - - - - - - Qt::Vertical - - - - - - - - - - Hide: - - - - - - - Hide topics without subscribers - - - Dead sinks - - - - - - - Hide topics with one connection only (implicates dead sinks) - - - Leaf topics - - - - - - - Hide common debugging nodes - - - Debug - - - - - - - - - - Qt::Vertical - - - - - - - Highlight incoming and outgoing connections on mouse over - - - Highlight - - - true - - - - - - - Automatically fit graph into view on update - - - Fit - - - true - - - - - - - Fit graph in view - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - - QPainter::Antialiasing|QPainter::HighQualityAntialiasing|QPainter::SmoothPixmapTransform|QPainter::TextAntialiasing - - - QGraphicsView::AnchorViewCenter - - - - - - - - InteractiveGraphicsView - QGraphicsView -
rqt_graph.interactive_graphics_view
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph b/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph deleted file mode 100755 index cfb71a9d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/scripts/rqt_graph +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_graph.ros_graph.RosGraph')) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/setup.py b/workspace/src/rqt_common_plugins/rqt_graph/setup.py deleted file mode 100644 index de78f248..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_graph'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_graph'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/__init__.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py deleted file mode 100644 index af115a28..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/dotcode.py +++ /dev/null @@ -1,580 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# this is a modified version of rx/rxgraph/src/rxgraph/dotcode.py - -import re -import copy - -import rosgraph.impl.graph -import roslib -import math - -import rospy -import pydot - -# node/node connectivity -NODE_NODE_GRAPH = 'node_node' -# node/topic connections where an actual network connection exists -NODE_TOPIC_GRAPH = 'node_topic' -# all node/topic connections, even if no actual network connection -NODE_TOPIC_ALL_GRAPH = 'node_topic_all' - -QUIET_NAMES = ['/diag_agg', '/runtime_logger', '/pr2_dashboard', '/rviz', '/rosout', '/cpu_monitor', '/monitor', '/hd_monitor', '/rxloggerlevel', '/clock', '/rqt', '/statistics'] - -def _conv(n): - """Convert a node name to a valid dot name, which can't contain the leading space""" - if n.startswith(' '): - return 't_' + n[1:] - else: - return 'n_' + n - -def matches_any(name, patternlist): - if patternlist is None or len(patternlist) == 0: - return False - for pattern in patternlist: - if str(name).strip() == pattern: - return True - if re.match("^[a-zA-Z0-9_/]+$", pattern) is None: - if re.match(str(pattern), name.strip()) is not None: - return True - return False - - -class NodeConnections: - def __init__(self, incoming=None, outgoing=None): - self.incoming = incoming or [] - self.outgoing = outgoing or [] - - -class RosGraphDotcodeGenerator: - - # topic/topic -> graph.edge object - edges = dict([]) - - # ROS node name -> graph.node object - nodes = dict([]) - - def __init__(self): - try: - from rosgraph_msgs.msg import TopicStatistics - self.stats_sub = rospy.Subscriber('/statistics', TopicStatistics, self.statistics_callback) - except ImportError: - # not supported before Indigo - pass - - def statistics_callback(self,msg): - - # add connections (if new) - if not msg.node_sub in self.edges: - self.edges[msg.node_sub] = dict([]) - - if not msg.topic in self.edges[msg.node_sub]: - self.edges[msg.node_sub][msg.topic] = dict([]) - - self.edges[msg.node_sub][msg.topic][msg.node_pub] = msg - - def _get_max_traffic(self): - traffic = 10000 # start at 10kb - for sub in self.edges: - for topic in self.edges[sub]: - for pub in self.edges[sub][topic]: - traffic = max(traffic, self.edges[sub][topic][pub].traffic) - return traffic - - def _get_max_age(self): - age = 0.1 # start at 100ms - for sub in self.edges: - for topic in self.edges[sub]: - for pub in self.edges[sub][topic]: - age = max(age, self.edges[sub][topic][pub].stamp_age_mean.to_sec()) - return age - - def _get_max_age_on_topic(self, sub, topic): - age = 0.0 - for pub in self.edges[sub][topic]: - age = max(age, self.edges[sub][topic][pub].stamp_age_mean.to_sec()) - return age - - def _calc_edge_color(self, sub, topic, pub=None): - - age = 0.0 - - if pub is None: - age = self._get_max_age_on_topic(sub, topic) - elif sub in self.edges and topic in self.edges[sub] and pub in self.edges[sub][topic]: - age = self.edges[sub][topic][pub].stamp_age_mean.to_sec() - - if age == 0.0: - return [0, 0, 0] - - # calc coloring using the age - heat = max(age, 0) / self._get_max_age() - - # we assume that heat is normalized between 0.0 (green) and 1.0 (red) - # 0.0->green(0,255,0) to 0.5->yellow (255,255,0) to red 1.0(255,0,0) - if heat < 0: - red = 0 - green = 0 - elif heat <= 0.5: - red = int(heat * 255 * 2) - green = 255 - elif heat > 0.5: - red = 255 - green = 255 - int((heat - 0.5) * 255 * 2) - else: - red = 0 - green = 0 - return [red, green, 0] - - def _calc_edge_penwidth(self, sub, topic, pub=None): - if pub is None and sub in self.edges and topic in self.edges[sub]: - traffic = 0 - for p in self.edges[sub][topic]: - if pub is None or p == pub: - traffic += self.edges[sub][topic][p].traffic - - # calc penwidth using the traffic in kb/s - return int(traffic / self._get_max_traffic() * 5) - else: - return 1 - - def _calc_statistic_info(self, sub, topic, pub=None): - if pub is None and sub in self.edges and topic in self.edges[sub]: - conns = len(self.edges[sub][topic]) - if conns == 1: - pub = next(self.edges[sub][topic].iterkeys()) - else: - penwidth = self._calc_edge_penwidth(sub,topic) - color = self._calc_edge_color(sub,topic) - label = "("+str(conns) + " connections)" - return [label, penwidth, color] - - if sub in self.edges and topic in self.edges[sub] and pub in self.edges[sub][topic]: - penwidth = self._calc_edge_penwidth(sub,topic,pub) - color = self._calc_edge_color(sub,topic,pub) - period = self.edges[sub][topic][pub].period_mean.to_sec() - if period > 0.0: - freq = str(round(1.0 / period, 1)) - else: - freq = "?" - age = self.edges[sub][topic][pub].stamp_age_mean.to_sec() - age_string = "" - if age > 0.0: - age_string = " // " + str(round(age, 2) * 1000) + " ms" - label = freq + " Hz" + age_string - return [label, penwidth, color] - else: - return [None, None, None] - - def _add_edge(self, edge, dotcode_factory, dotgraph, is_topic=False): - if is_topic: - sub = edge.end - topic = edge.label - pub = edge.start - [stat_label, penwidth, color] = self._calc_statistic_info(sub, topic, pub) - if stat_label is not None: - temp_label = edge.label + "\\n" + stat_label - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=temp_label, url='topic:%s' % edge.label, penwidth=penwidth, color=color) - else: - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=edge.label, url='topic:%s' % edge.label) - else: - sub = edge.end.strip() - topic = edge.start.strip() - [stat_label, penwidth, color] = self._calc_statistic_info(sub, topic) - if stat_label is not None: - temp_label = edge.label + "\\n" + stat_label - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=temp_label, penwidth=penwidth, color=color) - else: - dotcode_factory.add_edge_to_graph(dotgraph, _conv(edge.start), _conv(edge.end), label=edge.label) - - - def _add_node(self, node, rosgraphinst, dotcode_factory, dotgraph, quiet): - if node in rosgraphinst.bad_nodes: - if quiet: - return '' - bn = rosgraphinst.bad_nodes[node] - if bn.type == rosgraph.impl.graph.BadNode.DEAD: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape="doublecircle", - url=node, - color="red") - else: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape="doublecircle", - url=node, - color="orange") - else: - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=node, - shape='ellipse', - url=node) - - def _add_topic_node(self, node, dotcode_factory, dotgraph, quiet): - label = rosgraph.impl.graph.node_topic(node) - dotcode_factory.add_node_to_graph(dotgraph, - nodename=_conv(node), - nodelabel=label, - shape='box', - url="topic:%s" % label) - - def _quiet_filter(self, name): - # ignore viewers - for n in QUIET_NAMES: - if n in name: - return False - return True - - def quiet_filter_topic_edge(self, edge): - for quiet_label in ['/time', '/clock', '/rosout', '/statistics']: - if quiet_label == edge.label: - return False - return self._quiet_filter(edge.start) and self._quiet_filter(edge.end) - - def generate_namespaces(self, graph, graph_mode, quiet=False): - """ - Determine the namespaces of the nodes being displayed - """ - namespaces = [] - if graph_mode == NODE_NODE_GRAPH: - nodes = graph.nn_nodes - if quiet: - nodes = [n for n in nodes if not n in QUIET_NAMES] - namespaces = list(set([roslib.names.namespace(n) for n in nodes])) - - elif graph_mode == NODE_TOPIC_GRAPH or \ - graph_mode == NODE_TOPIC_ALL_GRAPH: - nn_nodes = graph.nn_nodes - nt_nodes = graph.nt_nodes - if quiet: - nn_nodes = [n for n in nn_nodes if not n in QUIET_NAMES] - nt_nodes = [n for n in nt_nodes if not n in QUIET_NAMES] - if nn_nodes or nt_nodes: - namespaces = [roslib.names.namespace(n) for n in nn_nodes] - # an annoyance with the rosgraph library is that it - # prepends a space to topic names as they have to have - # different graph node namees from nodes. we have to strip here - namespaces.extend([roslib.names.namespace(n[1:]) for n in nt_nodes]) - - return list(set(namespaces)) - - def _filter_orphaned_edges(self, edges, nodes): - nodenames = [str(n).strip() for n in nodes] - # currently using and rule as the or rule generates orphan nodes with the current logic - return [e for e in edges if e.start.strip() in nodenames and e.end.strip() in nodenames] - - def _filter_orphaned_topics(self, nt_nodes, edges): - '''remove topic graphnodes without connected ROS nodes''' - removal_nodes = [] - for n in nt_nodes: - keep = False - for e in edges: - if (e.start.strip() == str(n).strip() or e.end.strip() == str(n).strip()): - keep = True - break - if not keep: - removal_nodes.append(n) - for n in removal_nodes: - nt_nodes.remove(n) - return nt_nodes - - def _split_filter_string(self, ns_filter): - '''splits a string after each comma, and treats tokens with leading dash as exclusions. - Adds .* as inclusion if no other inclusion option was given''' - includes = [] - excludes = [] - for name in ns_filter.split(','): - if name.strip().startswith('-'): - excludes.append(name.strip()[1:]) - else: - includes.append(name.strip()) - if includes == [] or includes == ['/'] or includes == ['']: - includes = ['.*'] - return includes, excludes - - def _get_node_edge_map(self, edges): - '''returns a dict mapping node name to edge objects partitioned in incoming and outgoing edges''' - node_connections = {} - for edge in edges: - if not edge.start in node_connections: - node_connections[edge.start] = NodeConnections() - if not edge.end in node_connections: - node_connections[edge.end] = NodeConnections() - node_connections[edge.start].outgoing.append(edge) - node_connections[edge.end].incoming.append(edge) - return node_connections - - def _filter_leaf_topics(self, - nodes_in, - edges_in, - node_connections, - hide_single_connection_topics, - hide_dead_end_topics): - ''' - removes certain ending topic nodes and their edges from list of nodes and edges - - @param hide_single_connection_topics: if true removes topics that are only published/subscribed by one node - @param hide_dead_end_topics: if true removes topics having only publishers - ''' - if not hide_dead_end_topics and not hide_single_connection_topics: - return nodes_in, edges_in - # do not manipulate incoming structures - nodes = copy.copy(nodes_in) - edges = copy.copy(edges_in) - removal_nodes = [] - for n in nodes: - if n in node_connections: - node_edges = [] - has_out_edges = False - node_edges.extend(node_connections[n].outgoing) - if len(node_connections[n].outgoing) > 0: - has_out_edges = True - node_edges.extend(node_connections[n].incoming) - if ((hide_single_connection_topics and len(node_edges) < 2) or - (hide_dead_end_topics and not has_out_edges)): - removal_nodes.append(n) - for e in node_edges: - if e in edges: - edges.remove(e) - for n in removal_nodes: - nodes.remove(n) - return nodes, edges - - def _accumulate_action_topics(self, nodes_in, edges_in, node_connections): - '''takes topic nodes, edges and node connections. - Returns topic nodes where action topics have been removed, - edges where the edges to action topics have been removed, and - a map with the connection to each virtual action topic node''' - removal_nodes = [] - action_nodes = {} - # do not manipulate incoming structures - nodes = copy.copy(nodes_in) - edges = copy.copy(edges_in) - for n in nodes: - if str(n).endswith('/feedback'): - prefix = str(n)[:-len('/feedback')].strip() - action_topic_nodes = [] - action_topic_edges_out = set() - action_topic_edges_in = set() - for suffix in ['/status', '/result', '/goal', '/cancel', '/feedback']: - for n2 in nodes: - if str(n2).strip() == prefix + suffix: - action_topic_nodes.append(n2) - if n2 in node_connections: - action_topic_edges_out.update(node_connections[n2].outgoing) - action_topic_edges_in.update(node_connections[n2].incoming) - if len(action_topic_nodes) == 5: - # found action - removal_nodes.extend(action_topic_nodes) - for e in action_topic_edges_out: - if e in edges: - edges.remove(e) - for e in action_topic_edges_in: - if e in edges: - edges.remove(e) - action_nodes[prefix] = {'topics': action_topic_nodes, - 'outgoing': action_topic_edges_out, - 'incoming': action_topic_edges_in} - for n in removal_nodes: - nodes.remove(n) - return nodes, edges, action_nodes - - def generate_dotgraph(self, - rosgraphinst, - ns_filter, - topic_filter, - graph_mode, - dotcode_factory, - hide_single_connection_topics=False, - hide_dead_end_topics=False, - cluster_namespaces_level=0, - accumulate_actions=True, - orientation='LR', - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - quiet=False): - """ - See generate_dotcode - """ - includes, excludes = self._split_filter_string(ns_filter) - topic_includes, topic_excludes = self._split_filter_string(topic_filter) - - nn_nodes = [] - nt_nodes = [] - # create the node definitions - if graph_mode == NODE_NODE_GRAPH: - nn_nodes = rosgraphinst.nn_nodes - nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)] - edges = rosgraphinst.nn_edges - edges = [e for e in edges if matches_any(e.label, topic_includes) and not matches_any(e.label, topic_excludes)] - - elif graph_mode == NODE_TOPIC_GRAPH or \ - graph_mode == NODE_TOPIC_ALL_GRAPH: - nn_nodes = rosgraphinst.nn_nodes - nt_nodes = rosgraphinst.nt_nodes - nn_nodes = [n for n in nn_nodes if matches_any(n, includes) and not matches_any(n, excludes)] - nt_nodes = [n for n in nt_nodes if matches_any(n, topic_includes) and not matches_any(n, topic_excludes)] - - # create the edge definitions, unwrap EdgeList objects into python lists - if graph_mode == NODE_TOPIC_GRAPH: - edges = [e for e in rosgraphinst.nt_edges] - else: - edges = [e for e in rosgraphinst.nt_all_edges] - - if quiet: - nn_nodes = filter(self._quiet_filter, nn_nodes) - nt_nodes = filter(self._quiet_filter, nt_nodes) - if graph_mode == NODE_NODE_GRAPH: - edges = filter(self.quiet_filter_topic_edge, edges) - - # for accumulating actions topics - action_nodes = {} - - if graph_mode != NODE_NODE_GRAPH and (hide_single_connection_topics or hide_dead_end_topics or accumulate_actions): - # maps outgoing and incoming edges to nodes - node_connections = self._get_node_edge_map(edges) - - nt_nodes, edges = self._filter_leaf_topics(nt_nodes, - edges, - node_connections, - hide_single_connection_topics, - hide_dead_end_topics) - - if accumulate_actions: - nt_nodes, edges, action_nodes = self._accumulate_action_topics(nt_nodes, edges, node_connections) - - edges = self._filter_orphaned_edges(edges, list(nn_nodes) + list(nt_nodes)) - nt_nodes = self._filter_orphaned_topics(nt_nodes, edges) - - # create the graph - # result = "digraph G {\n rankdir=%(orientation)s;\n%(nodes_str)s\n%(edges_str)s}\n" % vars() - dotgraph = dotcode_factory.get_graph(rank=rank, - ranksep=ranksep, - simplify=simplify, - rankdir=orientation) - - ACTION_TOPICS_SUFFIX = '/action_topics' - namespace_clusters = {} - for n in (nt_nodes or []) + [action_prefix + ACTION_TOPICS_SUFFIX for (action_prefix, _) in action_nodes.items()]: - # cluster topics with same namespace - if (cluster_namespaces_level > 0 and - str(n).count('/') > 1 and - len(str(n).split('/')[1]) > 0): - namespace = str(n).split('/')[1] - if namespace not in namespace_clusters: - namespace_clusters[namespace] = dotcode_factory.add_subgraph_to_graph(dotgraph, namespace, rank=rank, rankdir=orientation, simplify=simplify) - self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) - else: - self._add_topic_node(n, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - - # for ROS node, if we have created a namespace clusters for - # one of its peer topics, drop it into that cluster - if nn_nodes is not None: - for n in nn_nodes: - if (cluster_namespaces_level > 0 and - str(n).count('/') >= 1 and - len(str(n).split('/')[1]) > 0): - namespace = str(n).split('/')[1] - if namespace not in namespace_clusters: - namespace_clusters[namespace] = dotcode_factory.add_subgraph_to_graph(dotgraph, namespace, rank=rank, rankdir=orientation, simplify=simplify) - self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=namespace_clusters[namespace], quiet=quiet) - else: - self._add_node(n, rosgraphinst=rosgraphinst, dotcode_factory=dotcode_factory, dotgraph=dotgraph, quiet=quiet) - - for e in edges: - self._add_edge(e, dotcode_factory, dotgraph=dotgraph, is_topic=(graph_mode == NODE_NODE_GRAPH)) - - for (action_prefix, node_connections) in action_nodes.items(): - for out_edge in node_connections.get('outgoing', []): - dotcode_factory.add_edge_to_graph(dotgraph, _conv(action_prefix + ACTION_TOPICS_SUFFIX), _conv(out_edge.end)) - for in_edge in node_connections.get('incoming', []): - dotcode_factory.add_edge_to_graph(dotgraph, _conv(in_edge.start), _conv(action_prefix + ACTION_TOPICS_SUFFIX)) - return dotgraph - - def generate_dotcode(self, - rosgraphinst, - ns_filter, - topic_filter, - graph_mode, - dotcode_factory, - hide_single_connection_topics=False, - hide_dead_end_topics=False, - cluster_namespaces_level=0, - accumulate_actions=True, - orientation='LR', - rank='same', # None, same, min, max, source, sink - ranksep=0.2, # vertical distance between layers - rankdir='TB', # direction of layout (TB top > bottom, LR left > right) - simplify=True, # remove double edges - quiet=False): - """ - @param rosgraphinst: RosGraph instance - @param ns_filter: nodename filter - @type ns_filter: string - @param topic_filter: topicname filter - @type ns_filter: string - @param graph_mode str: NODE_NODE_GRAPH | NODE_TOPIC_GRAPH | NODE_TOPIC_ALL_GRAPH - @type graph_mode: str - @param orientation: rankdir value (see ORIENTATIONS dict) - @type dotcode_factory: object - @param dotcode_factory: abstract factory manipulating dot language objects - @param hide_single_connection_topics: if true remove topics with just one connection - @param hide_dead_end_topics: if true remove topics with publishers only - @param cluster_namespaces_level: if > 0 places box around members of same namespace (TODO: multiple namespace layers) - @param accumulate_actions: if true each 5 action topic graph nodes are shown as single graph node - @return: dotcode generated from graph singleton - @rtype: str - """ - dotgraph = self.generate_dotgraph(rosgraphinst=rosgraphinst, - ns_filter=ns_filter, - topic_filter=topic_filter, - graph_mode=graph_mode, - dotcode_factory=dotcode_factory, - hide_single_connection_topics=hide_single_connection_topics, - hide_dead_end_topics=hide_dead_end_topics, - cluster_namespaces_level=cluster_namespaces_level, - accumulate_actions=accumulate_actions, - orientation=orientation, - rank=rank, - ranksep=ranksep, - rankdir=rankdir, - simplify=simplify, - quiet=quiet) - dotcode = dotcode_factory.create_dot(dotgraph) - return dotcode diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py deleted file mode 100644 index 7d038e45..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/interactive_graphics_view.py +++ /dev/null @@ -1,103 +0,0 @@ -# Copyright (c) 2011, Dirk Thomas, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division - -from python_qt_binding.QtCore import QPointF, QRectF, Qt -from python_qt_binding.QtGui import QGraphicsView, QTransform - - -class InteractiveGraphicsView(QGraphicsView): - - def __init__(self, parent=None): - super(InteractiveGraphicsView, self).__init__(parent) - self.setObjectName('InteractiveGraphicsView') - - self._last_pan_point = None - self._last_scene_center = None - - def mousePressEvent(self, mouse_event): - self._last_pan_point = mouse_event.pos() - self._last_scene_center = self._map_to_scene_f(QRectF(self.frameRect()).center()) - self.setCursor(Qt.ClosedHandCursor) - - def mouseReleaseEvent(self, mouse_event): - self.setCursor(Qt.OpenHandCursor) - self._last_pan_point = None - - def mouseMoveEvent(self, mouse_event): - if self._last_pan_point is not None: - delta_scene = self.mapToScene(mouse_event.pos()) - self.mapToScene(self._last_pan_point) - if not delta_scene.isNull(): - self.centerOn(self._last_scene_center - delta_scene) - self._last_scene_center -= delta_scene - self._last_pan_point = mouse_event.pos() - QGraphicsView.mouseMoveEvent(self, mouse_event) - - def wheelEvent(self, wheel_event): - if wheel_event.modifiers() == Qt.NoModifier: - delta = wheel_event.delta() - delta = max(min(delta, 480), -480) - mouse_before_scale_in_scene = self.mapToScene(wheel_event.pos()) - - scale_factor = 1 + (0.2 * (delta / 120.0)) - scaling = QTransform(scale_factor, 0, 0, scale_factor, 0, 0) - self.setTransform(self.transform() * scaling) - - mouse_after_scale_in_scene = self.mapToScene(wheel_event.pos()) - center_in_scene = self.mapToScene(self.frameRect().center()) - self.centerOn(center_in_scene + mouse_before_scale_in_scene - mouse_after_scale_in_scene) - - wheel_event.accept() - else: - QGraphicsView.wheelEvent(self, wheel_event) - - def _map_to_scene_f(self, pointf): - point = pointf.toPoint() - if pointf.x() == point.x() and pointf.y() == point.y(): - # map integer coordinates - return self.mapToScene(point) - elif pointf.x() == point.x(): - # map integer x and decimal y coordinates - pointA = self.mapToScene((pointf + QPointF(0, -0.5)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(0, 0.5)).toPoint()) - return (pointA + pointB) / 2.0 - elif pointf.y() == point.y(): - # map decimal x and integer y and coordinates - pointA = self.mapToScene((pointf + QPointF(-0.5, 0)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(0.5, 0)).toPoint()) - return (pointA + pointB) / 2.0 - else: - # map decimal coordinates - pointA = self.mapToScene((pointf + QPointF(-0.5, -0.5)).toPoint()) - pointB = self.mapToScene((pointf + QPointF(-0.5, 0.5)).toPoint()) - pointC = self.mapToScene((pointf + QPointF(0.5, -0.5)).toPoint()) - pointD = self.mapToScene((pointf + QPointF(0.5, 0.5)).toPoint()) - return (pointA + pointB + pointC + pointD) / 4.0 diff --git a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py b/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py deleted file mode 100644 index bb2708f2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/src/rqt_graph/ros_graph.py +++ /dev/null @@ -1,368 +0,0 @@ -# Copyright (c) 2011, Dirk Thomas, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QAbstractListModel, QFile, QIODevice, Qt, Signal -from python_qt_binding.QtGui import QCompleter, QFileDialog, QGraphicsScene, QIcon, QImage, QPainter, QWidget -from python_qt_binding.QtSvg import QSvgGenerator - -import rosgraph.impl.graph -import rosservice -import rostopic - -from qt_dotgraph.dot_to_qt import DotToQtGenerator -# pydot requires some hacks -from qt_dotgraph.pydotfactory import PydotFactory -from rqt_gui_py.plugin import Plugin -# TODO: use pygraphviz instead, but non-deterministic layout will first be resolved in graphviz 2.30 -# from qtgui_plugin.pygraphvizfactory import PygraphvizFactory - -from .dotcode import RosGraphDotcodeGenerator, NODE_NODE_GRAPH, NODE_TOPIC_ALL_GRAPH, NODE_TOPIC_GRAPH -from .interactive_graphics_view import InteractiveGraphicsView - - -class RepeatedWordCompleter(QCompleter): - """A completer that completes multiple times from a list""" - def init(self, parent=None): - QCompleter.init(self, parent) - - def pathFromIndex(self, index): - path = QCompleter.pathFromIndex(self, index) - lst = str(self.widget().text()).split(',') - if len(lst) > 1: - path = '%s, %s' % (','.join(lst[:-1]), path) - return path - - def splitPath(self, path): - path = str(path.split(',')[-1]).lstrip(' ') - return [path] - - -class NamespaceCompletionModel(QAbstractListModel): - """Ros package and stacknames""" - def __init__(self, linewidget, topics_only): - super(NamespaceCompletionModel, self).__init__(linewidget) - self.names = [] - - def refresh(self, names): - namesset = set() - for n in names: - namesset.add(str(n).strip()) - namesset.add("-%s" % (str(n).strip())) - self.names = sorted(namesset) - - def rowCount(self, parent): - return len(self.names) - - def data(self, index, role): - if index.isValid() and (role == Qt.DisplayRole or role == Qt.EditRole): - return self.names[index.row()] - return None - - -class RosGraph(Plugin): - - _deferred_fit_in_view = Signal() - - def __init__(self, context): - super(RosGraph, self).__init__(context) - self.initialized = False - self.setObjectName('RosGraph') - - self._graph = None - self._current_dotcode = None - - self._widget = QWidget() - - # factory builds generic dotcode items - self.dotcode_factory = PydotFactory() - # self.dotcode_factory = PygraphvizFactory() - # generator builds rosgraph - self.dotcode_generator = RosGraphDotcodeGenerator() - # dot_to_qt transforms into Qt elements using dot layout - self.dot_to_qt = DotToQtGenerator() - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_graph'), 'resource', 'RosGraph.ui') - loadUi(ui_file, self._widget, {'InteractiveGraphicsView': InteractiveGraphicsView}) - self._widget.setObjectName('RosGraphUi') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - self._scene = QGraphicsScene() - self._scene.setBackgroundBrush(Qt.white) - self._widget.graphics_view.setScene(self._scene) - - self._widget.graph_type_combo_box.insertItem(0, self.tr('Nodes only'), NODE_NODE_GRAPH) - self._widget.graph_type_combo_box.insertItem(1, self.tr('Nodes/Topics (active)'), NODE_TOPIC_GRAPH) - self._widget.graph_type_combo_box.insertItem(2, self.tr('Nodes/Topics (all)'), NODE_TOPIC_ALL_GRAPH) - self._widget.graph_type_combo_box.setCurrentIndex(0) - self._widget.graph_type_combo_box.currentIndexChanged.connect(self._refresh_rosgraph) - - self.node_completionmodel = NamespaceCompletionModel(self._widget.filter_line_edit, False) - completer = RepeatedWordCompleter(self.node_completionmodel, self) - completer.setCompletionMode(QCompleter.PopupCompletion) - completer.setWrapAround(True) - completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.filter_line_edit.editingFinished.connect(self._refresh_rosgraph) - self._widget.filter_line_edit.setCompleter(completer) - - self.topic_completionmodel = NamespaceCompletionModel(self._widget.topic_filter_line_edit, False) - topic_completer = RepeatedWordCompleter(self.topic_completionmodel, self) - topic_completer.setCompletionMode(QCompleter.PopupCompletion) - topic_completer.setWrapAround(True) - topic_completer.setCaseSensitivity(Qt.CaseInsensitive) - self._widget.topic_filter_line_edit.editingFinished.connect(self._refresh_rosgraph) - self._widget.topic_filter_line_edit.setCompleter(topic_completer) - - self._widget.namespace_cluster_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.actionlib_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.dead_sinks_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.leaf_topics_check_box.clicked.connect(self._refresh_rosgraph) - self._widget.quiet_check_box.clicked.connect(self._refresh_rosgraph) - - self._widget.refresh_graph_push_button.setIcon(QIcon.fromTheme('view-refresh')) - self._widget.refresh_graph_push_button.pressed.connect(self._update_rosgraph) - - self._widget.highlight_connections_check_box.toggled.connect(self._redraw_graph_view) - self._widget.auto_fit_graph_check_box.toggled.connect(self._redraw_graph_view) - self._widget.fit_in_view_push_button.setIcon(QIcon.fromTheme('zoom-original')) - self._widget.fit_in_view_push_button.pressed.connect(self._fit_in_view) - - self._widget.load_dot_push_button.setIcon(QIcon.fromTheme('document-open')) - self._widget.load_dot_push_button.pressed.connect(self._load_dot) - self._widget.save_dot_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_dot_push_button.pressed.connect(self._save_dot) - self._widget.save_as_svg_push_button.setIcon(QIcon.fromTheme('document-save-as')) - self._widget.save_as_svg_push_button.pressed.connect(self._save_svg) - self._widget.save_as_image_push_button.setIcon(QIcon.fromTheme('image')) - self._widget.save_as_image_push_button.pressed.connect(self._save_image) - - self._update_rosgraph() - self._deferred_fit_in_view.connect(self._fit_in_view, Qt.QueuedConnection) - self._deferred_fit_in_view.emit() - - context.add_widget(self._widget) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('graph_type_combo_box_index', self._widget.graph_type_combo_box.currentIndex()) - instance_settings.set_value('filter_line_edit_text', self._widget.filter_line_edit.text()) - instance_settings.set_value('topic_filter_line_edit_text', self._widget.topic_filter_line_edit.text()) - instance_settings.set_value('namespace_cluster_check_box_state', self._widget.namespace_cluster_check_box.isChecked()) - instance_settings.set_value('actionlib_check_box_state', self._widget.actionlib_check_box.isChecked()) - instance_settings.set_value('dead_sinks_check_box_state', self._widget.dead_sinks_check_box.isChecked()) - instance_settings.set_value('leaf_topics_check_box_state', self._widget.leaf_topics_check_box.isChecked()) - instance_settings.set_value('quiet_check_box_state', self._widget.quiet_check_box.isChecked()) - instance_settings.set_value('auto_fit_graph_check_box_state', self._widget.auto_fit_graph_check_box.isChecked()) - instance_settings.set_value('highlight_connections_check_box_state', self._widget.highlight_connections_check_box.isChecked()) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.graph_type_combo_box.setCurrentIndex(int(instance_settings.value('graph_type_combo_box_index', 0))) - self._widget.filter_line_edit.setText(instance_settings.value('filter_line_edit_text', '/')) - self._widget.topic_filter_line_edit.setText(instance_settings.value('topic_filter_line_edit_text', '/')) - self._widget.namespace_cluster_check_box.setChecked(instance_settings.value('namespace_cluster_check_box_state', True) in [True, 'true']) - self._widget.actionlib_check_box.setChecked(instance_settings.value('actionlib_check_box_state', True) in [True, 'true']) - self._widget.dead_sinks_check_box.setChecked(instance_settings.value('dead_sinks_check_box_state', True) in [True, 'true']) - self._widget.leaf_topics_check_box.setChecked(instance_settings.value('leaf_topics_check_box_state', True) in [True, 'true']) - self._widget.quiet_check_box.setChecked(instance_settings.value('quiet_check_box_state', True) in [True, 'true']) - self._widget.auto_fit_graph_check_box.setChecked(instance_settings.value('auto_fit_graph_check_box_state', True) in [True, 'true']) - self._widget.highlight_connections_check_box.setChecked(instance_settings.value('highlight_connections_check_box_state', True) in [True, 'true']) - self.initialized = True - self._refresh_rosgraph() - - def _update_rosgraph(self): - # re-enable controls customizing fetched ROS graph - self._widget.graph_type_combo_box.setEnabled(True) - self._widget.filter_line_edit.setEnabled(True) - self._widget.topic_filter_line_edit.setEnabled(True) - self._widget.namespace_cluster_check_box.setEnabled(True) - self._widget.actionlib_check_box.setEnabled(True) - self._widget.dead_sinks_check_box.setEnabled(True) - self._widget.leaf_topics_check_box.setEnabled(True) - self._widget.quiet_check_box.setEnabled(True) - - self._graph = rosgraph.impl.graph.Graph() - self._graph.set_master_stale(5.0) - self._graph.set_node_stale(5.0) - self._graph.update() - self.node_completionmodel.refresh(self._graph.nn_nodes) - self.topic_completionmodel.refresh(self._graph.nt_nodes) - self._refresh_rosgraph() - - def _refresh_rosgraph(self): - if not self.initialized: - return - self._update_graph_view(self._generate_dotcode()) - - def _generate_dotcode(self): - ns_filter = self._widget.filter_line_edit.text() - topic_filter = self._widget.topic_filter_line_edit.text() - graph_mode = self._widget.graph_type_combo_box.itemData(self._widget.graph_type_combo_box.currentIndex()) - orientation = 'LR' - if self._widget.namespace_cluster_check_box.isChecked(): - namespace_cluster = 1 - else: - namespace_cluster = 0 - accumulate_actions = self._widget.actionlib_check_box.isChecked() - hide_dead_end_topics = self._widget.dead_sinks_check_box.isChecked() - hide_single_connection_topics = self._widget.leaf_topics_check_box.isChecked() - quiet = self._widget.quiet_check_box.isChecked() - - return self.dotcode_generator.generate_dotcode( - rosgraphinst=self._graph, - ns_filter=ns_filter, - topic_filter=topic_filter, - graph_mode=graph_mode, - hide_single_connection_topics=hide_single_connection_topics, - hide_dead_end_topics=hide_dead_end_topics, - cluster_namespaces_level=namespace_cluster, - accumulate_actions=accumulate_actions, - dotcode_factory=self.dotcode_factory, - orientation=orientation, - quiet=quiet) - - def _update_graph_view(self, dotcode): - if dotcode == self._current_dotcode: - return - self._current_dotcode = dotcode - self._redraw_graph_view() - - def _generate_tool_tip(self, url): - if url is not None and ':' in url: - item_type, item_path = url.split(':', 1) - if item_type == 'node': - tool_tip = 'Node:\n %s' % (item_path) - service_names = rosservice.get_service_list(node=item_path) - if service_names: - tool_tip += '\nServices:' - for service_name in service_names: - try: - service_type = rosservice.get_service_type(service_name) - tool_tip += '\n %s [%s]' % (service_name, service_type) - except rosservice.ROSServiceIOException as e: - tool_tip += '\n %s' % (e) - return tool_tip - elif item_type == 'topic': - topic_type, topic_name, _ = rostopic.get_topic_type(item_path) - return 'Topic:\n %s\nType:\n %s' % (topic_name, topic_type) - return url - - def _redraw_graph_view(self): - self._scene.clear() - - if self._widget.highlight_connections_check_box.isChecked(): - highlight_level = 3 - else: - highlight_level = 1 - - # layout graph and create qt items - (nodes, edges) = self.dot_to_qt.dotcode_to_qt_items(self._current_dotcode, - highlight_level=highlight_level, - same_label_siblings=True) - - for node_item in nodes.itervalues(): - self._scene.addItem(node_item) - for edge_items in edges.itervalues(): - for edge_item in edge_items: - edge_item.add_to_scene(self._scene) - - self._scene.setSceneRect(self._scene.itemsBoundingRect()) - if self._widget.auto_fit_graph_check_box.isChecked(): - self._fit_in_view() - - def _load_dot(self, file_name=None): - if file_name is None: - file_name, _ = QFileDialog.getOpenFileName(self._widget, self.tr('Open graph from file'), None, self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - try: - fh = open(file_name, 'rb') - dotcode = fh.read() - fh.close() - except IOError: - return - - # disable controls customizing fetched ROS graph - self._widget.graph_type_combo_box.setEnabled(False) - self._widget.filter_line_edit.setEnabled(False) - self._widget.topic_filter_line_edit.setEnabled(False) - self._widget.namespace_cluster_check_box.setEnabled(False) - self._widget.actionlib_check_box.setEnabled(False) - self._widget.dead_sinks_check_box.setEnabled(False) - self._widget.leaf_topics_check_box.setEnabled(False) - self._widget.quiet_check_box.setEnabled(False) - - self._update_graph_view(dotcode) - - def _fit_in_view(self): - self._widget.graphics_view.fitInView(self._scene.itemsBoundingRect(), Qt.KeepAspectRatio) - - def _save_dot(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as DOT'), 'rosgraph.dot', self.tr('DOT graph (*.dot)')) - if file_name is None or file_name == '': - return - - handle = QFile(file_name) - if not handle.open(QIODevice.WriteOnly | QIODevice.Text): - return - - handle.write(self._current_dotcode) - handle.close() - - def _save_svg(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as SVG'), 'rosgraph.svg', self.tr('Scalable Vector Graphic (*.svg)')) - if file_name is None or file_name == '': - return - - generator = QSvgGenerator() - generator.setFileName(file_name) - generator.setSize((self._scene.sceneRect().size() * 2.0).toSize()) - - painter = QPainter(generator) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - - def _save_image(self): - file_name, _ = QFileDialog.getSaveFileName(self._widget, self.tr('Save as image'), 'rosgraph.png', self.tr('Image (*.bmp *.jpg *.png *.tiff)')) - if file_name is None or file_name == '': - return - - img = QImage((self._scene.sceneRect().size() * 2.0).toSize(), QImage.Format_ARGB32_Premultiplied) - painter = QPainter(img) - painter.setRenderHint(QPainter.Antialiasing) - self._scene.render(painter) - painter.end() - img.save(file_name) diff --git a/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py b/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py deleted file mode 100644 index dd2c5b71..00000000 --- a/workspace/src/rqt_common_plugins/rqt_graph/test/dotcode_test.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest -import rospkg - -import os -from rqt_graph.dotcode import RosGraphDotcodeGenerator - -PKG='rqt_graph' - -class DotcodeTest(unittest.TestCase): - - def test_split_filter_empty(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('') - self.assertEqual(['.*'], inc) - self.assertEqual(0, len(exc)) - inc, exc = gen._split_filter_string('/') - self.assertEqual(['.*'], inc) - self.assertEqual(0, len(exc)) - - - def test_split_filter_includes(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('foo') - self.assertEqual(['foo'], inc) - self.assertEqual(0, len(exc)) - inc, exc = gen._split_filter_string('foo,bar') - self.assertEqual(['foo', 'bar'], inc) - self.assertEqual(0, len(exc)) - - def test_split_filter_excludes(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('-foo') - self.assertEqual(['.*'], inc) - self.assertEqual(['foo'], exc) - inc, exc = gen._split_filter_string('-foo,-bar') - self.assertEqual(['.*'], inc) - self.assertEqual(['foo', 'bar'], exc) - - def test_split_filter_both(self): - gen = RosGraphDotcodeGenerator() - inc, exc = gen._split_filter_string('-foo , bar ,baz, -bam') - self.assertEqual(['bar', 'baz'], inc) - self.assertEqual(['foo', 'bam'], exc) - - class MockEdge(): - def __init__(self, start, end): - self.start = start - self.end = end - - def test_get_node_edge_map(self): - gen = RosGraphDotcodeGenerator() - e1 = self.MockEdge('foo', 'bar') - nmap = gen._get_node_edge_map([e1]) - self.assertEqual(2, len(nmap)) - self.assertTrue('foo' in nmap) - self.assertTrue('bar' in nmap) - self.assertEqual([], nmap['foo'].incoming) - self.assertEqual([e1], nmap['foo'].outgoing) - self.assertEqual([e1], nmap['bar'].incoming) - self.assertEqual([], nmap['bar'].outgoing) - - def test_get_node_edge_map(self): - gen = RosGraphDotcodeGenerator() - e1 = self.MockEdge('foo', 'bar') - e2 = self.MockEdge('bar', 'foo') - e3 = self.MockEdge('foo', 'pam') - nmap = gen._get_node_edge_map([e1, e2, e3]) - self.assertEqual(3, len(nmap)) - self.assertTrue('foo' in nmap) - self.assertTrue('bar' in nmap) - self.assertTrue('pam' in nmap) - self.assertEqual([e2], nmap['foo'].incoming) - self.assertEqual([e1, e3], nmap['foo'].outgoing) - self.assertEqual([e1], nmap['bar'].incoming) - self.assertEqual([e2], nmap['bar'].outgoing) - - def test_filter_leaf_topics_single_connection(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - True, #hide_single_connection_topics, - False #hide_dead_end_topics - ) - self.assertEqual(['foo', 'bar'], rnodes) - self.assertEqual([e1, e2, e3, e4], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - def test_filter_leaf_topics_dead_end(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - False, #hide_single_connection_topics, - True #hide_dead_end_topics - ) - self.assertEqual(['bar', 'boo'], rnodes) - self.assertEqual([e3, e4, e6], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - - def test_filter_leaf_topics_both(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo', 'bar', 'pam', 'boo'] - e1 = self.MockEdge('n1', 'foo') - e2 = self.MockEdge('n2', 'foo') - e3 = self.MockEdge('n3', 'bar') - e4 = self.MockEdge('bar', 'n4') - e5 = self.MockEdge('n5', 'pam') - e6 = self.MockEdge('boo', 'n6') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - print(node_connections) - rnodes, redges = gen._filter_leaf_topics(topic_nodes, - edges, - node_connections, - True, #hide_single_connection_topics, - True #hide_dead_end_topics - ) - self.assertEqual(['bar'], rnodes) - self.assertEqual([e3, e4], redges) - self.assertEqual(['foo', 'bar', 'pam', 'boo'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - def test_accumulate_action_topics(self): - gen = RosGraphDotcodeGenerator() - topic_nodes = ['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'] - e1 = self.MockEdge('n1', 'foo/feedback') - e2 = self.MockEdge('n1', 'foo/goal') - e3 = self.MockEdge('n1', 'foo/cancel') - e4 = self.MockEdge('n1', 'foo/result') - e5 = self.MockEdge('n1', 'foo/status') - e6 = self.MockEdge('n2', 'bar') - edges = [e1, e2, e3, e4, e5, e6] - node_connections = gen._get_node_edge_map(edges) - rnodes, redges, raction_nodes = gen._accumulate_action_topics(topic_nodes, edges, node_connections) - self.assertEqual(1, len(rnodes)) # node 'bar' - self.assertEqual(1, len(redges)) - self.assertEqual(1, len(raction_nodes)) - self.assertEqual(['foo/feedback', 'foo/goal', 'foo/cancel', 'foo/result', 'foo/status', 'bar'], topic_nodes) - self.assertEqual([e1, e2, e3, e4, e5, e6], edges) - - -if __name__ == '__main__': - import rosunit - rosunit.unitrun(PKG, 'dotcode_test', DotcodeTest) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst deleted file mode 100644 index a82b4842..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/CHANGELOG.rst +++ /dev/null @@ -1,115 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_image_view -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* use proper icon for images -* add optional topic argument to rqt_image_view -* fix width of save-as-image button -* Contributors: Dirk Thomas, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- -* Added button to save current image to file -* Contributors: Dirk Thomas - -0.3.11 (2015-04-30) -------------------- -* fix image shrinking problem (`#291 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- -* list image transport topics if parent image topic is not available (`#215 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* properly handle aligned images -* wrap cv calls in try-catch-block (`#201 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* fix event handling for rqt_image_view enabling to run multiple instances simultaneously (`#66 `_) -* add rqt_image_view to global bin (`#168 `_) - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- -* Optimized by taking more advantage of cv_bridge - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt deleted file mode 100644 index addc49af..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) - -project(rqt_image_view) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED COMPONENTS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge) - -find_package(Qt4 COMPONENTS QtCore QtGui REQUIRED) - -include(${QT_USE_FILE}) - -set(rqt_image_view_SRCS - src/rqt_image_view/image_view.cpp - src/rqt_image_view/ratio_layouted_frame.cpp -) - -set(rqt_image_view_HDRS - include/rqt_image_view/image_view.h - include/rqt_image_view/ratio_layouted_frame.h -) - -set(rqt_image_view_UIS - src/rqt_image_view/image_view.ui -) - -set(rqt_image_view_INCLUDE_DIRECTORIES - include - ${CMAKE_CURRENT_BINARY_DIR} -) - -catkin_package( - INCLUDE_DIRS ${rqt_image_view_INCLUDE_DIRECTORIES} - LIBRARIES ${PROJECT_NAME} - CATKIN_DEPENDS rqt_gui rqt_gui_cpp image_transport sensor_msgs cv_bridge -) -catkin_python_setup() - -qt4_wrap_cpp(rqt_image_view_MOCS ${rqt_image_view_HDRS}) -qt4_wrap_ui(rqt_image_view_UIS_H ${rqt_image_view_UIS}) - -include_directories(${rqt_image_view_INCLUDE_DIRECTORIES} ${catkin_INCLUDE_DIRS}) -add_library(${PROJECT_NAME} ${rqt_image_view_SRCS} ${rqt_image_view_MOCS} ${rqt_image_view_UIS_H}) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_QTCORE_LIBRARY} ${QT_QTGUI_LIBRARY}) - -find_package(class_loader) -class_loader_hide_library_symbols(${PROJECT_NAME}) - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -install(PROGRAMS scripts/rqt_image_view - DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) -install(PROGRAMS scripts/rqt_image_view - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h b/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h deleted file mode 100644 index 005a9500..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/image_view.h +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef rqt_image_view__ImageView_H -#define rqt_image_view__ImageView_H - -#include - -#include - -#include -#include -#include - -#include - -#include -#include -#include -#include - -#include - -namespace rqt_image_view { - -class ImageView - : public rqt_gui_cpp::Plugin -{ - - Q_OBJECT - -public: - - ImageView(); - - virtual void initPlugin(qt_gui_cpp::PluginContext& context); - - virtual void shutdownPlugin(); - - virtual void saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const; - - virtual void restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings); - -protected slots: - - virtual void updateTopicList(); - -protected: - - // deprecated function for backward compatibility only, use getTopics() instead - ROS_DEPRECATED virtual QList getTopicList(const QSet& message_types, const QList& transports); - - virtual QSet getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports); - - virtual void selectTopic(const QString& topic); - -protected slots: - - virtual void onTopicChanged(int index); - - virtual void onZoom1(bool checked); - - virtual void onDynamicRange(bool checked); - - virtual void saveImage(); - -protected: - - virtual void callbackImage(const sensor_msgs::Image::ConstPtr& msg); - - Ui::ImageViewWidget ui_; - - QWidget* widget_; - - image_transport::Subscriber subscriber_; - - cv::Mat conversion_mat_; - -private: - - QString arg_topic_name; - -}; - -} - -#endif // rqt_image_view__ImageView_H diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h b/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h deleted file mode 100644 index dccf9636..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/include/rqt_image_view/ratio_layouted_frame.h +++ /dev/null @@ -1,102 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef rqt_image_view__RatioLayoutedFrame_H -#define rqt_image_view__RatioLayoutedFrame_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace rqt_image_view { - -/** - * RatioLayoutedFrame is a layout containing a single frame with a fixed aspect ratio. - * The default aspect ratio is 4:3. - */ -class RatioLayoutedFrame - : public QFrame -{ - - Q_OBJECT - -public: - - RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags = 0); - - virtual ~RatioLayoutedFrame(); - - const QImage& getImage() const; - - QImage getImageCopy() const; - - void setImage(const QImage& image); - - QRect getAspectRatioCorrectPaintArea(); - - void resizeToFitAspectRatio(); - - void setInnerFrameMinimumSize(const QSize& size); - - void setInnerFrameMaximumSize(const QSize& size); - - void setInnerFrameFixedSize(const QSize& size); - -signals: - - void delayed_update(); - -protected: - - void setAspectRatio(unsigned short width, unsigned short height); - - void paintEvent(QPaintEvent* event); - -private: - - static int greatestCommonDivisor(int a, int b); - - QSize aspect_ratio_; - - QImage qimage_; - mutable QMutex qimage_mutex_; - -}; - -} - -#endif // rqt_image_view__RatioLayoutedFrame_H diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox deleted file mode 100644 index 93550db9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_image_view provides a GUI plugin for displaying images using image_transport. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/package.xml b/workspace/src/rqt_common_plugins/rqt_image_view/package.xml deleted file mode 100644 index ebc59429..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_image_view - 0.3.13 - rqt_image_view provides a GUI plugin for displaying images using image_transport. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_image_view - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dirk Thomas - - catkin - - cv_bridge - image_transport - rqt_gui - rqt_gui_cpp - sensor_msgs - cv_bridge - image_transport - rqt_gui - rqt_gui_cpp - sensor_msgs - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml b/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml deleted file mode 100644 index 824cfbf9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A GUI plugin to view a sensor_msgs/Image topic. - - - - - folder - Plugins related to visualization. - - - image-x-generic - A GUI plugin for viewing sensor_msgs/Image topics. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view b/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view deleted file mode 100755 index b655911a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/scripts/rqt_image_view +++ /dev/null @@ -1,16 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - - -def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_image_view plugin') - group.add_argument('topic', nargs='?', help='The topic name to subscribe to') - -main = Main() -sys.exit(main.main( - sys.argv, - standalone='rqt_image_view/ImageView', - plugin_argument_provider=add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/setup.py b/workspace/src/rqt_common_plugins/rqt_image_view/setup.py deleted file mode 100644 index ae646dc3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_image_view'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp deleted file mode 100644 index 8ad95938..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.cpp +++ /dev/null @@ -1,371 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include -#include -#include - -#include -#include - -#include -#include -#include - -namespace rqt_image_view { - -ImageView::ImageView() - : rqt_gui_cpp::Plugin() - , widget_(0) -{ - setObjectName("ImageView"); -} - -void ImageView::initPlugin(qt_gui_cpp::PluginContext& context) -{ - widget_ = new QWidget(); - ui_.setupUi(widget_); - - if (context.serialNumber() > 1) - { - widget_->setWindowTitle(widget_->windowTitle() + " (" + QString::number(context.serialNumber()) + ")"); - } - context.addWidget(widget_); - - updateTopicList(); - ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText("")); - connect(ui_.topics_combo_box, SIGNAL(currentIndexChanged(int)), this, SLOT(onTopicChanged(int))); - - ui_.refresh_topics_push_button->setIcon(QIcon::fromTheme("view-refresh")); - connect(ui_.refresh_topics_push_button, SIGNAL(pressed()), this, SLOT(updateTopicList())); - - ui_.zoom_1_push_button->setIcon(QIcon::fromTheme("zoom-original")); - connect(ui_.zoom_1_push_button, SIGNAL(toggled(bool)), this, SLOT(onZoom1(bool))); - - connect(ui_.dynamic_range_check_box, SIGNAL(toggled(bool)), this, SLOT(onDynamicRange(bool))); - - ui_.save_as_image_push_button->setIcon(QIcon::fromTheme("image-x-generic")); - connect(ui_.save_as_image_push_button, SIGNAL(pressed()), this, SLOT(saveImage())); - - // set topic name if passed in as argument - const QStringList& argv = context.argv(); - if (!argv.empty()) { - arg_topic_name = argv[0]; - // add topic name to list if not yet in - int index = ui_.topics_combo_box->findText(arg_topic_name); - if (index == -1) { - QString label(arg_topic_name); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(arg_topic_name)); - ui_.topics_combo_box->setCurrentIndex(ui_.topics_combo_box->findText(arg_topic_name)); - } - } -} - -void ImageView::shutdownPlugin() -{ - subscriber_.shutdown(); -} - -void ImageView::saveSettings(qt_gui_cpp::Settings& plugin_settings, qt_gui_cpp::Settings& instance_settings) const -{ - QString topic = ui_.topics_combo_box->currentText(); - //qDebug("ImageView::saveSettings() topic '%s'", topic.toStdString().c_str()); - instance_settings.setValue("topic", topic); - instance_settings.setValue("zoom1", ui_.zoom_1_push_button->isChecked()); - instance_settings.setValue("dynamic_range", ui_.dynamic_range_check_box->isChecked()); - instance_settings.setValue("max_range", ui_.max_range_double_spin_box->value()); -} - -void ImageView::restoreSettings(const qt_gui_cpp::Settings& plugin_settings, const qt_gui_cpp::Settings& instance_settings) -{ - bool zoom1_checked = instance_settings.value("zoom1", false).toBool(); - ui_.zoom_1_push_button->setChecked(zoom1_checked); - - bool dynamic_range_checked = instance_settings.value("dynamic_range", false).toBool(); - ui_.dynamic_range_check_box->setChecked(dynamic_range_checked); - - double max_range = instance_settings.value("max_range", ui_.max_range_double_spin_box->value()).toDouble(); - ui_.max_range_double_spin_box->setValue(max_range); - - QString topic = instance_settings.value("topic", "").toString(); - // don't overwrite topic name passed as command line argument - if (!arg_topic_name.isEmpty()) - { - arg_topic_name = ""; - } - else - { - //qDebug("ImageView::restoreSettings() topic '%s'", topic.toStdString().c_str()); - selectTopic(topic); - } -} - -void ImageView::updateTopicList() -{ - QSet message_types; - message_types.insert("sensor_msgs/Image"); - QSet message_sub_types; - message_sub_types.insert("sensor_msgs/CompressedImage"); - - // get declared transports - QList transports; - image_transport::ImageTransport it(getNodeHandle()); - std::vector declared = it.getDeclaredTransports(); - for (std::vector::const_iterator it = declared.begin(); it != declared.end(); it++) - { - //qDebug("ImageView::updateTopicList() declared transport '%s'", it->c_str()); - QString transport = it->c_str(); - - // strip prefix from transport name - QString prefix = "image_transport/"; - if (transport.startsWith(prefix)) - { - transport = transport.mid(prefix.length()); - } - transports.append(transport); - } - - QString selected = ui_.topics_combo_box->currentText(); - - // fill combo box - QList topics = getTopics(message_types, message_sub_types, transports).values(); - topics.append(""); - qSort(topics); - ui_.topics_combo_box->clear(); - for (QList::const_iterator it = topics.begin(); it != topics.end(); it++) - { - QString label(*it); - label.replace(" ", "/"); - ui_.topics_combo_box->addItem(label, QVariant(*it)); - } - - // restore previous selection - selectTopic(selected); -} - -QList ImageView::getTopicList(const QSet& message_types, const QList& transports) -{ - QSet message_sub_types; - return getTopics(message_types, message_sub_types, transports).values(); -} - -QSet ImageView::getTopics(const QSet& message_types, const QSet& message_sub_types, const QList& transports) -{ - ros::master::V_TopicInfo topic_info; - ros::master::getTopics(topic_info); - - QSet all_topics; - for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++) - { - all_topics.insert(it->name.c_str()); - } - - QSet topics; - for (ros::master::V_TopicInfo::const_iterator it = topic_info.begin(); it != topic_info.end(); it++) - { - if (message_types.contains(it->datatype.c_str())) - { - QString topic = it->name.c_str(); - - // add raw topic - topics.insert(topic); - //qDebug("ImageView::getTopics() raw topic '%s'", topic.toStdString().c_str()); - - // add transport specific sub-topics - for (QList::const_iterator jt = transports.begin(); jt != transports.end(); jt++) - { - if (all_topics.contains(topic + "/" + *jt)) - { - QString sub = topic + " " + *jt; - topics.insert(sub); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", sub.toStdString().c_str()); - } - } - } - if (message_sub_types.contains(it->datatype.c_str())) - { - QString topic = it->name.c_str(); - int index = topic.lastIndexOf("/"); - if (index != -1) - { - topic.replace(index, 1, " "); - topics.insert(topic); - //qDebug("ImageView::getTopics() transport specific sub-topic '%s'", topic.toStdString().c_str()); - } - } - } - return topics; -} - -void ImageView::selectTopic(const QString& topic) -{ - int index = ui_.topics_combo_box->findText(topic); - if (index == -1) - { - index = ui_.topics_combo_box->findText(""); - } - ui_.topics_combo_box->setCurrentIndex(index); -} - -void ImageView::onTopicChanged(int index) -{ - subscriber_.shutdown(); - - // reset image on topic change - ui_.image_frame->setImage(QImage()); - - QStringList parts = ui_.topics_combo_box->itemData(index).toString().split(" "); - QString topic = parts.first(); - QString transport = parts.length() == 2 ? parts.last() : "raw"; - - if (!topic.isEmpty()) - { - image_transport::ImageTransport it(getNodeHandle()); - image_transport::TransportHints hints(transport.toStdString()); - try { - subscriber_ = it.subscribe(topic.toStdString(), 1, &ImageView::callbackImage, this, hints); - //qDebug("ImageView::onTopicChanged() to topic '%s' with transport '%s'", topic.toStdString().c_str(), subscriber_.getTransport().c_str()); - } catch (image_transport::TransportLoadException& e) { - QMessageBox::warning(widget_, tr("Loading image transport plugin failed"), e.what()); - } - } -} - -void ImageView::onZoom1(bool checked) -{ - if (checked) - { - if (ui_.image_frame->getImage().isNull()) - { - return; - } - ui_.image_frame->setInnerFrameFixedSize(ui_.image_frame->getImage().size()); - widget_->resize(ui_.image_frame->size()); - widget_->setMinimumSize(widget_->sizeHint()); - widget_->setMaximumSize(widget_->sizeHint()); - } else { - ui_.image_frame->setInnerFrameMinimumSize(QSize(80, 60)); - ui_.image_frame->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - widget_->setMinimumSize(QSize(80, 60)); - widget_->setMaximumSize(QSize(QWIDGETSIZE_MAX, QWIDGETSIZE_MAX)); - } -} - -void ImageView::onDynamicRange(bool checked) -{ - ui_.max_range_double_spin_box->setEnabled(!checked); -} - -void ImageView::saveImage() -{ - // take a snapshot before asking for the filename - QImage img = ui_.image_frame->getImageCopy(); - - QString file_name = QFileDialog::getSaveFileName(widget_, tr("Save as image"), "image.png", tr("Image (*.bmp *.jpg *.png *.tiff)")); - if (file_name.isEmpty()) - { - return; - } - - img.save(file_name); -} - -void ImageView::callbackImage(const sensor_msgs::Image::ConstPtr& msg) -{ - try - { - // First let cv_bridge do its magic - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::RGB8); - conversion_mat_ = cv_ptr->image; - } - catch (cv_bridge::Exception& e) - { - try - { - // If we're here, there is no conversion that makes sense, but let's try to imagine a few first - cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(msg); - if (msg->encoding == "CV_8UC3") - { - // assuming it is rgb - conversion_mat_ = cv_ptr->image; - } else if (msg->encoding == "8UC1") { - // convert gray to rgb - cv::cvtColor(cv_ptr->image, conversion_mat_, CV_GRAY2RGB); - } else if (msg->encoding == "16UC1" || msg->encoding == "32FC1") { - // scale / quantify - double min = 0; - double max = ui_.max_range_double_spin_box->value(); - if (msg->encoding == "16UC1") max *= 1000; - if (ui_.dynamic_range_check_box->isChecked()) - { - // dynamically adjust range based on min/max in image - cv::minMaxLoc(cv_ptr->image, &min, &max); - if (min == max) { - // completely homogeneous images are displayed in gray - min = 0; - max = 2; - } - } - cv::Mat img_scaled_8u; - cv::Mat(cv_ptr->image-min).convertTo(img_scaled_8u, CV_8UC1, 255. / (max - min)); - cv::cvtColor(img_scaled_8u, conversion_mat_, CV_GRAY2RGB); - } else { - qWarning("ImageView.callback_image() could not convert image from '%s' to 'rgb8' (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; - } - } - catch (cv_bridge::Exception& e) - { - qWarning("ImageView.callback_image() while trying to convert image from '%s' to 'rgb8' an exception was thrown (%s)", msg->encoding.c_str(), e.what()); - ui_.image_frame->setImage(QImage()); - return; - } - } - - // image must be copied since it uses the conversion_mat_ for storage which is asynchronously overwritten in the next callback invocation - QImage image(conversion_mat_.data, conversion_mat_.cols, conversion_mat_.rows, conversion_mat_.step[0], QImage::Format_RGB888); - ui_.image_frame->setImage(image); - - if (!ui_.zoom_1_push_button->isEnabled()) - { - ui_.zoom_1_push_button->setEnabled(true); - onZoom1(ui_.zoom_1_push_button->isChecked()); - } -} - -} - -PLUGINLIB_EXPORT_CLASS(rqt_image_view::ImageView, rqt_gui_cpp::Plugin) diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui deleted file mode 100644 index bad42ee9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/image_view.ui +++ /dev/null @@ -1,169 +0,0 @@ - - - ImageViewWidget - - - true - - - - 0 - 0 - 400 - 300 - - - - Image View - - - - - - - - QComboBox::AdjustToContents - - - - - - - - - - false - - - true - - - - - - - Dynamic depth range - - - - - - - - - - Max depth - - - m - - - 0.010000000000000 - - - 100.000000000000000 - - - 10.000000000000000 - - - - - - - Save as image - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - 0 - - - - - - 0 - 0 - - - - - 80 - 60 - - - - QFrame::Box - - - 1 - - - - - - - Qt::Horizontal - - - QSizePolicy::Expanding - - - - 0 - 20 - - - - - - - - - - Qt::Vertical - - - - 20 - 0 - - - - - - - - - rqt_image_view::RatioLayoutedFrame - QFrame -
rqt_image_view/ratio_layouted_frame.h
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp b/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp deleted file mode 100644 index bf608613..00000000 --- a/workspace/src/rqt_common_plugins/rqt_image_view/src/rqt_image_view/ratio_layouted_frame.cpp +++ /dev/null @@ -1,162 +0,0 @@ -/* - * Copyright (c) 2011, Dirk Thomas, TU Darmstadt - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the TU Darmstadt nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#include - -#include - -namespace rqt_image_view { - -RatioLayoutedFrame::RatioLayoutedFrame(QWidget* parent, Qt::WFlags flags) - : QFrame() - , aspect_ratio_(4, 3) -{ - connect(this, SIGNAL(delayed_update()), this, SLOT(update()), Qt::QueuedConnection); -} - -RatioLayoutedFrame::~RatioLayoutedFrame() -{ -} - -const QImage& RatioLayoutedFrame::getImage() const -{ - return qimage_; -} - -QImage RatioLayoutedFrame::getImageCopy() const -{ - QImage img; - qimage_mutex_.lock(); - img = qimage_.copy(); - qimage_mutex_.unlock(); - return img; -} - -void RatioLayoutedFrame::setImage(const QImage& image)//, QMutex* image_mutex) -{ - qimage_mutex_.lock(); - qimage_ = image.copy(); - qimage_mutex_.unlock(); - setAspectRatio(qimage_.width(), qimage_.height()); - emit delayed_update(); -} - -void RatioLayoutedFrame::resizeToFitAspectRatio() -{ - QRect rect = contentsRect(); - - // reduce longer edge to aspect ration - double width = double(rect.width()); - double height = double(rect.height()); - if (width * aspect_ratio_.height() / height > aspect_ratio_.width()) - { - // too large width - width = height * aspect_ratio_.width() / aspect_ratio_.height(); - rect.setWidth(int(width + 0.5)); - } - else - { - // too large height - height = width * aspect_ratio_.height() / aspect_ratio_.width(); - rect.setHeight(int(height + 0.5)); - } - - // resize taking the border line into account - int border = lineWidth(); - resize(rect.width() + 2 * border, rect.height() + 2 * border); -} - -void RatioLayoutedFrame::setInnerFrameMinimumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMinimumSize(new_size); - emit delayed_update(); -} - -void RatioLayoutedFrame::setInnerFrameMaximumSize(const QSize& size) -{ - int border = lineWidth(); - QSize new_size = size; - new_size += QSize(2 * border, 2 * border); - setMaximumSize(new_size); - emit delayed_update(); -} - -void RatioLayoutedFrame::setInnerFrameFixedSize(const QSize& size) -{ - setInnerFrameMinimumSize(size); - setInnerFrameMaximumSize(size); -} - -void RatioLayoutedFrame::setAspectRatio(unsigned short width, unsigned short height) -{ - int divisor = greatestCommonDivisor(width, height); - if (divisor != 0) { - aspect_ratio_.setWidth(width / divisor); - aspect_ratio_.setHeight(height / divisor); - } -} - -void RatioLayoutedFrame::paintEvent(QPaintEvent* event) -{ - QPainter painter(this); - qimage_mutex_.lock(); - if (!qimage_.isNull()) - { - resizeToFitAspectRatio(); - // TODO: check if full draw is really necessary - //QPaintEvent* paint_event = dynamic_cast(event); - //painter.drawImage(paint_event->rect(), qimage_); - painter.drawImage(contentsRect(), qimage_); - } else { - // default image with gradient - QLinearGradient gradient(0, 0, frameRect().width(), frameRect().height()); - gradient.setColorAt(0, Qt::white); - gradient.setColorAt(1, Qt::black); - painter.setBrush(gradient); - painter.drawRect(0, 0, frameRect().width() + 1, frameRect().height() + 1); - } - qimage_mutex_.unlock(); -} - -int RatioLayoutedFrame::greatestCommonDivisor(int a, int b) -{ - if (b==0) - { - return a; - } - return greatestCommonDivisor(b, a % b); -} - -} diff --git a/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst deleted file mode 100644 index e2f9dc59..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/CHANGELOG.rst +++ /dev/null @@ -1,75 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_launch -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* mark rqt_launch and rqt_reconfigure as experimental (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* add stop-all-nodes button - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- -* Fix; Start-Stop buttons now reflect nodes status, except when they crashed - -0.2.13 (2013-03-11 22:14) -------------------------- -* Nodes are stop-able now. -* Capable to add node widgets as children of nodes. - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt deleted file mode 100644 index a47cf796..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_launch) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_launch - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/package.xml b/workspace/src/rqt_common_plugins/rqt_launch/package.xml deleted file mode 100644 index 0d73bed3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - rqt_launch - 0.3.13 - This rqt plugin ROS package provides easy view of .launch files. - User can also start and end node by node that are defined in those files. - - - Isaac Saito - BSD - - http://ros.org/wiki/rqt_launch - https://github.com/ros-visualization/rqt_common_plugins/rqt_launch - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Saito - Stuart Glaser - - catkin - rqt_py_common - roslaunch - rospy - rqt_console - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml b/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml deleted file mode 100644 index 9536e0e9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/plugin.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - A Python GUI plugin for for edit launch file, run and stop nodes. - - - - - folder - Plugins related to configuration. - - - mail-message-new - A Python GUI plugin for editing, rnuning and stopping launch files. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui b/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui deleted file mode 100644 index e6e5fffc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/resource/launch_widget.ui +++ /dev/null @@ -1,77 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 761 - 484 - - - - - 300 - 30 - - - - ROS Launch GUI - - - - - - - - - - - - - - S&TART ALL - - - - - - - false - - - STO&P ALL - - - - - - - (Not implemented yet) - - - &Open Console - - - - - - - - - true - - - true - - - true - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui b/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui deleted file mode 100644 index 2dcc29b3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/resource/node_widget.ui +++ /dev/null @@ -1,108 +0,0 @@ - - - NodeWidget - - - - 0 - 0 - 764 - 57 - - - - - 300 - 30 - - - - ROS Launch GUI - - - - - - - - Node name - - - &Status - - - - - - - #spawned so far - - - (0) - - - - - - - Start node - - - - - - - - true - - - - - - - Reload - - - - - - - - true - - - - - - - Pkg name - - - Package name - - - - - - - Executable name - - - Executable - - - - - - - (argument) - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch b/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch deleted file mode 100755 index 55f94f40..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/scripts/rqt_launch +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_launch.launch_plugin.LaunchPlugin')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/setup.py b/workspace/src/rqt_common_plugins/rqt_launch/setup.py deleted file mode 100644 index add6e5ea..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_launch'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/__init__.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py deleted file mode 100644 index 192baafa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_main.py +++ /dev/null @@ -1,154 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import sys - -import rospy -from rqt_launch.launch_widget import LaunchWidget -from rqt_py_common.plugin_container_widget import PluginContainerWidget -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - -import re - - - -def is_int(s): - try: - int(s) - return True - except ValueError: - return False - - -def is_float(s): - try: - float(s) - return True - except ValueError: - return False - - -class LaunchMain(object): - def __init__(self, plugin_context): - super(LaunchMain, self).__init__() - print 'sfdfdsfsdfds' - self._plugin_context = plugin_context - - self._main_launch_widget = LaunchWidget(self) - self._mainwidget = PluginContainerWidget(self._main_launch_widget, - True, False) - - self._run_id = None - self._node_controllers = [] - self._config = None - - #RqtRoscommUtil.load_parameters(self._config, '/rqt_launch') - - def get_widget(self): - return self._mainwidget - - def set_node_controllers(self, node_controllers): - self._node_controllers = node_controllers - - def set_config(self, config): - self._config = config - - def start_all(self): - ''' - Checks nodes that's set (via self.set_node_controllers) one by one and - starts one if each node is not running. - Then disable START ALL button and enable STOP ALL button. - ''' - - pat = re.compile(r'.*=(.*)') - if self._config: - for k in self._config.params.keys(): - s = str(self._config.params[k]) - result = re.match(pat, s) - - if result: - val = result.groups()[0] - - if is_int(val): - rospy.set_param(k, int(val)) - elif is_float(val): - rospy.set_param(k, float(val)) - else: - rospy.set_param(k, val) - - - for n in self._node_controllers: - if not n.is_node_running(): - n.start(restart=False) - - # Disable START ALL button. - self._main_launch_widget._pushbutton_start_all.setEnabled(False) - self._main_launch_widget._pushbutton_stop_all.setEnabled(True) - - def stop_all(self): - ''' - Checks nodes that's set (via self.set_node_controllers) one by one and - stops one if each node is running. - Then enable START ALL button and disable STOP ALL button. - ''' - - for n in self._node_controllers: - if n.is_node_running(): - n.stop() - - # Disable STOP ALL button. - self._main_launch_widget._pushbutton_start_all.setEnabled(True) - self._main_launch_widget._pushbutton_stop_all.setEnabled(False) - - def check_process_statuses(self): - for n in self._node_controllers: - n.check_process_status() - - def shutdown(self): - rospy.logdebug('Launchmain.shutdown') - self.stop_all() - - def save_settings(self, plugin_settings, instance_settings): - self._mainwidget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._mainwidget.restore_settings(plugin_settings, instance_settings) - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_launch')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py deleted file mode 100644 index 5aed4801..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_plugin.py +++ /dev/null @@ -1,64 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from rqt_gui_py.plugin import Plugin - -from rqt_launch.launch_main import LaunchMain - - -class LaunchPlugin(Plugin): - - def __init__(self, context): - ''' - :type context: qt_gui.PluginContext - ''' - - super(LaunchPlugin, self).__init__(context) - - self._main = LaunchMain(context) - - self._widget = self._main.get_widget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._main.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - self._main.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._main.restore_settings(plugin_settings, instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py deleted file mode 100644 index c96be051..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/launch_widget.py +++ /dev/null @@ -1,274 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os -import sys - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import QModelIndex, Signal -from python_qt_binding.QtGui import QDialog, QStandardItem, \ - QStandardItemModel -from rosgraph import rosenv -import roslaunch -from roslaunch.core import RLException -import rospkg -import rospy - -#from rqt_console.console_widget import ConsoleWidget -from rqt_launch.node_proxy import NodeProxy -from rqt_launch.node_controller import NodeController -from rqt_launch.node_delegate import NodeDelegate -from rqt_launch.status_indicator import StatusIndicator -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - - -class LaunchWidget(QDialog): - '''#TODO: comment - ''' - - # To be connected to PluginContainerWidget - sig_sysmsg = Signal(str) - - def __init__(self, parent): - ''' - @type parent: LaunchMain - ''' - super(LaunchWidget, self).__init__() - self._parent = parent - print 'FROM WIDGET' - - self._config = None - - #TODO: should be configurable from gui - self._port_roscore = 11311 - - self._run_id = None - - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path('rqt_launch'), - 'resource', 'launch_widget.ui') - loadUi(ui_file, self) - - # row=0 allows any number of rows to be added. - self._datamodel = QStandardItemModel(0, 1) - - master_uri = rosenv.get_master_uri() - rospy.loginfo('LaunchWidget master_uri={}'.format(master_uri)) - self._delegate = NodeDelegate(master_uri, - self._rospack) - self._treeview.setModel(self._datamodel) - self._treeview.setItemDelegate(self._delegate) - - # NodeController used in controller class for launch operation. - self._node_controllers = [] - - self._pushbutton_start_all.clicked.connect(self._parent.start_all) - self._pushbutton_stop_all.clicked.connect(self._parent.stop_all) - # Bind package selection with .launch file selection. - self._combobox_pkg.currentIndexChanged[str].connect( - self._refresh_launchfiles) - # Bind a launch file selection with launch GUI generation. - self._combobox_launchfile_name.currentIndexChanged[str].connect( - self._load_launchfile_slot) - self._update_pkgs_contain_launchfiles() - - # Used for removing previous nodes - self._num_nodes_previous = 0 - - def _load_launchfile_slot(self, launchfile_name): - # Not yet sure why, but everytime combobox.currentIndexChanged occurs, - # this method gets called twice with launchfile_name=='' in 1st call. - if launchfile_name == None or launchfile_name == '': - return - - _config = None - - rospy.logdebug('_load_launchfile_slot launchfile_name={}'.format( - launchfile_name)) - - try: - _config = self._create_launchconfig(launchfile_name, - self._port_roscore) - #TODO: folder_name_launchfile should be able to specify arbitrarily - # _create_launchconfig takes 3rd arg for it. - - except IndexError as e: - msg = 'IndexError={} launchfile={}'.format(e.message, - launchfile_name) - rospy.logerr(msg) - self.sig_sysmsg.emit(msg) - return - except RLException as e: - msg = 'RLException={} launchfile={}'.format(e.message, - launchfile_name) - rospy.logerr(msg) - self.sig_sysmsg.emit(msg) - return - - self._create_widgets_for_launchfile(_config) - - def _create_launchconfig(self, launchfile_name, port_roscore=11311, - folder_name_launchfile='launch'): - ''' - @rtype: ROSLaunchConfig - @raises RLException: raised by roslaunch.config.load_config_default. - ''' - - pkg_name = self._combobox_pkg.currentText() - - try: - launchfile = os.path.join(self._rospack.get_path(pkg_name), - folder_name_launchfile, launchfile_name) - except IndexError as e: - raise RLException('IndexError: {}'.format(e.message)) - - try: - launch_config = roslaunch.config.load_config_default([launchfile], - port_roscore) - except rospkg.common.ResourceNotFound as e: - raise RLException('ResourceNotFound: {}'.format(e.message)) - except RLException as e: - raise e - - return launch_config - - def _create_widgets_for_launchfile(self, config): - self._config = config - - # Delete old nodes' GUIs. - self._node_controllers = [] - - # These lines seem to remove indexWidgets previously set on treeview. - # Per suggestion in API doc, we are not using reset(). Instead, - # using 2 methods below without any operation in between, which - # I suspect might be inproper. - # http://qt-project.org/doc/qt-4.8/qabstractitemmodel.html#reset - self._datamodel.beginResetModel() - self._datamodel.endResetModel() - - # Need to store the num of nodes outside of the loop -- this will - # be used for removing excessive previous node rows. - order_xmlelement = 0 - # Loop per xml element - for order_xmlelement, node in enumerate(self._config.nodes): - proxy = NodeProxy(self._run_id, self._config.master.uri, node) - - # TODO: consider using QIcon.fromTheme() - status_label = StatusIndicator() - - qindex_nodewidget = self._datamodel.index(order_xmlelement, - 0, QModelIndex()) - node_widget = self._delegate.create_node_widget(qindex_nodewidget, - proxy.config, - status_label) - - #TODO: Ideally find a way so that we don't need this block. - #BEGIN If these lines are missing, widget won't be shown either. - std_item = QStandardItem( - #node_widget.get_node_name() - ) - self._datamodel.setItem(order_xmlelement, 0, std_item) - #END If these lines are missing, widget won't be shown either. - - self._treeview.setIndexWidget(qindex_nodewidget, node_widget) - - node_controller = NodeController(proxy, node_widget) - self._node_controllers.append(node_controller) - - node_widget.connect_start_stop_button( \ - node_controller.start_stop_slot) - rospy.logdebug('loop #%d proxy.config.namespace=%s ' + - 'proxy.config.name=%s', - order_xmlelement, proxy.config.namespace, - proxy.config.name) - - self._num_nodes_previous = order_xmlelement - - self._parent.set_node_controllers(self._node_controllers) - self._parent.set_config(config) - - def _update_pkgs_contain_launchfiles(self): - ''' - Inspired by rqt_msg.MessageWidget._update_pkgs_contain_launchfiles - ''' - packages = sorted([pkg_tuple[0] - for pkg_tuple - in RqtRoscommUtil.iterate_packages('launch')]) - self._package_list = packages - rospy.logdebug('pkgs={}'.format(self._package_list)) - self._combobox_pkg.clear() - self._combobox_pkg.addItems(self._package_list) - self._combobox_pkg.setCurrentIndex(0) - - def _refresh_launchfiles(self, package=None): - ''' - Inspired by rqt_msg.MessageWidget._refresh_msgs - ''' - if package is None or len(package) == 0: - return - self._launchfile_instances = [] # Launch[] - #TODO: RqtRoscommUtil.list_files's 2nd arg 'subdir' should NOT be - # hardcoded later. - _launch_instance_list = RqtRoscommUtil.list_files(package, - 'launch') - - rospy.logdebug('_refresh_launches package={} instance_list={}'.format( - package, - _launch_instance_list)) - - self._launchfile_instances = [x.split('/')[1] - for x in _launch_instance_list] - - self._combobox_launchfile_name.clear() - self._combobox_launchfile_name.addItems(self._launchfile_instances) - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value('splitter', self._splitter.saveState()) - pass - - def restore_settings(self, plugin_settings, instance_settings): -# if instance_settings.contains('splitter'): -# self._splitter.restoreState(instance_settings.value('splitter')) -# else: -# self._splitter.setSizes([100, 100, 200]) - pass - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_launch')) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py deleted file mode 100644 index 241c3cd4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/name_surrogate.py +++ /dev/null @@ -1,61 +0,0 @@ -#! /usr/bin/env python - - -class NamesSurrogate(object): - ''' - Because some functions in roslib.names cannot be referred in the original - rxlaunch code, the codes of those function are copied here. This class - should not be used for any other purpose than to be used within this .py - file. - - :author: Isaac Saito - ''' - - PRIV_NAME = '~' - SEP = '/' - - @staticmethod - def is_global(name): - ''' - Test if name is a global graph resource name. 116 117 - @param name: must be a legal name in canonical form 118 - @type name: str 119 - @return: True if name is a globally referenced name (i.e. /ns/name) 120 - @rtype: bool - ''' - return name and name[0] == NamesSurrogate.SEP - - @staticmethod - def is_private(name): - ''' 126 Test if name is a private graph resource name. 127 128 - @param name: must be a legal name in canonical form 129 - @type name: str 130 @return bool: True if name is a privately - referenced name (i.e. ~name) 131 ''' - return name and name[0] == NamesSurrogate.PRIV_NAME - - @staticmethod - def ns_join(ns, name): - ''' - Taken from - http://ros.org/rosdoclite/groovy/api/roslib/html/python/roslib.names-pysrc.html#ns_join - since roslib.names is not found for some reason, and also the entire - module seems deprecated. - - Join a namespace and name. If name is unjoinable (i.e. ~private or - 162 /global) it will be returned without joining 163 164 - @param ns: namespace ('/' and '~' are both legal). If ns is the empty - string, name will be returned. 165 - @type ns: str 166 - @param name str: a legal name 167 - @return str: name concatenated to ns, or name if it's 168 unjoinable. 169 - @rtype: str 170 - ''' - if NamesSurrogate.is_private(name) or NamesSurrogate.is_global(name): - return name - if ns == NamesSurrogate.PRIV_NAME: - return NamesSurrogate.PRIV_NAME + name - if not ns: - return name - if ns[-1] == NamesSurrogate.SEP: - return ns + name - return ns + NamesSurrogate.SEP + name diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py deleted file mode 100644 index fbd860b4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_controller.py +++ /dev/null @@ -1,116 +0,0 @@ -#! /usr/bin/env python - -import rospy - - -# Provides callback functions for the start and stop buttons -class NodeController(object): - ''' - Containing both proxy and gui instances, this class gives a control of - a node on both ROS & GUI sides. - ''' - - # these values need to synch with member variables. - # Eg. self.gui isn't legal. - __slots__ = ['_proxy', '_gui'] - - def __init__(self, proxy, gui): - ''' - @type proxy: rqt_launch.NodeProxy - @type gui: QWidget - ''' - self._proxy = proxy - - self._gui = gui - self._gui.set_node_controller(self) - - def start_stop_slot(self, signal): - ''' - Works as a slot particularly intended to work for - QAbstractButton::toggled(checked). Internally calls - NodeController.start / stop depending on `signal`. - - @type signal: bool - ''' - if self._proxy.is_running(): - self.stop() - rospy.logdebug('---start_stop_slot stOP') - else: - self.start() - rospy.logdebug('==start_stop_slot StART') - - def start(self, restart=True): - ''' - Start a ROS node as a new _process. - ''' - rospy.logdebug('Controller.start restart={}'.format(restart)) - - # Should be almost unreachable under current design where this 'start' - # method only gets called when _proxy.is_running() returns false. - - if self._proxy.is_running(): - if not restart: - # If the node is already running and restart isn't desired, - # do nothing further. - return - #TODO: Need to consider...is stopping node here - # (i.e. in 'start' method) good? - self.stop() - - # If the launch_prefix has changed, then the _process must be recreated - if (self._proxy.config.launch_prefix != - self._gui._lineEdit_launch_args.text()): - - self._proxy.config.launch_prefix = \ - self._gui._lineEdit_launch_args.text() - self._proxy.recreate_process() - - self._gui.set_node_started(False) - self._gui.label_status.set_starting() - self._proxy.start_process() - self._gui.label_status.set_running() - self._gui.label_spawncount.setText("({})".format( - self._proxy.get_spawn_count())) - - def stop(self): - ''' - Stop a ROS node's _process. - ''' - - #TODO: Need to check if the node is really running. - - #if self._proxy.is_running(): - self._gui.set_node_started(True) - self._gui.label_status.set_stopping() - self._proxy.stop_process() - self._gui.label_status.set_stopped() - - def check_process_status(self): - if self._proxy.has_died(): - rospy.logerr("Process died: {}".format( - self._proxy.get_proc_name())) - self._proxy.stop_process() - self._gui.set_node_started(True) - if self._proxy._process.exit_code == 0: - self._gui.label_status.set_stopped() - else: - self._gui.label_status.set_died() - - # Checks if it should be respawned - if self._gui.respawn_toggle.isChecked(): - rospy.loginfo("Respawning _process: {}".format( - self._proxy._process.name)) - self._gui.label_status.set_starting() - self._proxy.start_process() - self._gui.label_status.set_running() - self._gui.label_spawncount.setText("({})".format( - self._proxy._process.spawn_count)) - - def get_node_widget(self): - ''' - @rtype: QWidget - ''' - return self._gui - - def is_node_running(self): - return self._proxy.is_running() diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py deleted file mode 100644 index 3e548efc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_delegate.py +++ /dev/null @@ -1,93 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QStyledItemDelegate -import rospkg - -from rqt_launch.node_widget import NodeWidget - - -class NodeDelegate(QStyledItemDelegate): - __slots__ = ['status_label', 'respawn_toggle', 'spawn_count_label', - 'launch_prefix_edit'] - - def __init__(self, master_uri, rospack=None): - #respawn_toggle, spawn_count_label, launch_prefix_edit): - super(NodeDelegate, self).__init__() - self._master_uri = master_uri - - self._rospack = rospack - if rospack == None: - self._rospack = rospkg.RosPack() - - self._nodewidget_dict = {} # { QModelIndex : QWidget } - - def createEditor(self, parent, option, index): - '''Overridden''' - - nodewidget = self._nodewidget_dict[index] - #TODO: handle exception - return nodewidget - - def setEditorData(self, spinBox, index): - '''Overridden''' - value = index.model().data(index, Qt.EditRole) - - spinBox.setValue(value) - - def setModelData(self, spinBox, model, index): - '''Overridden''' - spinBox.interpretText() - value = spinBox.value() - - model.setData(index, value, Qt.EditRole) - - def updateEditorGeometry(self, editor, option, index): - '''Overridden''' - editor.setGeometry(option.rect) - - def create_node_widget(self, qindex, launch_config, - status_label): - ''' - @type status_label: StatusIndicator - ''' - nodewidget = NodeWidget(self._rospack, - self._master_uri, launch_config, - status_label) - self._nodewidget_dict[qindex] = nodewidget - return nodewidget - - def get_node_widget(self): - return self._node_widget diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py deleted file mode 100644 index 91d92a37..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_proxy.py +++ /dev/null @@ -1,125 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import time -import threading - -from roslaunch import node_args, nodeprocess -import rospy - - -class Polling(threading.Thread): - def __init__(self, parent): - super(Polling, self).__init__() - self._parent = parent - - def run(self): - while True: - rospy.logdebug('Proc={} Died? {}'.format( - self._parent.get_proc_name(), - self._parent.has_died())) - time.sleep(1.0) - - -class NodeProxy(object): - ''' - Works as a proxy between ROS Node - (more in particular, roslaunch.nodeprocess) & GUI. - ''' - - __slots__ = ['_run_id', 'master_uri', 'config', '_process'] - - def __init__(self, run_id, master_uri, config): - self._run_id = run_id - self.master_uri = master_uri - self.config = config - - # @type: roslaunch.nodeprocess.LocalProcess - self._process = self.recreate_process() - - # LocalProcess.is_alive() does not do what you would expect - def is_running(self): - rospy.logdebug('BEFORE started={}, stopped={} alive={}'.format( - self._process.started, - self._process.stopped, - self._process.is_alive())) - return self._process.started and not self._process.stopped - #and self._process.is_alive() - # is_alive() returns False once nodeprocess was stopped. - - def has_died(self): - rospy.logdebug('Proc={} started={}, stopped={}, is_alive={}'.format( - self.get_proc_name(), self._process.started, self._process.stopped, - self._process.is_alive())) - - return (self._process.started and not self._process.stopped - and not self._process.is_alive()) - - def recreate_process(self): - ''' - Create and set roslaunch.nodeprocess.LocalProcess to member variable. - @rtype: roslaunch.nodeprocess.LocalProcess - ''' - _local_process = nodeprocess.LocalProcess - try: - _local_process = nodeprocess.create_node_process( - self._run_id, self.config, self.master_uri) - except node_args.NodeParamsException as e: - rospy.logerr('roslaunch failed to load the node. {}'.format( - e.message)) - #TODO: Show msg on GUI that the node wasn't loaded. - - return _local_process - - def start_process(self): - #TODO: add null exception check for _process - self._process.start() - - def stop_process(self): - #TODO: add null exception check for _process - - try: - self._process.stop() - except Exception as e: - #TODO: Change roslaunch to raise exception - rospy.logdebug('Proxy stop_process exception={}'.format(e.message)) - - def get_spawn_count(self): - return self._process.spawn_count - - def get_proc_name(self): - return self._process.name - - def get_proc_exit_code(self): - return self._process.exit_code diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py deleted file mode 100644 index c6ddcbf1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/node_widget.py +++ /dev/null @@ -1,119 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QIcon, QLineEdit, QWidget -import rospy - -from rqt_launch.name_surrogate import NamesSurrogate - - -class NodeWidget(QWidget): - ''' - Works as a proxy between ROS Node - (more in particular, roslaunch.nodeprocess) and GUI. - ''' - - __slots__ = ['_run_id', 'master_uri', 'config', '_process'] - - def __init__(self, rospack, master_uri, launch_config, - label_status): - ''' - @type launch_node: roslaunch.core.Node - @type launch_config: roslaunch.core.Config - @type label_status: StatusIndicator - ''' - super(NodeWidget, self).__init__() - self._rospack = rospack - self._master_uri = master_uri - self._launch_config = launch_config - - ui_file = os.path.join(self._rospack.get_path('rqt_launch'), - 'resource', 'node_widget.ui') - loadUi(ui_file, self) - - self.label_status = label_status # Public - # stop_button = QPushButton(self.style().standardIcon( - # QStyle.SP_MediaStop), "") - self._respawn_toggle.setChecked(self._launch_config.respawn) - self._lineEdit_launch_args = QLineEdit( - self._launch_config.launch_prefix) - - rospy.logdebug('_proxy.conf.namespace={} launch_config={}'.format( - self._launch_config.namespace, self._launch_config.name)) - self._resolved_node_name = NamesSurrogate.ns_join( - self._launch_config.namespace, self._launch_config.name) - self._label_nodename.setText(self._get_node_name()) - self._label_pkg_name.setText(self._launch_config.package) - self._label_name_executable.setText(self._launch_config.type) - - self._icon_node_start = QIcon.fromTheme('media-playback-start') - self._icon_node_stop = QIcon.fromTheme('media-playback-stop') - self._icon_respawn_toggle = QIcon.fromTheme('view-refresh') - - self._pushbutton_start_stop_node.setIcon(self._icon_node_start) - self._respawn_toggle.setIcon(self._icon_respawn_toggle) - - self._node_controller = None # Connected in self.set_node_controller - - def _get_node_name(self): - return self._resolved_node_name - - def connect_start_stop_button(self, slot): - self._pushbutton_start_stop_node.toggled.connect(slot) - - def set_node_started(self, is_started=True): - # If the button is not down yet - is_node_running = self._node_controller.is_node_running() - rospy.logdebug('NodeWidget.set_node_started running?={}'.format( - is_node_running)) - #if is_node_running: - if is_started: - #and self._pushbutton_start_stop_node.isDown(): - self._pushbutton_start_stop_node.setIcon(self._icon_node_start) - self._pushbutton_start_stop_node.setDown(False) - - #elif not is_node_running: # To START the node - else: - #and not self._pushbutton_start_stop_node.isDown(): - self._pushbutton_start_stop_node.setIcon(self._icon_node_stop) - self._pushbutton_start_stop_node.setDown(True) - - def set_node_controller(self, node_controller): - #TODO: Null check - self._node_controller = node_controller - #self._pushbutton_start_stop_node.pressed.connect( - # self._node_controller.start_stop_slot) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py b/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py deleted file mode 100644 index 2421e2be..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/src/rqt_launch/status_indicator.py +++ /dev/null @@ -1,40 +0,0 @@ -#! /usr/bin/env python - -from python_qt_binding.QtGui import QLabel, QStyle -import rospy - - -class StatusIndicator(QLabel): - def __init__(self, *args): - super(StatusIndicator, self).__init__(*args) - self.set_stopped() - - def set_running(self): - self.setPixmap( - self.style().standardIcon(QStyle.SP_DialogApplyButton).pixmap(16)) - - def set_starting(self): - rospy.logdebug('StatusIndicator.set_starting') - self.setPixmap(self.style().standardIcon( - QStyle.SP_DialogResetButton).pixmap(16)) - - def set_stopping(self): - """ - Show msg that the process is "stopping". - - cf. set_stopped() - """ - self.setPixmap(self.style().standardIcon( - QStyle.SP_DialogResetButton).pixmap(16)) - - def set_stopped(self): - """ - Show msg that the process is "stopped". - - cf. set_stopping() - """ - self.setText(" ") - - def set_died(self): - self.setPixmap(self.style().standardIcon( - QStyle.SP_MessageBoxCritical).pixmap(16)) diff --git a/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch b/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch deleted file mode 100644 index 0ebae4b2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_launch/test/test.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst deleted file mode 100644 index de15ff5d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_logger_level -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt deleted file mode 100644 index 0ed180dd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_logger_level) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_logger_level - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox deleted file mode 100644 index 33481e79..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_logger_level provides a GUI plugin for configuring the logger level of ROS nodes. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml b/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml deleted file mode 100644 index 8c6662dc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - rqt_logger_level - 0.3.13 - rqt_logger_level provides a GUI plugin for configuring the logger level of ROS nodes.
-
- rqt_logger_level takes over `wx`-based tool [[rxloggerlevel]]. - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_logger_level - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - rosnode - rosservice - rospy - rqt_gui - rqt_gui_py - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml b/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml deleted file mode 100644 index 7bfb4045..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin configuring the logger level of ROS nodes. - - - - - folder - Plugins related to logging. - - - format-indent-more - A Python GUI plugin for configuring the level of ROS loggers. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui b/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui deleted file mode 100644 index b49274cd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/resource/logger_level.ui +++ /dev/null @@ -1,74 +0,0 @@ - - - LoggerLevel - - - - 0 - 0 - 597 - 347 - - - - Logger Level - - - - - - - - - - Nodes - - - - - - - - - - Refresh - - - - - - - - - - - Loggers - - - - - - - - - - - - - - Levels - - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level b/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level deleted file mode 100755 index e167c745..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/scripts/rqt_logger_level +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_logger_level.logger_level.LoggerLevel')) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py b/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py deleted file mode 100644 index 7e671f79..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_logger_level'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_logger_level'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/__init__.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py deleted file mode 100644 index bb821cf8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level.py +++ /dev/null @@ -1,70 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from .logger_level_widget import LoggerLevelWidget -from .logger_level_service_caller import LoggerLevelServiceCaller - - -class LoggerLevel(Plugin): - """ - rqt_logger_level plugin's main class. Creates a widget and a - service caller object and displays the widget. - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(LoggerLevel, self).__init__(context) - self.setObjectName('LoggerLevel') - self._service_caller = LoggerLevelServiceCaller() - self._widget = LoggerLevelWidget(self._service_caller) - - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - pass - - def save_settings(self, plugin_settings, instance_settings): - # TODO: implement save functionality for the current logger states - pass - - def restore_settings(self, plugin_settings, instance_settings): - # TODO: implement restore functionality for the current logger states - pass - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure it - # Usually used to open a dialog to offer the user a set of configuration diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py deleted file mode 100644 index 405e33ff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_service_caller.py +++ /dev/null @@ -1,126 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosnode -import rospy -import rosservice - -from python_qt_binding.QtCore import QObject, qWarning - - -class LoggerLevelServiceCaller(QObject): - """ - Handles service calls for getting lists of nodes and loggers - Also handles sending requests to change logger levels - """ - def __init__(self): - super(LoggerLevelServiceCaller, self).__init__() - - def get_levels(self): - return [self.tr('Debug'), self.tr('Info'), self.tr('Warn'), self.tr('Error'), self.tr('Fatal')] - - def get_loggers(self, node): - if self._refresh_loggers(node): - return self._current_loggers - else: - return [] - - def get_node_names(self): - """ - Gets a list of available services via a ros service call. - :returns: a list of all nodes that provide the set_logger_level service, ''list(str)'' - """ - set_logger_level_nodes = [] - nodes = rosnode.get_node_names() - for name in sorted(nodes): - for service in rosservice.get_service_list(name): - if service == name + '/set_logger_level': - set_logger_level_nodes.append(name) - return set_logger_level_nodes - - def _refresh_loggers(self, node): - """ - Stores a list of loggers available for passed in node - :param node: name of the node to query, ''str'' - """ - self._current_loggers = [] - self._current_levels = {} - servicename = node + '/get_loggers' - try: - service = rosservice.get_service_class_by_name(servicename) - except rosservice.ROSServiceIOException as e: - qWarning('During get_service_class_by_name "%s":\n%s' % (servicename, e)) - return False - request = service._request_class() - proxy = rospy.ServiceProxy(str(servicename), service) - try: - response = proxy(request) - except rospy.ServiceException as e: - qWarning('SetupDialog.get_loggers(): request:\n%s' % (type(request))) - qWarning('SetupDialog.get_loggers() "%s":\n%s' % (servicename, e)) - return False - - if response._slot_types[0] == 'roscpp/Logger[]': - for logger in getattr(response, response.__slots__[0]): - self._current_loggers.append(getattr(logger, 'name')) - self._current_levels[getattr(logger, 'name')] = getattr(logger, 'level') - else: - qWarning(repr(response)) - return False - return True - - def send_logger_change_message(self, node, logger, level): - """ - Sends a logger level change request to 'node'. - :param node: name of the node to chaange, ''str'' - :param logger: name of the logger to change, ''str'' - :param level: name of the level to change, ''str'' - :returns: True if the response is valid, ''bool'' - :returns: False if the request raises an exception or would not change the cached state, ''bool'' - """ - servicename = node + '/set_logger_level' - if self._current_levels[logger].lower() == level.lower(): - return False - - service = rosservice.get_service_class_by_name(servicename) - request = service._request_class() - setattr(request, 'logger', logger) - setattr(request, 'level', level) - proxy = rospy.ServiceProxy(str(servicename), service) - try: - proxy(request) - self._current_levels[logger] = level.upper() - except rospy.ServiceException as e: - qWarning('SetupDialog.level_changed(): request:\n%r' % (request)) - qWarning('SetupDialog.level_changed() "%s":\n%s' % (servicename, e)) - return False - return True diff --git a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py b/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py deleted file mode 100644 index ac71f16e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_logger_level/src/rqt_logger_level/logger_level_widget.py +++ /dev/null @@ -1,126 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import qWarning -from python_qt_binding.QtGui import QWidget - - -class LoggerLevelWidget(QWidget): - """ - Widget for use with LoggerLevelServiceCaller class to alter the ROS logger levels - """ - def __init__(self, caller): - """ - :param caller: service caller instance for sending service calls, ''LoggerLevelServiceCaller'' - """ - super(LoggerLevelWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_logger_level'), 'resource', 'logger_level.ui') - loadUi(ui_file, self) - self.setObjectName('LoggerLevelWidget') - self._caller = caller - - self.node_list.currentRowChanged[int].connect(self.node_changed) - self.logger_list.currentRowChanged[int].connect(self.logger_changed) - self.level_list.currentRowChanged[int].connect(self.level_changed) - self.refresh_button.clicked[bool].connect(self.refresh_nodes) - - self.refresh_nodes() - if self.node_list.count() > 0: - self.node_list.setCurrentRow(0) - - def refresh_nodes(self): - """ - Refreshes the top level node list and repoulates the node_list widget. - As a side effect the level and logger lists are cleared - """ - self.level_list.clear() - self.logger_list.clear() - self.node_list.clear() - for name in self._caller.get_node_names(): - self.node_list.addItem(name) - - def node_changed(self, row): - """ - Handles the rowchanged event for the node_list widget - Populates logger_list with the loggers for the node selected - :param row: the selected row in node_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.node_list.count(): - qWarning('Node row %s out of bounds. Current count: %s' % (row, self.node_list.count())) - return - self.logger_list.clear() - self.level_list.clear() - loggers = self._caller.get_loggers(self.node_list.item(row).text()) - if len(loggers) == 0: - return - for logger in sorted(loggers): - self.logger_list.addItem(logger) - if self.logger_list.count() != 0: - self.logger_list.setCurrentRow(0) - - def logger_changed(self, row): - """ - Handles the rowchanged event for the logger_list widget - Populates level_list with the levels for the logger selected - :param row: the selected row in logger_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.logger_list.count(): - qWarning('Logger row %s out of bounds. Current count: %s' % (row, self.logger_list.count())) - return - if self.level_list.count() == 0: - for level in self._caller.get_levels(): - self.level_list.addItem(level) - for index in range(self.level_list.count()): - if self.level_list.item(index).text().lower() == self._caller._current_levels[self.logger_list.currentItem().text()].lower(): - self.level_list.setCurrentRow(index) - - def level_changed(self, row): - """ - Handles the rowchanged event for the level_list widget - Makes a service call to change the logger level for the indicated node/logger to the selected value - :param row: the selected row in level_list, ''int'' - """ - if row == -1: - return - if row < 0 or row >= self.level_list.count(): - qWarning('Level row %s out of bounds. Current count: %s' % (row, self.level_list.count())) - return - self._caller.send_logger_change_message(self.node_list.currentItem().text(), self.logger_list.currentItem().text(), self.level_list.item(row).text()) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst deleted file mode 100644 index 623d61db..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/CHANGELOG.rst +++ /dev/null @@ -1,107 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_msg -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix right click view (`#317 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Fix; IndexError: list index out of range (`#26 `_) -* A common module (rqt_msg.MessageWidget) optimized so that other pkgs that provide similar functionality (eg. rqt_srv, rqt_action) can use it - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt deleted file mode 100644 index e40e30e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_msg) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_msg - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox deleted file mode 100644 index 5fe1b0f3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_msg provides a GUI plugin for introspecting available ROS message types. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_msg/package.xml b/workspace/src/rqt_common_plugins/rqt_msg/package.xml deleted file mode 100644 index c8a745ba..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/package.xml +++ /dev/null @@ -1,33 +0,0 @@ - - rqt_msg - 0.3.13 - A Python GUI plugin for introspecting available ROS message types. - Note that the msgs available through this plugin is the ones that are stored - on your machine, not on the ROS core your rqt instance connects to. - - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_msg - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - roslib - rosmsg - rospy - rqt_gui - rqt_gui_py - rqt_py_common - rqt_console - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml b/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml deleted file mode 100644 index 5ac26514..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for browsing available ROS message types. - - - - - folder - Plugins related to ROS topics. - - - mail-read - A Python GUI plugin for browsing available ROS message types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui b/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui deleted file mode 100644 index fe06ab16..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/resource/messages.ui +++ /dev/null @@ -1,124 +0,0 @@ - - - MessagesWidget - - - - 0 - 0 - 916 - 561 - - - - Message Type Browser - - - - 0 - - - - - 0 - - - - - 2 - - - - - Message: - - - - - - - - 250 - 0 - - - - Package - - - QComboBox::InsertAlphabetically - - - - - - - / - - - - - - - - 250 - 0 - - - - Message Name - - - - - - - Add Currently Selected Message - - - - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - - - Select a message above and click add to see it in this viewing area. - - - - - - - - - - MessagesTreeView - QTreeView -
rqt_msg.messages_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg b/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg deleted file mode 100755 index 682a731b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/scripts/rqt_msg +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_msg.messages.Messages')) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/setup.py b/workspace/src/rqt_common_plugins/rqt_msg/setup.py deleted file mode 100644 index 9bd3ce73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_msg'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/__init__.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py deleted file mode 100644 index fb37cf3e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages.py +++ /dev/null @@ -1,56 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin -from .messages_widget import MessagesWidget - - -class Messages(Plugin): - def __init__(self, context): - super(Messages, self).__init__(context) - self.setObjectName('Messages') - self._widget = MessagesWidget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py deleted file mode 100644 index a416c3a7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_model.py +++ /dev/null @@ -1,42 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from rqt_py_common.message_tree_model import MessageTreeModel - - -class MessagesTreeModel(MessageTreeModel): - def __init__(self, parent=None): - super(MessagesTreeModel, self).__init__() - self.setHorizontalHeaderLabels([self.tr('Tree'), - self.tr('Type'), - self.tr('Path')]) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py deleted file mode 100644 index f5a99e8c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_tree_view.py +++ /dev/null @@ -1,47 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -from rqt_py_common.message_tree_widget import MessageTreeWidget -from .messages_tree_model import MessagesTreeModel - - -class MessagesTreeView(MessageTreeWidget): - def __init__(self, parent=None): - super(MessagesTreeView, self).__init__() - self.setModel(MessagesTreeModel(self)) - - def _recursive_set_editable(self, item, editable=True): - item.setEditable(editable) - for row in range(item.rowCount()): - for col in range(item.columnCount()): - self._recursive_set_editable(item.child(row, col), editable) diff --git a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py b/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py deleted file mode 100644 index ff53fa58..00000000 --- a/workspace/src/rqt_common_plugins/rqt_msg/src/rqt_msg/messages_widget.py +++ /dev/null @@ -1,216 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import (QAction, QIcon, QMenu, QMessageBox, - QTreeView, QWidget) -import roslib -import rosmsg -import rospkg -import rospy - - -from .messages_tree_view import MessagesTreeView -from rqt_py_common import rosaction -from rqt_console.text_browse_dialog import TextBrowseDialog - - -class MessagesWidget(QWidget): - """ - This class is intended to be able to handle msg, srv & action (actionlib). - The name of the class is kept to use message, by following the habit of - rosmsg (a script that can handle both msg & srv). - """ - def __init__(self, mode=rosmsg.MODE_MSG, - pkg_name='rqt_msg', - ui_filename='messages.ui'): - """ - :param ui_filename: This Qt-based .ui file must have elements that are - referred from this class. Otherwise unexpected - errors are likely to happen. Best way to avoid that - situation when you want to give your own .ui file - is to implement all Qt components in - rqt_msg/resource/message.ui file. - """ - - super(MessagesWidget, self).__init__() - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path(pkg_name), 'resource', ui_filename) - loadUi(ui_file, self, {'MessagesTreeView': MessagesTreeView}) - self.setObjectName(ui_filename) - self._mode = mode - - self._add_button.setIcon(QIcon.fromTheme('list-add')) - self._add_button.clicked.connect(self._add_message) - self._refresh_packages(mode) - self._refresh_msgs(self._package_combo.itemText(0)) - self._package_combo.currentIndexChanged[str].connect(self._refresh_msgs) - self._messages_tree.mousePressEvent = self._handle_mouse_press - - self._browsers = [] - - def _refresh_packages(self, mode=rosmsg.MODE_MSG): - if (self._mode == rosmsg.MODE_MSG) or self._mode == rosmsg.MODE_SRV: - packages = sorted([pkg_tuple[0] for pkg_tuple in - rosmsg.iterate_packages(self._rospack, self._mode)]) - elif self._mode == rosaction.MODE_ACTION: - packages = sorted([pkg_tuple[0] - for pkg_tuple in rosaction.iterate_packages( - self._rospack, self._mode)]) - self._package_list = packages - rospy.logdebug('pkgs={}'.format(self._package_list)) - self._package_combo.clear() - self._package_combo.addItems(self._package_list) - self._package_combo.setCurrentIndex(0) - - def _refresh_msgs(self, package=None): - if package is None or len(package) == 0: - return - self._msgs = [] - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_list = rosmsg.list_msgs(package) - elif self._mode == rosmsg.MODE_SRV: - msg_list = rosmsg.list_srvs(package) - - rospy.logdebug('_refresh_msgs package={} msg_list={}'.format(package, - msg_list)) - for msg in msg_list: - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_class = roslib.message.get_message_class(msg) - elif self._mode == rosmsg.MODE_SRV: - msg_class = roslib.message.get_service_class(msg) - - rospy.logdebug('_refresh_msgs msg_class={}'.format(msg_class)) - - if msg_class is not None: - self._msgs.append(msg) - - self._msgs = [x.split('/')[1] for x in self._msgs] - - self._msgs_combo.clear() - self._msgs_combo.addItems(self._msgs) - - def _add_message(self): - if self._msgs_combo.count() == 0: - return - msg = (self._package_combo.currentText() + - '/' + self._msgs_combo.currentText()) - - rospy.logdebug('_add_message msg={}'.format(msg)) - - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - msg_class = roslib.message.get_message_class(msg)() - if self._mode == rosmsg.MODE_MSG: - text_tree_root = 'Msg Root' - elif self._mode == rosaction.MODE_ACTION: - text_tree_root = 'Action Root' - self._messages_tree.model().add_message(msg_class, - self.tr(text_tree_root), msg, msg) - - elif self._mode == rosmsg.MODE_SRV: - msg_class = roslib.message.get_service_class(msg)() - self._messages_tree.model().add_message(msg_class._request_class, - self.tr('Service Request'), - msg, msg) - self._messages_tree.model().add_message(msg_class._response_class, - self.tr('Service Response'), - msg, msg) - self._messages_tree._recursive_set_editable( - self._messages_tree.model().invisibleRootItem(), False) - - def _handle_mouse_press(self, event, - old_pressEvent=QTreeView.mousePressEvent): - if (event.buttons() & Qt.RightButton and - event.modifiers() == Qt.NoModifier): - self._rightclick_menu(event) - event.accept() - return old_pressEvent(self._messages_tree, event) - - def _rightclick_menu(self, event): - """ - :type event: QEvent - """ - - # QTreeview.selectedIndexes() returns 0 when no node is selected. - # This can happen when after booting no left-click has been made yet - # (ie. looks like right-click doesn't count). These lines are the - # workaround for that problem. - selected = self._messages_tree.selectedIndexes() - if len(selected) == 0: - return - - menu = QMenu() - text_action = QAction(self.tr('View Text'), menu) - menu.addAction(text_action) - raw_action = QAction(self.tr('View Raw'), menu) - menu.addAction(raw_action) - - action = menu.exec_(event.globalPos()) - - if action == raw_action or action == text_action: - rospy.logdebug('_rightclick_menu selected={}'.format(selected)) - selected_type = selected[1].data() - - if selected_type[-2:] == '[]': - selected_type = selected_type[:-2] - browsetext = None - try: - if (self._mode == rosmsg.MODE_MSG or - self._mode == rosaction.MODE_ACTION): - browsetext = rosmsg.get_msg_text(selected_type, - action == raw_action) - elif self._mode == rosmsg.MODE_SRV: - browsetext = rosmsg.get_srv_text(selected_type, - action == raw_action) - - else: - raise - except rosmsg.ROSMsgException, e: - QMessageBox.warning(self, self.tr('Warning'), - self.tr('The selected item component ' + - 'does not have text to view.')) - if browsetext is not None: - self._browsers.append(TextBrowseDialog(browsetext, - self._rospack)) - self._browsers[-1].show() - else: - return - - def cleanup_browsers_on_close(self): - for browser in self._browsers: - browser.close() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst deleted file mode 100644 index 826e6b5f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/CHANGELOG.rst +++ /dev/null @@ -1,125 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_plot -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* Added missing include -* use proper icon names for add/remove -* Contributors: Jochen Sprickerhof, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* save and restore axes settings (`#234 `_) -* remove warning when backend is not found (`#301 `_) -* fix version clash for matplot backend when PyQt5 is installed (`#299 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- -* fix handling of variable-sized arrays (`#261 `_) - -0.3.8 (2014-07-15) ------------------- -* fix missing installation of Python subpackage - -0.3.7 (2014-07-11) ------------------- -* fix missing import (`#248 `_) -* significant improvements and unification of different plot backends (`#239 `_, `#231 `_) -* make more things plottable including arrays and simple message types (`#246 `_) -* make DataPlot a proxy for its plot widget, redraw after loading new data, add clear_values (`#236 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* subscribe to any known topic, even if currently not available (`#233 `_) - -0.3.5 (2014-05-07) ------------------- -* change minimum padding to enable viewing arbitrarily small values (`#223 `_) -* redraw plot only on new data to reduce cpu load, especially with matplot (`#219 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* add checkbox to toggle automatic scrolling of plot with data -* add simple legend for pyqtgraph backend - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix waiting on unpublished topics (`#110 `_) -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- -* command line arguments enabled - -0.2.7 (2012-12-24) ------------------- -* update mat plot, remove usage of collections and numpy, calculate y range once when adding data instead of on draw (`ros-visualization/rqt#48 `_) -* automatically adjust margins for matplot on resize - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt deleted file mode 100644 index b23f3048..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_plot) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_plot - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox deleted file mode 100644 index 7130ce61..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_plot provides a GUI plugin for visualizing numeric values in a 2D plot using PyQwt5. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_plot/package.xml b/workspace/src/rqt_common_plugins/rqt_plot/package.xml deleted file mode 100644 index 18c0a27b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - rqt_plot - 0.3.13 - rqt_plot provides a GUI plugin visualizing numeric values in a 2D plot using different plotting backends. - Dorian Scholz - Austin Hendrix - - BSD - - http://ros.org/wiki/rqt_plot - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-matplotlib - python-qt-bindings-qwt5 - python-rospkg - qt_gui_py_common - rosgraph - rostopic - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml b/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml deleted file mode 100644 index fb771b83..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A plugin visualizing numeric values in a 2D plot using different plotting backends. - - - - - folder - Plugins related to visualization. - - - utilities-system-monitor - A plugin for visualizing numeric values in a 2D plot using different plotting backends. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui b/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui deleted file mode 100644 index 8419a72f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/resource/plot.ui +++ /dev/null @@ -1,157 +0,0 @@ - - - DataPlotWidget - - - - 0 - 0 - 561 - 61 - - - - true - - - Plot - - - - 0 - - - 0 - - - - - 0 - - - 5 - - - - - Topic - - - - - - - / - - - - - - - - 0 - 0 - - - - Qt::DefaultContextMenu - - - add topic to plot - - - - 16 - 16 - - - - - - - - - 16 - 16 - - - - - - - - Qt::Horizontal - - - - 40 - 20 - - - - - - - - autoscroll - - - true - - - - - - - - 0 - 0 - - - - pause plot - - - - 16 - 16 - - - - true - - - - - - - - 0 - 0 - - - - clear plot - - - - 16 - 16 - - - - - - - - - - 0 - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot b/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot deleted file mode 100755 index 57e1225a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/scripts/rqt_plot +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from rqt_plot.plot import Plot - -plugin = 'rqt_plot.plot.Plot' -main = Main(filename=plugin) -sys.exit(main.main(standalone=plugin, plugin_argument_provider=Plot.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/setup.py b/workspace/src/rqt_common_plugins/rqt_plot/setup.py deleted file mode 100644 index 4ab6df5b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/setup.py +++ /dev/null @@ -1,12 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_plot', 'rqt_plot.data_plot'], - package_dir={'': 'src'}, - scripts=['scripts/rqt_plot'] -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/__init__.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py deleted file mode 100644 index b2ed810e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/__init__.py +++ /dev/null @@ -1,526 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2014, Austin Hendrix -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import numpy - -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog -from python_qt_binding import QT_BINDING -from python_qt_binding.QtCore import Qt, qWarning, Signal -from python_qt_binding.QtGui import QColor, QWidget, QHBoxLayout -from rqt_py_common.ini_helper import pack, unpack - -try: - from pyqtgraph_data_plot import PyQtGraphDataPlot -except ImportError: - PyQtGraphDataPlot = None - -try: - from mat_data_plot import MatDataPlot -except ImportError: - MatDataPlot = None - -try: - from qwt_data_plot import QwtDataPlot -except ImportError: - QwtDataPlot = None - -# separate class for DataPlot exceptions, just so that users can differentiate -# errors from the DataPlot widget from exceptions generated by the underlying -# libraries -class DataPlotException(Exception): - pass - -class DataPlot(QWidget): - """A widget for displaying a plot of data - - The DataPlot widget displays a plot, on one of several plotting backends, - depending on which backend(s) are available at runtime. It currently - supports PyQtGraph, MatPlot and QwtPlot backends. - - The DataPlot widget manages the plot backend internally, and can save - and restore the internal state using `save_settings` and `restore_settings` - functions. - - Currently, the user MUST call `restore_settings` before using the widget, - to cause the creation of the enclosed plotting widget. - """ - # plot types in order of priority - plot_types = [ - { - 'title': 'PyQtGraph', - 'widget_class': PyQtGraphDataPlot, - 'description': 'Based on PyQtGraph\n- installer: http://luke.campagnola.me/code/pyqtgraph\n', - 'enabled': PyQtGraphDataPlot is not None, - }, - { - 'title': 'MatPlot', - 'widget_class': MatDataPlot, - 'description': 'Based on MatPlotLib\n- needs most CPU\n- needs matplotlib >= 1.1.0\n- if using PySide: PySide > 1.1.0\n', - 'enabled': MatDataPlot is not None, - }, - { - 'title': 'QwtPlot', - 'widget_class': QwtDataPlot, - 'description': 'Based on QwtPlot\n- does not use timestamps\n- uses least CPU\n- needs Python Qwt bindings\n', - 'enabled': QwtDataPlot is not None, - }, - ] - - # pre-defined colors: - RED=(255, 0, 0) - GREEN=(0, 255, 0) - BLUE=(0, 0, 255) - - SCALE_ALL=1 - SCALE_VISIBLE=2 - SCALE_EXTEND=4 - - _colors = [Qt.blue, Qt.red, Qt.cyan, Qt.magenta, Qt.green, Qt.darkYellow, Qt.black, Qt.darkCyan, Qt.darkRed, Qt.gray] - - limits_changed = Signal() - _redraw = Signal() - _add_curve = Signal(str, str, 'QColor', bool) - - def __init__(self, parent=None): - """Create a new, empty DataPlot - - This will raise a RuntimeError if none of the supported plotting - backends can be found - """ - super(DataPlot, self).__init__(parent) - self._plot_index = 0 - self._color_index = 0 - self._markers_on = False - self._autoscroll = True - - self._autoscale_x = True - self._autoscale_y = DataPlot.SCALE_ALL - - # the backend widget that we're trying to hide/abstract - self._data_plot_widget = None - self._curves = {} - self._vline = None - self._redraw.connect(self._do_redraw) - - self._layout = QHBoxLayout() - self.setLayout(self._layout) - - enabled_plot_types = [pt for pt in self.plot_types if pt['enabled']] - if not enabled_plot_types: - version_info = ' and PySide > 1.1.0' if QT_BINDING == 'pyside' else '' - raise RuntimeError('No usable plot type found. Install at least one of: PyQtGraph, MatPlotLib (at least 1.1.0%s) or Python-Qwt5.' % version_info) - - self._switch_data_plot_widget(self._plot_index) - - self.show() - - def _switch_data_plot_widget(self, plot_index, markers_on=False): - """Internal method for activating a plotting backend by index""" - # check if selected plot type is available - if not self.plot_types[plot_index]['enabled']: - # find other available plot type - for index, plot_type in enumerate(self.plot_types): - if plot_type['enabled']: - plot_index = index - break - - self._plot_index = plot_index - self._markers_on = markers_on - selected_plot = self.plot_types[plot_index] - - if self._data_plot_widget: - x_limits = self.get_xlim() - y_limits = self.get_ylim() - - self._layout.removeWidget(self._data_plot_widget) - self._data_plot_widget.close() - self._data_plot_widget = None - else: - x_limits = [0.0, 10.0] - y_limits = [-0.001, 0.001] - - self._data_plot_widget = selected_plot['widget_class'](self) - self._data_plot_widget.limits_changed.connect(self.limits_changed) - self._add_curve.connect(self._data_plot_widget.add_curve) - self._layout.addWidget(self._data_plot_widget) - - # restore old data - for curve_id in self._curves: - curve = self._curves[curve_id] - self._data_plot_widget.add_curve(curve_id, curve['name'], curve['color'], markers_on) - - if self._vline: - self.vline(*self._vline) - - self.set_xlim(x_limits) - self.set_ylim(y_limits) - self.redraw() - - def _switch_plot_markers(self, markers_on): - self._markers_on = markers_on - self._data_plot_widget._color_index = 0 - - for curve_id in self._curves: - self._data_plot_widget.remove_curve(curve_id) - curve = self._curves[curve_id] - self._data_plot_widget.add_curve(curve_id, curve['name'], curve['color'], markers_on) - - self.redraw() - - # interface out to the managing GUI component: get title, save, restore, - # etc - def getTitle(self): - """get the title of the current plotting backend""" - return self.plot_types[self._plot_index]['title'] - - def save_settings(self, plugin_settings, instance_settings): - """Save the settings associated with this widget - - Currently, this is just the plot type, but may include more useful - data in the future""" - instance_settings.set_value('plot_type', self._plot_index) - xlim = self.get_xlim() - ylim = self.get_ylim() - # convert limits to normal arrays of floats; some backends return numpy - # arrays - xlim = [float(x) for x in xlim] - ylim = [float(y) for y in ylim] - instance_settings.set_value('x_limits', pack(xlim)) - instance_settings.set_value('y_limits', pack(ylim)) - - def restore_settings(self, plugin_settings, instance_settings): - """Restore the settings for this widget - - Currently, this just restores the plot type.""" - self._switch_data_plot_widget(int(instance_settings.value('plot_type', 0))) - xlim = unpack(instance_settings.value('x_limits', [])) - ylim = unpack(instance_settings.value('y_limits', [])) - if xlim: - # convert limits to an array of floats; they're often lists of - # strings - try: - xlim = [float(x) for x in xlim] - self.set_xlim(xlim) - except: - qWarning("Failed to restore X limits") - if ylim: - try: - ylim = [float(y) for y in ylim] - self.set_ylim(ylim) - except: - qWarning("Failed to restore Y limits") - - - def doSettingsDialog(self): - """Present the user with a dialog for choosing the plot backend - - This displays a SimpleSettingsDialog asking the user to choose a - plot type, gets the result, and updates the plot type as necessary - - This method is blocking""" - - marker_settings = [ - { - 'title': 'Show Plot Markers', - 'description': 'Warning: Displaying markers in rqt_plot may cause\n \t high cpu load, especially using PyQtGraph\n', - 'enabled': True, - }] - if self._markers_on: - selected_checkboxes = [0] - else: - selected_checkboxes = [] - - dialog = SimpleSettingsDialog(title='Plot Options') - dialog.add_exclusive_option_group(title='Plot Type', options=self.plot_types, selected_index=self._plot_index) - dialog.add_checkbox_group(title='Plot Markers', options=marker_settings, selected_indexes=selected_checkboxes) - [plot_type, checkboxes] = dialog.get_settings() - if plot_type is not None and plot_type['selected_index'] is not None and self._plot_index != plot_type['selected_index']: - self._switch_data_plot_widget(plot_type['selected_index'], 0 in checkboxes['selected_indexes']) - else: - if checkboxes is not None and self._markers_on != (0 in checkboxes['selected_indexes']): - self._switch_plot_markers(0 in checkboxes['selected_indexes']) - - # interface out to the managing DATA component: load data, update data, - # etc - def autoscroll(self, enabled=True): - """Enable or disable autoscrolling of the plot""" - self._autoscroll = enabled - - def redraw(self): - self._redraw.emit() - - def _do_redraw(self): - """Redraw the underlying plot - - This causes the underlying plot to be redrawn. This is usually used - after adding or updating the plot data""" - if self._data_plot_widget: - self._merged_autoscale() - for curve_id in self._curves: - curve = self._curves[curve_id] - self._data_plot_widget.set_values(curve_id, curve['x'], curve['y']) - self._data_plot_widget.redraw() - - def _get_curve(self, curve_id): - if curve_id in self._curves: - return self._curves[curve_id] - else: - raise DataPlotException("No curve named %s in this DataPlot" % - ( curve_id) ) - - def add_curve(self, curve_id, curve_name, data_x, data_y): - """Add a new, named curve to this plot - - Add a curve named `curve_name` to the plot, with initial data series - `data_x` and `data_y`. - - Future references to this curve should use the provided `curve_id` - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - curve_color = QColor(self._colors[self._color_index % len(self._colors)]) - self._color_index += 1 - - self._curves[curve_id] = { 'x': numpy.array(data_x), - 'y': numpy.array(data_y), - 'name': curve_name, - 'color': curve_color} - if self._data_plot_widget: - self._add_curve.emit(curve_id, curve_name, curve_color, self._markers_on) - - def remove_curve(self, curve_id): - """Remove the specified curve from this plot""" - # TODO: do on UI thread with signals - if curve_id in self._curves: - del self._curves[curve_id] - if self._data_plot_widget: - self._data_plot_widget.remove_curve(curve_id) - - def update_values(self, curve_id, values_x, values_y): - """Append new data to an existing curve - - `values_x` and `values_y` will be appended to the existing data for - `curve_id` - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - curve = self._get_curve(curve_id) - curve['x'] = numpy.append(curve['x'], values_x) - curve['y'] = numpy.append(curve['y'], values_y) - # sort resulting data, so we can slice it later - sort_order = curve['x'].argsort() - curve['x'] = curve['x'][sort_order] - curve['y'] = curve['y'][sort_order] - - def clear_values(self, curve_id=None): - """Clear the values for the specified curve, or all curves - - This will erase the data series associaed with `curve_id`, or all - curves if `curve_id` is not present or is None - - Note that the plot is not redraw automatically; call `redraw()` to make - any changes visible to the user. - """ - # clear internal curve representation - if curve_id: - curve = self._get_curve(curve_id) - curve['x'] = numpy.array([]) - curve['y'] = numpy.array([]) - else: - for curve_id in self._curves: - self._curves[curve_id]['x'] = numpy.array([]) - self._curves[curve_id]['y'] = numpy.array([]) - - - def vline(self, x, color=RED): - """Draw a vertical line on the plot - - Draw a line a position X, with the given color - - @param x: position of the vertical line to draw - @param color: optional parameter specifying the color, as tuple of - RGB values from 0 to 255 - """ - self._vline = (x, color) - if self._data_plot_widget: - self._data_plot_widget.vline(x, color) - - # autoscaling methods - def set_autoscale(self, x=None, y=None): - """Change autoscaling of plot axes - - if a parameter is not passed, the autoscaling setting for that axis is - not changed - - @param x: enable or disable autoscaling for X - @param y: set autoscaling mode for Y - """ - if x is not None: - self._autoscale_x = x - if y is not None: - self._autoscale_y = y - - # autoscaling: adjusting the plot bounds fit the data - # autoscrollig: move the plot X window to show the most recent data - # - # what order do we do these adjustments in? - # * assuming the various stages are enabled: - # * autoscale X to bring all data into view - # * else, autoscale X to determine which data we're looking at - # * autoscale Y to fit the data we're viewing - # - # * autoscaling of Y might have several modes: - # * scale Y to fit the entire dataset - # * scale Y to fit the current view - # * increase the Y scale to fit the current view - # - # TODO: incrmenetal autoscaling: only update the autoscaling bounds - # when new data is added - def _merged_autoscale(self): - x_limit = [numpy.inf, -numpy.inf] - if self._autoscale_x: - for curve_id in self._curves: - curve = self._curves[curve_id] - if len(curve['x']) > 0: - x_limit[0] = min(x_limit[0], curve['x'].min()) - x_limit[1] = max(x_limit[1], curve['x'].max()) - elif self._autoscroll: - # get current width of plot - x_limit = self.get_xlim() - x_width = x_limit[1] - x_limit[0] - - # reset the upper x_limit so that we ignore the previous position - x_limit[1] = -numpy.inf - - # get largest X value - for curve_id in self._curves: - curve = self._curves[curve_id] - if len(curve['x']) > 0: - x_limit[1] = max(x_limit[1], curve['x'].max()) - - # set lower limit based on width - x_limit[0] = x_limit[1] - x_width - else: - # don't modify limit, or get it from plot - x_limit = self.get_xlim() - - # set sane limits if our limits are infinite - if numpy.isinf(x_limit[0]): - x_limit[0] = 0.0 - if numpy.isinf(x_limit[1]): - x_limit[1] = 1.0 - - - y_limit = [numpy.inf, -numpy.inf] - if self._autoscale_y: - # if we're extending the y limits, initialize them with the - # current limits - if self._autoscale_y & DataPlot.SCALE_EXTEND: - y_limit = self.get_ylim() - for curve_id in self._curves: - curve = self._curves[curve_id] - start_index = 0 - end_index = len(curve['x']) - - # if we're scaling based on the visible window, find the - # start and end indicies of our window - if self._autoscale_y & DataPlot.SCALE_VISIBLE: - # indexof x_limit[0] in curves['x'] - start_index = curve['x'].searchsorted(x_limit[0]) - # indexof x_limit[1] in curves['x'] - end_index = curve['x'].searchsorted(x_limit[1]) - - # region here is cheap because it is a numpy view and not a - # copy of the underlying data - region = curve['y'][start_index:end_index] - if len(region) > 0: - y_limit[0] = min(y_limit[0], region.min()) - y_limit[1] = max(y_limit[1], region.max()) - - # TODO: compute padding around new min and max values - # ONLY consider data for new values; not - # existing limits, or we'll add padding on top of old - # padding in SCALE_EXTEND mode - # - # pad the min/max - # TODO: invert this padding in get_ylim - #ymin = limits[0] - #ymax = limits[1] - #delta = ymax - ymin if ymax != ymin else 0.1 - #ymin -= .05 * delta - #ymax += .05 * delta - else: - y_limit = self.get_ylim() - - # set sane limits if our limits are infinite - if numpy.isinf(y_limit[0]): - y_limit[0] = 0.0 - if numpy.isinf(y_limit[1]): - y_limit[1] = 1.0 - - self.set_xlim(x_limit) - self.set_ylim(y_limit) - - def get_xlim(self): - """get X limits""" - if self._data_plot_widget: - return self._data_plot_widget.get_xlim() - else: - qWarning("No plot widget; returning default X limits") - return [0.0, 1.0] - - def set_xlim(self, limits): - """set X limits""" - if self._data_plot_widget: - self._data_plot_widget.set_xlim(limits) - else: - qWarning("No plot widget; can't set X limits") - - def get_ylim(self): - """get Y limits""" - if self._data_plot_widget: - return self._data_plot_widget.get_ylim() - else: - qWarning("No plot widget; returning default Y limits") - return [0.0, 10.0] - - def set_ylim(self, limits): - """set Y limits""" - if self._data_plot_widget: - self._data_plot_widget.set_ylim(limits) - else: - qWarning("No plot widget; can't set Y limits") - - # signal on y limit changed? diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py deleted file mode 100644 index 91a26031..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/mat_data_plot.py +++ /dev/null @@ -1,155 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Ye Cheng, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding import QT_BINDING, QT_BINDING_VERSION -if QT_BINDING == 'pyside': - try: - from pkg_resources import parse_version - except: - import re - - def parse_version(s): - return [int(x) for x in re.sub(r'(\.0+)*$', '', s).split('.')] - if parse_version(QT_BINDING_VERSION) <= parse_version('1.1.2'): - raise ImportError('A PySide version newer than 1.1.0 is required.') - -from python_qt_binding.QtCore import Slot, Qt, qWarning, Signal -from python_qt_binding.QtGui import QWidget, QVBoxLayout, QSizePolicy, QColor - -import operator -import matplotlib -if matplotlib.__version__ < '1.1.0': - raise ImportError('A newer matplotlib is required (at least 1.1.0)') - -try: - matplotlib.use('Qt4Agg') - from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas -except ImportError: - # work around bug in dateutil - import sys - import thread - sys.modules['_thread'] = thread - from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas -from matplotlib.backends.backend_qt4agg import NavigationToolbar2QTAgg as NavigationToolbar -from matplotlib.figure import Figure - -import numpy - - -class MatDataPlot(QWidget): - class Canvas(FigureCanvas): - """Ultimately, this is a QWidget (as well as a FigureCanvasAgg, etc.).""" - def __init__(self, parent=None): - super(MatDataPlot.Canvas, self).__init__(Figure()) - self.axes = self.figure.add_subplot(111) - self.axes.grid(True, color='gray') - self.figure.tight_layout() - self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) - self.updateGeometry() - - def resizeEvent(self, event): - super(MatDataPlot.Canvas, self).resizeEvent(event) - self.figure.tight_layout() - - limits_changed = Signal() - - def __init__(self, parent=None): - super(MatDataPlot, self).__init__(parent) - self._canvas = MatDataPlot.Canvas() - self._toolbar = NavigationToolbar(self._canvas, self._canvas) - vbox = QVBoxLayout() - vbox.addWidget(self._toolbar) - vbox.addWidget(self._canvas) - self.setLayout(vbox) - - self._curves = {} - self._current_vline = None - self._canvas.mpl_connect('button_release_event', self._limits_changed) - - def _limits_changed(self, event): - self.limits_changed.emit() - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - - # adding an empty curve and change the limits, so save and restore them - x_limits = self.get_xlim() - y_limits = self.get_ylim() - if markers_on: - marker_size = 3 - else: - marker_size = 0 - line = self._canvas.axes.plot([], [], 'o-', markersize=marker_size, label=curve_name, linewidth=1, picker=5, color=curve_color.name())[0] - self._curves[curve_id] = line - self._update_legend() - self.set_xlim(x_limits) - self.set_ylim(y_limits) - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._curves[curve_id].remove() - del self._curves[curve_id] - self._update_legend() - - def _update_legend(self): - handles, labels = self._canvas.axes.get_legend_handles_labels() - if handles: - hl = sorted(zip(handles, labels), key=operator.itemgetter(1)) - handles, labels = zip(*hl) - self._canvas.axes.legend(handles, labels, loc='upper left') - - def set_values(self, curve, data_x, data_y): - line = self._curves[curve] - line.set_data(data_x, data_y) - - def redraw(self): - self._canvas.axes.grid(True, color='gray') - self._canvas.draw() - - def vline(self, x, color): - # convert color range from (0,255) to (0,1.0) - matcolor=(color[0]/255.0, color[1]/255.0, color[2]/255.0) - if self._current_vline: - self._current_vline.remove() - self._current_vline = self._canvas.axes.axvline(x=x, color=matcolor) - - def set_xlim(self, limits): - self._canvas.axes.set_xbound(lower=limits[0], upper=limits[1]) - - def set_ylim(self, limits): - self._canvas.axes.set_ybound(lower=limits[0], upper=limits[1]) - - def get_xlim(self): - return list(self._canvas.axes.get_xbound()) - - def get_ylim(self): - return list(self._canvas.axes.get_ybound()) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py deleted file mode 100644 index a23b7074..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/pyqtgraph_data_plot.py +++ /dev/null @@ -1,111 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Slot, Qt, qWarning, Signal -from python_qt_binding.QtGui import QColor, QVBoxLayout, QWidget - -from pyqtgraph import PlotWidget, mkPen, mkBrush -import numpy - - -class PyQtGraphDataPlot(QWidget): - - limits_changed = Signal() - - def __init__(self, parent=None): - super(PyQtGraphDataPlot, self).__init__(parent) - self._plot_widget = PlotWidget() - self._plot_widget.getPlotItem().addLegend() - self._plot_widget.setBackground((255, 255, 255)) - self._plot_widget.setXRange(0, 10, padding=0) - vbox = QVBoxLayout() - vbox.addWidget(self._plot_widget) - self.setLayout(vbox) - self._plot_widget.getPlotItem().sigRangeChanged.connect(self.limits_changed) - - self._curves = {} - self._current_vline = None - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - pen = mkPen(curve_color, width=1) - symbol = "o" - symbolPen = mkPen(QColor(Qt.black)) - symbolBrush = mkBrush(curve_color) - # this adds the item to the plot and legend - if markers_on: - plot = self._plot_widget.plot(name=curve_name, pen=pen, symbol=symbol, symbolPen=symbolPen, symbolBrush=symbolBrush, symbolSize=4) - else: - plot = self._plot_widget.plot(name=curve_name, pen=pen) - self._curves[curve_id] = plot - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._plot_widget.removeItem(self._curves[curve_id]) - del self._curves[curve_id] - self._update_legend() - - def _update_legend(self): - # clear and rebuild legend (there is no remove item method for the legend...) - self._plot_widget.clear() - self._plot_widget.getPlotItem().legend.items = [] - for curve in self._curves.values(): - self._plot_widget.addItem(curve) - if self._current_vline: - self._plot_widget.addItem(self._current_vline) - - def redraw(self): - pass - - def set_values(self, curve_id, data_x, data_y): - curve = self._curves[curve_id] - curve.setData(data_x, data_y) - - def vline(self, x, color): - if self._current_vline: - self._plot_widget.removeItem(self._current_vline) - self._current_vline = self._plot_widget.addLine(x=x, pen=color) - - def set_xlim(self, limits): - # TODO: this doesn't seem to handle fast updates well - self._plot_widget.setXRange(limits[0], limits[1], padding=0) - - def set_ylim(self, limits): - self._plot_widget.setYRange(limits[0], limits[1], padding=0) - - def get_xlim(self): - x_range, _ = self._plot_widget.viewRange() - return x_range - - def get_ylim(self): - _, y_range = self._plot_widget.viewRange() - return y_range diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py deleted file mode 100644 index 8f5070c5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/data_plot/qwt_data_plot.py +++ /dev/null @@ -1,255 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# -*- coding: utf-8 -*- -from __future__ import division -import math -import sys - -from python_qt_binding.QtCore import QEvent, QSize, QPointF, Qt, SIGNAL, Signal, Slot, qWarning -from python_qt_binding.QtGui import QColor, QPen, QBrush, QVector2D -import Qwt - -from numpy import arange, zeros, concatenate - - -# create real QwtDataPlot class -class QwtDataPlot(Qwt.QwtPlot): - mouseCoordinatesChanged = Signal(QPointF) - _num_value_saved = 1000 - _num_values_ploted = 1000 - - limits_changed = Signal() - - def __init__(self, *args): - super(QwtDataPlot, self).__init__(*args) - self.setCanvasBackground(Qt.white) - self.insertLegend(Qwt.QwtLegend(), Qwt.QwtPlot.BottomLegend) - - self._curves = {} - - # TODO: rejigger these internal data structures so that they're easier - # to work with, and easier to use with set_xlim and set_ylim - # this may also entail rejiggering the _time_axis so that it's - # actually time axis data, or just isn't required any more - # new data structure - self._x_limits = [0.0, 10.0] - self._y_limits = [0.0, 10.0] - - # old data structures - self._last_canvas_x = 0 - self._last_canvas_y = 0 - self._pressed_canvas_y = 0 - self._pressed_canvas_x = 0 - self._last_click_coordinates = None - - marker_axis_y = Qwt.QwtPlotMarker() - marker_axis_y.setLabelAlignment(Qt.AlignRight | Qt.AlignTop) - marker_axis_y.setLineStyle(Qwt.QwtPlotMarker.HLine) - marker_axis_y.setYValue(0.0) - marker_axis_y.attach(self) - - self._picker = Qwt.QwtPlotPicker( - Qwt.QwtPlot.xBottom, Qwt.QwtPlot.yLeft, Qwt.QwtPicker.PolygonSelection, - Qwt.QwtPlotPicker.PolygonRubberBand, Qwt.QwtPicker.AlwaysOn, self.canvas() - ) - self._picker.setRubberBandPen(QPen(Qt.blue)) - self._picker.setTrackerPen(QPen(Qt.blue)) - - # Initialize data - self.rescale() - #self.move_canvas(0, 0) - self.canvas().setMouseTracking(True) - self.canvas().installEventFilter(self) - - def eventFilter(self, _, event): - if event.type() == QEvent.MouseButtonRelease: - x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x()) - y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y()) - self._last_click_coordinates = QPointF(x, y) - self.limits_changed.emit() - elif event.type() == QEvent.MouseMove: - x = self.invTransform(Qwt.QwtPlot.xBottom, event.pos().x()) - y = self.invTransform(Qwt.QwtPlot.yLeft, event.pos().y()) - coords = QPointF(x, y) - if self._picker.isActive() and self._last_click_coordinates is not None: - toolTip = 'origin x: %.5f, y: %.5f' % (self._last_click_coordinates.x(), self._last_click_coordinates.y()) - delta = coords - self._last_click_coordinates - toolTip += '\ndelta x: %.5f, y: %.5f\nlength: %.5f' % (delta.x(), delta.y(), QVector2D(delta).length()) - else: - toolTip = 'buttons\nleft: measure\nmiddle: move\nright: zoom x/y\nwheel: zoom y' - self.setToolTip(toolTip) - self.mouseCoordinatesChanged.emit(coords) - return False - - def log(self, level, message): - self.emit(SIGNAL('logMessage'), level, message) - - def resizeEvent(self, event): - Qwt.QwtPlot.resizeEvent(self, event) - self.rescale() - - def add_curve(self, curve_id, curve_name, curve_color=QColor(Qt.blue), markers_on=False): - curve_id = str(curve_id) - if curve_id in self._curves: - return - curve_object = Qwt.QwtPlotCurve(curve_name) - curve_object.attach(self) - curve_object.setPen(curve_color) - if markers_on: - curve_object.setSymbol(Qwt.QwtSymbol(Qwt.QwtSymbol.Ellipse, QBrush(curve_color), QPen(Qt.black), QSize(4,4))) - self._curves[curve_id] = curve_object - - def remove_curve(self, curve_id): - curve_id = str(curve_id) - if curve_id in self._curves: - self._curves[curve_id].hide() - self._curves[curve_id].attach(None) - del self._curves[curve_id] - - def set_values(self, curve_id, data_x, data_y): - curve = self._curves[curve_id] - curve.setData(data_x, data_y) - - def redraw(self): - self.replot() - - # ---------------------------------------------- - # begin qwtplot internal methods - # ---------------------------------------------- - def rescale(self): - self.setAxisScale(Qwt.QwtPlot.yLeft, - self._y_limits[0], - self._y_limits[1]) - self.setAxisScale(Qwt.QwtPlot.xBottom, - self._x_limits[0], - self._x_limits[1]) - - self._canvas_display_height = self._y_limits[1] - self._y_limits[0] - self._canvas_display_width = self._x_limits[1] - self._x_limits[0] - self.redraw() - - def rescale_axis_x(self, delta__x): - """ - add delta_x to the length of the x axis - """ - x_width = self._x_limits[1] - self._x_limits[0] - x_width += delta__x - self._x_limits[1] = x_width + self._x_limits[0] - self.rescale() - - def scale_axis_y(self, max_value): - """ - set the y axis height to max_value, about the current center - """ - canvas_display_height = max_value - canvas_offset_y = (self._y_limits[1] + self._y_limits[0])/2.0 - y_lower_limit = canvas_offset_y - (canvas_display_height / 2) - y_upper_limit = canvas_offset_y + (canvas_display_height / 2) - self._y_limits = [y_lower_limit, y_upper_limit] - self.rescale() - - def move_canvas(self, delta_x, delta_y): - """ - move the canvas by delta_x and delta_y in SCREEN COORDINATES - """ - canvas_offset_x = delta_x * self._canvas_display_width / self.canvas().width() - canvas_offset_y = delta_y * self._canvas_display_height / self.canvas().height() - self._x_limits = [ l + canvas_offset_x for l in self._x_limits] - self._y_limits = [ l + canvas_offset_y for l in self._y_limits] - self.rescale() - - def mousePressEvent(self, event): - self._last_canvas_x = event.x() - self.canvas().x() - self._last_canvas_y = event.y() - self.canvas().y() - self._pressed_canvas_y = event.y() - self.canvas().y() - - def mouseMoveEvent(self, event): - canvas_x = event.x() - self.canvas().x() - canvas_y = event.y() - self.canvas().y() - if event.buttons() & Qt.MiddleButton: # middle button moves the canvas - delta_x = self._last_canvas_x - canvas_x - delta_y = canvas_y - self._last_canvas_y - self.move_canvas(delta_x, delta_y) - elif event.buttons() & Qt.RightButton: # right button zooms - zoom_factor = max(-0.6, min(0.6, (self._last_canvas_y - canvas_y) / 20.0 / 2.0)) - delta_y = (self.canvas().height() / 2.0) - self._pressed_canvas_y - self.move_canvas(0, zoom_factor * delta_y * 1.0225) - self.scale_axis_y(max(0.005, self._canvas_display_height - (zoom_factor * self._canvas_display_height))) - self.rescale_axis_x(self._last_canvas_x - canvas_x) - self._last_canvas_x = canvas_x - self._last_canvas_y = canvas_y - - def wheelEvent(self, event): # mouse wheel zooms the y-axis - # y position of pointer in graph coordinates - canvas_y = event.y() - self.canvas().y() - - zoom_factor = max(-0.6, min(0.6, (event.delta() / 120) / 6.0)) - delta_y = (self.canvas().height() / 2.0) - canvas_y - self.move_canvas(0, zoom_factor * delta_y * 1.0225) - - self.scale_axis_y(max(0.0005, self._canvas_display_height - zoom_factor * self._canvas_display_height)) - self.limits_changed.emit() - - - def vline(self, x, color): - qWarning("QwtDataPlot.vline is not implemented yet") - - def set_xlim(self, limits): - self.setAxisScale(Qwt.QwtPlot.xBottom, limits[0], limits[1]) - self._x_limits = limits - - def set_ylim(self, limits): - self.setAxisScale(Qwt.QwtPlot.yLeft, limits[0], limits[1]) - self._y_limits = limits - - def get_xlim(self): - return self._x_limits - - def get_ylim(self): - return self._y_limits - - -if __name__ == '__main__': - from python_qt_binding.QtGui import QApplication - - app = QApplication(sys.argv) - plot = QwtDataPlot() - plot.resize(700, 500) - plot.show() - plot.add_curve(0, '(x/500)^2') - plot.add_curve(1, 'sin(x / 20) * 500') - for i in range(plot._num_value_saved): - plot.update_value(0, (i / 500.0) * (i / 5.0)) - plot.update_value(1, math.sin(i / 20.0) * 500) - - sys.exit(app.exec_()) diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py deleted file mode 100644 index 62586d85..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot.py +++ /dev/null @@ -1,143 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import argparse - -from python_qt_binding import QT_BINDING -from python_qt_binding.QtCore import qDebug -from rqt_gui_py.plugin import Plugin - -from rqt_py_common.ini_helper import pack, unpack - -from .plot_widget import PlotWidget - -from .data_plot import DataPlot - -class Plot(Plugin): - - def __init__(self, context): - super(Plot, self).__init__(context) - self.setObjectName('Plot') - - self._context = context - - self._args = self._parse_args(context.argv()) - self._widget = PlotWidget(initial_topics=self._args.topics, start_paused=self._args.start_paused) - self._data_plot = DataPlot(self._widget) - - # disable autoscaling of X, and set a sane default range - self._data_plot.set_autoscale(x=False) - self._data_plot.set_autoscale(y=DataPlot.SCALE_EXTEND|DataPlot.SCALE_VISIBLE) - self._data_plot.set_xlim([0, 10.0]) - - self._widget.switch_data_plot_widget(self._data_plot) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def _parse_args(self, argv): - parser = argparse.ArgumentParser(prog='rqt_plot', add_help=False) - Plot.add_arguments(parser) - args = parser.parse_args(argv) - - # convert topic arguments into topic names - topic_list = [] - for t in args.topics: - # c_topics is the list of topics to plot - c_topics = [] - # compute combined topic list, t == '/foo/bar1,/baz/bar2' - for sub_t in [x for x in t.split(',') if x]: - # check for shorthand '/foo/field1:field2:field3' - if ':' in sub_t: - base = sub_t[:sub_t.find(':')] - # the first prefix includes a field name, so save then strip it off - c_topics.append(base) - if not '/' in base: - parser.error("%s must contain a topic and field name" % sub_t) - base = base[:base.rfind('/')] - - # compute the rest of the field names - fields = sub_t.split(':')[1:] - c_topics.extend(["%s/%s" % (base, f) for f in fields if f]) - else: - c_topics.append(sub_t) - # #1053: resolve command-line topic names - import rosgraph - c_topics = [rosgraph.names.script_resolve_name('rqt_plot', n) for n in c_topics] - if type(c_topics) == list: - topic_list.extend(c_topics) - else: - topic_list.append(c_topics) - args.topics = topic_list - - return args - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_plot plugin') - group.add_argument('-P', '--pause', action='store_true', dest='start_paused', - help='Start in paused state') - group.add_argument('-e', '--empty', action='store_true', dest='start_empty', - help='Start without restoring previous topics') - group.add_argument('topics', nargs='*', default=[], help='Topics to plot') - - def _update_title(self): - self._widget.setWindowTitle(self._data_plot.getTitle()) - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - - def save_settings(self, plugin_settings, instance_settings): - self._data_plot.save_settings(plugin_settings, instance_settings) - instance_settings.set_value('autoscroll', self._widget.autoscroll_checkbox.isChecked()) - instance_settings.set_value('topics', pack(self._widget._rosdata.keys())) - - def restore_settings(self, plugin_settings, instance_settings): - autoscroll = instance_settings.value('autoscroll', True) in [True, 'true'] - self._widget.autoscroll_checkbox.setChecked(autoscroll) - self._data_plot.autoscroll(autoscroll) - - self._update_title() - - if len(self._widget._rosdata.keys()) == 0 and not self._args.start_empty: - topics = unpack(instance_settings.value('topics', [])) - if topics: - for topic in topics: - self._widget.add_topic(topic) - - self._data_plot.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - self._data_plot.doSettingsDialog() - self._update_title() - - def shutdown_plugin(self): - self._widget.clean_up_subscribers() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py deleted file mode 100644 index a8a30b8e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/plot_widget.py +++ /dev/null @@ -1,315 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg -import roslib - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QTimer, qWarning, Slot -from python_qt_binding.QtGui import QAction, QIcon, QMenu, QWidget - -import rospy - -from rqt_py_common.topic_completer import TopicCompleter -from rqt_py_common import topic_helpers - -from . rosplot import ROSData, RosPlotException - -def get_plot_fields(topic_name): - topic_type, real_topic, _ = topic_helpers.get_topic_type(topic_name) - if topic_type is None: - message = "topic %s does not exist" % ( topic_name ) - return [], message - field_name = topic_name[len(real_topic)+1:] - - slot_type, is_array, array_size = roslib.msgs.parse_type(topic_type) - field_class = roslib.message.get_message_class(slot_type) - - fields = [f for f in field_name.split('/') if f] - - for field in fields: - # parse the field name for an array index - try: - field, _, field_index = roslib.msgs.parse_type(field) - except roslib.msgs.MsgSpecException: - message = "invalid field %s in topic %s" % ( field, real_topic ) - return [], message - - if field not in getattr(field_class, '__slots__', []): - message = "no field %s in topic %s" % ( field_name, real_topic ) - return [], message - slot_type = field_class._slot_types[field_class.__slots__.index(field)] - slot_type, slot_is_array, array_size = roslib.msgs.parse_type(slot_type) - is_array = slot_is_array and field_index is None - - field_class = topic_helpers.get_type_class(slot_type) - - if field_class in (int, float): - if is_array: - if array_size is not None: - message = "topic %s is fixed-size numeric array" % ( topic_name ) - return [ "%s[%d]" % (topic_name, i) for i in range(array_size) ], message - else: - message = "topic %s is variable-size numeric array" % ( topic_name ) - return [], message - else: - message = "topic %s is numeric" % ( topic_name ) - return [ topic_name ], message - else: - if not roslib.msgs.is_valid_constant_type(slot_type): - numeric_fields = [] - for i, slot in enumerate(field_class.__slots__): - slot_type = field_class._slot_types[i] - slot_type, is_array, array_size = roslib.msgs.parse_type(slot_type) - slot_class = topic_helpers.get_type_class(slot_type) - if slot_class in (int, float) and not is_array: - numeric_fields.append(slot) - message = "" - if len(numeric_fields) > 0: - message = "%d plottable fields in %s" % ( len(numeric_fields), topic_name ) - else: - message = "No plottable fields in %s" % ( topic_name ) - return [ "%s/%s" % (topic_name, f) for f in numeric_fields ], message - else: - message = "Topic %s is not numeric" % ( topic_name ) - return [], message - -def is_plottable(topic_name): - fields, message = get_plot_fields(topic_name) - return len(fields) > 0, message - -class PlotWidget(QWidget): - _redraw_interval = 40 - - def __init__(self, initial_topics=None, start_paused=False): - super(PlotWidget, self).__init__() - self.setObjectName('PlotWidget') - - self._initial_topics = initial_topics - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_plot'), 'resource', 'plot.ui') - loadUi(ui_file, self) - self.subscribe_topic_button.setIcon(QIcon.fromTheme('list-add')) - self.remove_topic_button.setIcon(QIcon.fromTheme('list-remove')) - self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - self.data_plot = None - - self.subscribe_topic_button.setEnabled(False) - if start_paused: - self.pause_button.setChecked(True) - - self._topic_completer = TopicCompleter(self.topic_edit) - self._topic_completer.update_topics() - self.topic_edit.setCompleter(self._topic_completer) - - self._start_time = rospy.get_time() - self._rosdata = {} - self._remove_topic_menu = QMenu() - - # init and start update timer for plot - self._update_plot_timer = QTimer(self) - self._update_plot_timer.timeout.connect(self.update_plot) - - def switch_data_plot_widget(self, data_plot): - self.enable_timer(enabled=False) - - self.data_plot_layout.removeWidget(self.data_plot) - if self.data_plot is not None: - self.data_plot.close() - - self.data_plot = data_plot - self.data_plot_layout.addWidget(self.data_plot) - self.data_plot.autoscroll(self.autoscroll_checkbox.isChecked()) - - # setup drag 'n drop - self.data_plot.dropEvent = self.dropEvent - self.data_plot.dragEnterEvent = self.dragEnterEvent - - if self._initial_topics: - for topic_name in self._initial_topics: - self.add_topic(topic_name) - self._initial_topics = None - else: - for topic_name, rosdata in self._rosdata.items(): - data_x, data_y = rosdata.next() - self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) - - self._subscribed_topics_changed() - - @Slot('QDragEnterEvent*') - def dragEnterEvent(self, event): - # get topic name - if not event.mimeData().hasText(): - if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0: - qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0') - return - item = event.source().selectedItems()[0] - topic_name = item.data(0, Qt.UserRole) - if topic_name == None: - qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)') - return - else: - topic_name = str(event.mimeData().text()) - - # check for plottable field type - plottable, message = is_plottable(topic_name) - if plottable: - event.acceptProposedAction() - else: - qWarning('Plot.dragEnterEvent(): rejecting: "%s"' % (message)) - - @Slot('QDropEvent*') - def dropEvent(self, event): - if event.mimeData().hasText(): - topic_name = str(event.mimeData().text()) - else: - droped_item = event.source().selectedItems()[0] - topic_name = str(droped_item.data(0, Qt.UserRole)) - self.add_topic(topic_name) - - @Slot(str) - def on_topic_edit_textChanged(self, topic_name): - # on empty topic name, update topics - if topic_name in ('', '/'): - self._topic_completer.update_topics() - - plottable, message = is_plottable(topic_name) - self.subscribe_topic_button.setEnabled(plottable) - self.subscribe_topic_button.setToolTip(message) - - @Slot() - def on_topic_edit_returnPressed(self): - if self.subscribe_topic_button.isEnabled(): - self.add_topic(str(self.topic_edit.text())) - - @Slot() - def on_subscribe_topic_button_clicked(self): - self.add_topic(str(self.topic_edit.text())) - - @Slot(bool) - def on_pause_button_clicked(self, checked): - self.enable_timer(not checked) - - @Slot(bool) - def on_autoscroll_checkbox_clicked(self, checked): - self.data_plot.autoscroll(checked) - if checked: - self.data_plot.redraw() - - @Slot() - def on_clear_button_clicked(self): - self.clear_plot() - - def update_plot(self): - if self.data_plot is not None: - needs_redraw = False - for topic_name, rosdata in self._rosdata.items(): - try: - data_x, data_y = rosdata.next() - if data_x or data_y: - self.data_plot.update_values(topic_name, data_x, data_y) - needs_redraw = True - except RosPlotException as e: - qWarning('PlotWidget.update_plot(): error in rosplot: %s' % e) - if needs_redraw: - self.data_plot.redraw() - - def _subscribed_topics_changed(self): - self._update_remove_topic_menu() - if not self.pause_button.isChecked(): - # if pause button is not pressed, enable timer based on subscribed topics - self.enable_timer(self._rosdata) - self.data_plot.redraw() - - def _update_remove_topic_menu(self): - def make_remove_topic_function(x): - return lambda: self.remove_topic(x) - - self._remove_topic_menu.clear() - for topic_name in sorted(self._rosdata.keys()): - action = QAction(topic_name, self._remove_topic_menu) - action.triggered.connect(make_remove_topic_function(topic_name)) - self._remove_topic_menu.addAction(action) - - if len(self._rosdata) > 1: - all_action = QAction('All', self._remove_topic_menu) - all_action.triggered.connect(self.clean_up_subscribers) - self._remove_topic_menu.addAction(all_action) - - self.remove_topic_button.setMenu(self._remove_topic_menu) - - def add_topic(self, topic_name): - topics_changed = False - for topic_name in get_plot_fields(topic_name)[0]: - if topic_name in self._rosdata: - qWarning('PlotWidget.add_topic(): topic already subscribed: %s' % topic_name) - continue - self._rosdata[topic_name] = ROSData(topic_name, self._start_time) - if self._rosdata[topic_name].error is not None: - qWarning(str(self._rosdata[topic_name].error)) - del self._rosdata[topic_name] - else: - data_x, data_y = self._rosdata[topic_name].next() - self.data_plot.add_curve(topic_name, topic_name, data_x, data_y) - topics_changed = True - - if topics_changed: - self._subscribed_topics_changed() - - def remove_topic(self, topic_name): - self._rosdata[topic_name].close() - del self._rosdata[topic_name] - self.data_plot.remove_curve(topic_name) - - self._subscribed_topics_changed() - - def clear_plot(self): - for topic_name, _ in self._rosdata.items(): - self.data_plot.clear_values(topic_name) - self.data_plot.redraw() - - def clean_up_subscribers(self): - for topic_name, rosdata in self._rosdata.items(): - rosdata.close() - self.data_plot.remove_curve(topic_name) - self._rosdata = {} - - self._subscribed_topics_changed() - - def enable_timer(self, enabled=True): - if enabled: - self._update_plot_timer.start(self._redraw_interval) - else: - self._update_plot_timer.stop() diff --git a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py b/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py deleted file mode 100644 index 922583e6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_plot/src/rqt_plot/rosplot.py +++ /dev/null @@ -1,201 +0,0 @@ -#!/usr/bin/env python -# -# Software License Agreement (BSD License) -# -# Copyright (c) 2009, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# - -import string -import sys -import threading -import time - -import rosgraph -import roslib.message -import roslib.names -import rospy - - -class RosPlotException(Exception): - pass - - -def _get_topic_type(topic): - """ - subroutine for getting the topic type - (nearly identical to rostopic._get_topic_type, except it returns rest of name instead of fn) - - :returns: topic type, real topic name, and rest of name referenced - if the topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` - """ - try: - master = rosgraph.Master('/rosplot') - val = master.getTopicTypes() - except: - raise RosPlotException("unable to get list of topics from master") - matches = [(t, t_type) for t, t_type in val if t == topic or topic.startswith(t + '/')] - if matches: - t, t_type = matches[0] - if t_type == roslib.names.ANYTYPE: - return None, None, None - if t_type == topic: - return t_type, None - return t_type, t, topic[len(t):] - else: - return None, None, None - - -def get_topic_type(topic): - """ - Get the topic type (nearly identical to rostopic.get_topic_type, except it doesn't return a fn) - - :returns: topic type, real topic name, and rest of name referenced - if the \a topic points to a field within a topic, e.g. /rosout/msg, ``str, str, str`` - """ - topic_type, real_topic, rest = _get_topic_type(topic) - if topic_type: - return topic_type, real_topic, rest - else: - return None, None, None - - -class ROSData(object): - """ - Subscriber to ROS topic that buffers incoming data - """ - - def __init__(self, topic, start_time): - self.name = topic - self.start_time = start_time - self.error = None - - self.lock = threading.Lock() - self.buff_x = [] - self.buff_y = [] - - topic_type, real_topic, fields = get_topic_type(topic) - if topic_type is not None: - self.field_evals = generate_field_evals(fields) - data_class = roslib.message.get_message_class(topic_type) - self.sub = rospy.Subscriber(real_topic, data_class, self._ros_cb) - else: - self.error = RosPlotException("Can not resolve topic type of %s" % topic) - - def close(self): - self.sub.unregister() - - def _ros_cb(self, msg): - """ - ROS subscriber callback - :param msg: ROS message data - """ - try: - self.lock.acquire() - try: - self.buff_y.append(self._get_data(msg)) - # #944: use message header time if present - if msg.__class__._has_header: - self.buff_x.append(msg.header.stamp.to_sec() - self.start_time) - else: - self.buff_x.append(rospy.get_time() - self.start_time) - #self.axes[index].plot(datax, buff_y) - except AttributeError, e: - self.error = RosPlotException("Invalid topic spec [%s]: %s" % (self.name, str(e))) - finally: - self.lock.release() - - def next(self): - """ - Get the next data in the series - - :returns: [xdata], [ydata] - """ - if self.error: - raise self.error - try: - self.lock.acquire() - buff_x = self.buff_x - buff_y = self.buff_y - self.buff_x = [] - self.buff_y = [] - finally: - self.lock.release() - return buff_x, buff_y - - def _get_data(self, msg): - val = msg - try: - if not self.field_evals: - return float(val) - for f in self.field_evals: - val = f(val) - return float(val) - except IndexError: - self.error = RosPlotException("[%s] index error for: %s" % (self.name, str(val).replace('\n', ', '))) - except TypeError: - self.error = RosPlotException("[%s] value was not numeric: %s" % (self.name, val)) - - -def _array_eval(field_name, slot_num): - """ - :param field_name: name of field to index into, ``str`` - :param slot_num: index of slot to return, ``str`` - :returns: fn(msg_field)->msg_field[slot_num] - """ - def fn(f): - return getattr(f, field_name).__getitem__(slot_num) - return fn - - -def _field_eval(field_name): - """ - :param field_name: name of field to return, ``str`` - :returns: fn(msg_field)->msg_field.field_name - """ - def fn(f): - return getattr(f, field_name) - return fn - - -def generate_field_evals(fields): - try: - evals = [] - fields = [f for f in fields.split('/') if f] - for f in fields: - if '[' in f: - field_name, rest = f.split('[') - slot_num = string.atoi(rest[:rest.find(']')]) - evals.append(_array_eval(field_name, slot_num)) - else: - evals.append(_field_eval(f)) - return evals - except Exception, e: - raise RosPlotException("cannot parse field reference [%s]: %s" % (fields, str(e))) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst deleted file mode 100644 index 0fed08e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/CHANGELOG.rst +++ /dev/null @@ -1,110 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_publisher -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* fixed rqt_publisher plugin to fill message slots for individual fields of primitive arrays -* use proper icon names for add/remove -* Contributors: Robert Haschke, Vincent Rabaud - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix compatibility with Groovy, use queue_size for Python publishers only when available (`#243 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* use queue_size for Python publishers - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- -* fix regression of 0.3.1 (rospack not defined) - -0.3.1 (2013-10-09) ------------------- -* improve performance to fill combo box with message types (`#177 `_) - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt deleted file mode 100644 index d79f80f4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_publisher) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_publisher - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox deleted file mode 100644 index a22054d9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_publisher provides a ROS plugin for publishing arbitrary messages with fixed or computed field values. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/package.xml b/workspace/src/rqt_common_plugins/rqt_publisher/package.xml deleted file mode 100644 index 7301849e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/package.xml +++ /dev/null @@ -1,29 +0,0 @@ - - rqt_publisher - 0.3.13 - rqt_publisher provides a GUI plugin for publishing arbitrary messages with fixed or computed field values. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_publisher - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui_py_common - roslib - rosmsg - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml b/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml deleted file mode 100644 index d17cf61e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin publishing ROS messages. - - - - - folder - Plugins related to ROS topics. - - - media-playback-start - A Python GUI plugin for publishing ROS messages. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui b/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui deleted file mode 100644 index 091192a3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/resource/Publisher.ui +++ /dev/null @@ -1,220 +0,0 @@ - - - PublisherWidget - - - - 0 - 0 - 739 - 406 - - - - Message Publisher - - - - - - - - Refresh topics and types - - - - - - - - - - - 0 - 0 - - - - Topic - - - - - - - - 1 - 0 - - - - - 100 - 0 - - - - Topic name to publish on - - - true - - - QComboBox::AdjustToMinimumContentsLength - - - - - - - Type - - - - - - - - 2 - 0 - - - - - 100 - 0 - - - - Message type to publish - - - true - - - QComboBox::AdjustToMinimumContentsLength - - - - - - - Freq. - - - - - - - - 60 - 0 - - - - Frequency for periodic publishers in Hz (set to 0 for manual publishing) - - - true - - - - 1 - - - - - - - - Hz - - - - - - - Add new publisher - - - - - - - - - - Remove selected publisher - - - - - - Del - - - - - - - Qt::Horizontal - - - - 20 - 20 - - - - - - - - Clear all publishers - - - - - - - - - - - - Qt::CustomContextMenu - - - Right click on item or selection for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - true - - - - - - - - ExtendedComboBox - QComboBox -
rqt_py_common.extended_combo_box
- - - PublisherTreeWidget - QTreeView -
rqt_publisher.publisher_tree_widget
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher b/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher deleted file mode 100755 index 6a27309c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/scripts/rqt_publisher +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_publisher.publisher.Publisher')) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/setup.py b/workspace/src/rqt_common_plugins/rqt_publisher/setup.py deleted file mode 100644 index 61d0fe22..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_publisher'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/__init__.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py deleted file mode 100644 index 60a02c33..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher.py +++ /dev/null @@ -1,342 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import math -import random -import time - -from python_qt_binding.QtCore import Slot, QSignalMapper, QTimer, qWarning - -import roslib -import rospy -import genpy -from rqt_gui_py.plugin import Plugin -from .publisher_widget import PublisherWidget -from rqt_py_common.topic_helpers import get_field_type - - -class Publisher(Plugin): - - def __init__(self, context): - super(Publisher, self).__init__(context) - self.setObjectName('Publisher') - - # create widget - self._widget = PublisherWidget() - self._widget.add_publisher.connect(self.add_publisher) - self._widget.change_publisher.connect(self.change_publisher) - self._widget.publish_once.connect(self.publish_once) - self._widget.remove_publisher.connect(self.remove_publisher) - self._widget.clean_up_publishers.connect(self.clean_up_publishers) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - - # create context for the expression eval statement - self._eval_locals = {'i': 0} - for module in (math, random, time): - self._eval_locals.update(module.__dict__) - self._eval_locals['genpy'] = genpy - del self._eval_locals['__name__'] - del self._eval_locals['__doc__'] - - self._publishers = {} - self._id_counter = 0 - - self._timeout_mapper = QSignalMapper(self) - self._timeout_mapper.mapped[int].connect(self.publish_once) - - # add our self to the main window - context.add_widget(self._widget) - - @Slot(str, str, float, bool) - def add_publisher(self, topic_name, type_name, rate, enabled): - publisher_info = { - 'topic_name': str(topic_name), - 'type_name': str(type_name), - 'rate': float(rate), - 'enabled': bool(enabled), - } - self._add_publisher(publisher_info) - - def _add_publisher(self, publisher_info): - publisher_info['publisher_id'] = self._id_counter - self._id_counter += 1 - publisher_info['counter'] = 0 - publisher_info['enabled'] = publisher_info.get('enabled', False) - publisher_info['expressions'] = publisher_info.get('expressions', {}) - - publisher_info['message_instance'] = self._create_message_instance(publisher_info['type_name']) - if publisher_info['message_instance'] is None: - return - - # create publisher and timer - try: - publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance']), queue_size=100) - except TypeError: - publisher_info['publisher'] = rospy.Publisher(publisher_info['topic_name'], type(publisher_info['message_instance'])) - publisher_info['timer'] = QTimer(self) - - # add publisher info to _publishers dict and create signal mapping - self._publishers[publisher_info['publisher_id']] = publisher_info - self._timeout_mapper.setMapping(publisher_info['timer'], publisher_info['publisher_id']) - publisher_info['timer'].timeout.connect(self._timeout_mapper.map) - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - - self._widget.publisher_tree_widget.model().add_publisher(publisher_info) - - @Slot(int, str, str, str, object) - def change_publisher(self, publisher_id, topic_name, column_name, new_value, setter_callback): - handler = getattr(self, '_change_publisher_%s' % column_name, None) - if handler is not None: - new_text = handler(self._publishers[publisher_id], topic_name, new_value) - if new_text is not None: - setter_callback(new_text) - - def _change_publisher_topic(self, publisher_info, topic_name, new_value): - publisher_info['enabled'] = (new_value and new_value.lower() in ['1', 'true', 'yes']) - #qDebug('Publisher._change_publisher_enabled(): %s enabled: %s' % (publisher_info['topic_name'], publisher_info['enabled'])) - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - else: - publisher_info['timer'].stop() - return None - - def _change_publisher_type(self, publisher_info, topic_name, new_value): - type_name = new_value - # create new slot - slot_value = self._create_message_instance(type_name) - - # find parent slot - slot_path = topic_name[len(publisher_info['topic_name']):].strip('/').split('/') - parent_slot = eval('.'.join(["publisher_info['message_instance']"] + slot_path[:-1])) - - # find old slot - slot_name = slot_path[-1] - slot_index = parent_slot.__slots__.index(slot_name) - - # restore type if user value was invalid - if slot_value is None: - qWarning('Publisher._change_publisher_type(): could not find type: %s' % (type_name)) - return parent_slot._slot_types[slot_index] - - else: - # replace old slot - parent_slot._slot_types[slot_index] = type_name - setattr(parent_slot, slot_name, slot_value) - - self._widget.publisher_tree_widget.model().update_publisher(publisher_info) - - def _change_publisher_rate(self, publisher_info, topic_name, new_value): - try: - rate = float(new_value) - except Exception: - qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value)) - else: - publisher_info['rate'] = rate - #qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate'])) - publisher_info['timer'].stop() - if publisher_info['enabled'] and publisher_info['rate'] > 0: - publisher_info['timer'].start(int(1000.0 / publisher_info['rate'])) - # make sure the column value reflects the actual rate - return '%.2f' % publisher_info['rate'] - - def _change_publisher_expression(self, publisher_info, topic_name, new_value): - expression = str(new_value) - if len(expression) == 0: - if topic_name in publisher_info['expressions']: - del publisher_info['expressions'][topic_name] - #qDebug('Publisher._change_publisher_expression(): removed expression for: %s' % (topic_name)) - else: - slot_type, is_array = get_field_type(topic_name) - if is_array: - slot_type = list - # strip possible trailing error message from expression - error_prefix = '# error' - error_prefix_pos = expression.find(error_prefix) - if error_prefix_pos >= 0: - expression = expression[:error_prefix_pos] - success, _ = self._evaluate_expression(expression, slot_type) - if success: - old_expression = publisher_info['expressions'].get(topic_name, None) - publisher_info['expressions'][topic_name] = expression - #print 'Publisher._change_publisher_expression(): topic: %s, type: %s, expression: %s' % (topic_name, slot_type, new_value) - self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter']) - try: - publisher_info['message_instance']._check_types() - except Exception, e: - error_str = str(e) - print 'serialization error:', error_str - if old_expression is not None: - publisher_info['expressions'][topic_name] = old_expression - else: - del publisher_info['expressions'][topic_name] - return '%s %s: %s' % (expression, error_prefix, error_str) - return expression - else: - return '%s %s evaluating as "%s"' % (expression, error_prefix, slot_type.__name__) - - def _extract_array_info(self, type_str): - array_size = None - if '[' in type_str and type_str[-1] == ']': - type_str, array_size_str = type_str.split('[', 1) - array_size_str = array_size_str[:-1] - if len(array_size_str) > 0: - array_size = int(array_size_str) - else: - array_size = 0 - - return type_str, array_size - - def _create_message_instance(self, type_str): - base_type_str, array_size = self._extract_array_info(type_str) - - base_message_type = roslib.message.get_message_class(base_type_str) - if base_message_type is None: - print 'Could not create message of type "%s".' % base_type_str - return None - - if array_size is not None: - message = [] - for _ in range(array_size): - message.append(base_message_type()) - else: - message = base_message_type() - return message - - def _evaluate_expression(self, expression, slot_type): - successful_eval = True - - try: - # try to evaluate expression - value = eval(expression, {}, self._eval_locals) - except Exception: - successful_eval = False - - if slot_type is str: - if successful_eval: - value = str(value) - else: - # for string slots just convert the expression to str, if it did not evaluate successfully - value = str(expression) - successful_eval = True - - elif successful_eval: - type_set = set((slot_type, type(value))) - # check if value's type and slot_type belong to the same type group, i.e. array types, numeric types - # and if they do, make sure values's type is converted to the exact slot_type - if type_set <= set((list, tuple)) or type_set <= set((int, float)): - # convert to the right type - value = slot_type(value) - - if successful_eval and isinstance(value, slot_type): - return True, value - else: - qWarning('Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"' % (expression, slot_type.__name__)) - - return False, None - - def _fill_message_slots(self, message, topic_name, expressions, counter): - if topic_name in expressions and len(expressions[topic_name]) > 0: - - # get type - if hasattr(message, '_type'): - message_type = message._type - else: - message_type = type(message) - - self._eval_locals['i'] = counter - success, value = self._evaluate_expression(expressions[topic_name], message_type) - if not success: - value = message_type() - return value - - # if no expression exists for this topic_name, continue with it's child slots - elif hasattr(message, '__slots__'): - for slot_name in message.__slots__: - value = self._fill_message_slots(getattr(message, slot_name), topic_name + '/' + slot_name, expressions, counter) - if value is not None: - setattr(message, slot_name, value) - - elif type(message) in (list, tuple) and (len(message) > 0): - for index, slot in enumerate(message): - value = self._fill_message_slots(slot, topic_name + '[%d]' % index, expressions, counter) - # this deals with primitive-type arrays - if not hasattr(message[0], '__slots__') and value is not None: - message[index] = value - - return None - - @Slot(int) - def publish_once(self, publisher_id): - publisher_info = self._publishers.get(publisher_id, None) - if publisher_info is not None: - publisher_info['counter'] += 1 - self._fill_message_slots(publisher_info['message_instance'], publisher_info['topic_name'], publisher_info['expressions'], publisher_info['counter']) - publisher_info['publisher'].publish(publisher_info['message_instance']) - - @Slot(int) - def remove_publisher(self, publisher_id): - publisher_info = self._publishers.get(publisher_id, None) - if publisher_info is not None: - publisher_info['timer'].stop() - publisher_info['publisher'].unregister() - del self._publishers[publisher_id] - - def save_settings(self, plugin_settings, instance_settings): - publisher_copies = [] - for publisher in self._publishers.values(): - publisher_copy = {} - publisher_copy.update(publisher) - publisher_copy['enabled'] = False - del publisher_copy['timer'] - del publisher_copy['message_instance'] - del publisher_copy['publisher'] - publisher_copies.append(publisher_copy) - instance_settings.set_value('publishers', repr(publisher_copies)) - - def restore_settings(self, plugin_settings, instance_settings): - publishers = eval(instance_settings.value('publishers', '[]')) - for publisher in publishers: - self._add_publisher(publisher) - - def clean_up_publishers(self): - self._widget.publisher_tree_widget.model().clear() - for publisher_info in self._publishers.values(): - publisher_info['timer'].stop() - publisher_info['publisher'].unregister() - self._publishers = {} - - def shutdown_plugin(self): - self._widget.shutdown_plugin() - self.clean_up_publishers() diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py deleted file mode 100644 index 86cb18cc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_model.py +++ /dev/null @@ -1,128 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -import threading - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QStandardItem - -from rqt_py_common.message_tree_model import MessageTreeModel -from rqt_py_common.data_items import ReadonlyItem, CheckableItem - - -class PublisherTreeModel(MessageTreeModel): - _column_names = ['topic', 'type', 'rate', 'expression'] - item_value_changed = Signal(int, str, str, str, object) - - def __init__(self, parent=None): - super(PublisherTreeModel, self).__init__(parent) - self._column_index = {} - for column_name in self._column_names: - self._column_index[column_name] = len(self._column_index) - self.clear() - - self._item_change_lock = threading.Lock() - self.itemChanged.connect(self.handle_item_changed) - - def clear(self): - super(PublisherTreeModel, self).clear() - self.setHorizontalHeaderLabels(self._column_names) - - def get_publisher_ids(self, index_list): - return [item._user_data['publisher_id'] for item in self._get_toplevel_items(index_list)] - - def remove_items_with_parents(self, index_list): - for item in self._get_toplevel_items(index_list): - self.removeRow(item.row()) - - def handle_item_changed(self, item): - if not self._item_change_lock.acquire(False): - #qDebug('PublisherTreeModel.handle_item_changed(): could not acquire lock') - return - # lock has been acquired - topic_name = item._path - column_name = self._column_names[item.column()] - if item.isCheckable(): - new_value = str(item.checkState() == Qt.Checked) - else: - new_value = item.text().strip() - #print 'PublisherTreeModel.handle_item_changed(): %s, %s, %s' % (topic_name, column_name, new_value) - - self.item_value_changed.emit(item._user_data['publisher_id'], topic_name, column_name, new_value, item.setText) - - # release lock - self._item_change_lock.release() - - def remove_publisher(self, publisher_id): - for top_level_row_number in range(self.rowCount()): - item = self.item(top_level_row_number) - if item is not None and item._user_data['publisher_id'] == publisher_id: - self.removeRow(top_level_row_number) - return top_level_row_number - return None - - def update_publisher(self, publisher_info): - top_level_row_number = self.remove_publisher(publisher_info['publisher_id']) - self.add_publisher(publisher_info, top_level_row_number) - - def add_publisher(self, publisher_info, top_level_row_number=None): - # recursively create widget items for the message's slots - parent = self - slot = publisher_info['message_instance'] - slot_name = publisher_info['topic_name'] - slot_type_name = publisher_info['message_instance']._type - slot_path = publisher_info['topic_name'] - user_data = {'publisher_id': publisher_info['publisher_id']} - kwargs = { - 'user_data': user_data, - 'top_level_row_number': top_level_row_number, - 'expressions': publisher_info['expressions'], - } - top_level_row = self._recursive_create_items(parent, slot, slot_name, slot_type_name, slot_path, **kwargs) - - # fill tree widget columns of top level item - if publisher_info['enabled']: - top_level_row[self._column_index['topic']].setCheckState(Qt.Checked) - top_level_row[self._column_index['rate']].setText(str(publisher_info['rate'])) - - def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path, **kwargs): - if slot_name.startswith('/'): - return (CheckableItem(slot_name), ReadonlyItem(slot_type_name), QStandardItem(''), ReadonlyItem('')) - expression_item = QStandardItem('') - expression_item.setToolTip('enter valid Python expression here, using "i" as counter and functions from math, random and time modules') - return (ReadonlyItem(slot_name), QStandardItem(slot_type_name), ReadonlyItem(''), expression_item) - - def _recursive_create_items(self, parent, slot, slot_name, slot_type_name, slot_path, expressions={}, **kwargs): - row, is_leaf_node = super(PublisherTreeModel, self)._recursive_create_items(parent, slot, slot_name, slot_type_name, slot_path, expressions=expressions, **kwargs) - if is_leaf_node: - expression_text = expressions.get(slot_path, repr(slot)) - row[self._column_index['expression']].setText(expression_text) - return row diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py deleted file mode 100644 index 9bb3c479..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_tree_widget.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Signal, Slot -from python_qt_binding.QtGui import QAction, QIcon - -from .publisher_tree_model import PublisherTreeModel -from rqt_py_common.message_tree_widget import MessageTreeWidget -from rqt_py_common.item_delegates import SpinBoxDelegate - - -class PublisherTreeWidget(MessageTreeWidget): - remove_publisher = Signal(int) - publish_once = Signal(int) - - def __init__(self, parent=None): - super(PublisherTreeWidget, self).__init__(parent) - self.setModel(PublisherTreeModel(self)) - self._action_remove_publisher = QAction(QIcon.fromTheme('list-remove'), 'Remove Selected', self) - self._action_remove_publisher.triggered[bool].connect(self._handle_action_remove_publisher) - self._action_publish_once = QAction(QIcon.fromTheme('media-playback-start'), 'Publish Selected Once', self) - self._action_publish_once.triggered[bool].connect(self._handle_action_publish_once) - self.setItemDelegateForColumn(self.model()._column_index['rate'], SpinBoxDelegate(min_value=0, max_value=1000000, decimals=2)) - - @Slot() - def remove_selected_publishers(self): - publisher_ids = self.model().get_publisher_ids(self.selectedIndexes()) - for publisher_id in publisher_ids: - self.remove_publisher.emit(publisher_id) - self.model().remove_items_with_parents(self.selectedIndexes()) - - def _context_menu_add_actions(self, menu, pos): - if self.selectionModel().hasSelection(): - menu.addAction(self._action_remove_publisher) - menu.addAction(self._action_publish_once) - # let super class add actions - super(PublisherTreeWidget, self)._context_menu_add_actions(menu, pos) - - def _handle_action_remove_publisher(self, checked): - self.remove_selected_publishers() - - def _handle_action_publish_once(self, checked): - for publisher_id in self.model().get_publisher_ids(self.selectedIndexes()): - self.publish_once.emit(publisher_id) diff --git a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py b/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py deleted file mode 100644 index 686d237e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_publisher/src/rqt_publisher/publisher_widget.py +++ /dev/null @@ -1,131 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Signal, Slot -from python_qt_binding.QtGui import QIcon, QWidget - -import roslib -import rosmsg -import rospkg -import rospy - -from qt_gui_py_common.worker_thread import WorkerThread -from rqt_py_common.extended_combo_box import ExtendedComboBox -from .publisher_tree_widget import PublisherTreeWidget - - -# main class inherits from the ui window class -class PublisherWidget(QWidget): - add_publisher = Signal(str, str, float, bool) - change_publisher = Signal(int, str, str, str, object) - publish_once = Signal(int) - remove_publisher = Signal(int) - clean_up_publishers = Signal() - - def __init__(self, parent=None): - super(PublisherWidget, self).__init__(parent) - self._topic_dict = {} - self._update_thread = WorkerThread(self._update_thread_run, self._update_finished) - - self._rospack = rospkg.RosPack() - ui_file = os.path.join(self._rospack.get_path('rqt_publisher'), 'resource', 'Publisher.ui') - loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox, 'PublisherTreeWidget': PublisherTreeWidget}) - self.refresh_button.setIcon(QIcon.fromTheme('view-refresh')) - self.refresh_button.clicked.connect(self.refresh_combo_boxes) - self.add_publisher_button.setIcon(QIcon.fromTheme('list-add')) - self.remove_publisher_button.setIcon(QIcon.fromTheme('list-remove')) - self.clear_button.setIcon(QIcon.fromTheme('edit-clear')) - - self.refresh_combo_boxes() - - self.publisher_tree_widget.model().item_value_changed.connect(self.change_publisher) - self.publisher_tree_widget.remove_publisher.connect(self.remove_publisher) - self.publisher_tree_widget.publish_once.connect(self.publish_once) - self.remove_publisher_button.clicked.connect(self.publisher_tree_widget.remove_selected_publishers) - self.clear_button.clicked.connect(self.clean_up_publishers) - - def shutdown_plugin(self): - self._update_thread.kill() - - @Slot() - def refresh_combo_boxes(self): - self._update_thread.kill() - self.type_combo_box.setEnabled(False) - self.topic_combo_box.setEnabled(False) - self.type_combo_box.setEditText('updating...') - self.topic_combo_box.setEditText('updating...') - self._update_thread.start() - - # this runs in a non-gui thread, so don't access widgets here directly - def _update_thread_run(self): - # update type_combo_box - message_type_names = [] - try: - # this only works on fuerte and up - packages = sorted([pkg_tuple[0] for pkg_tuple in rosmsg.iterate_packages(self._rospack, rosmsg.MODE_MSG)]) - except: - # this works up to electric - packages = sorted(rosmsg.list_packages()) - for package in packages: - for base_type_str in rosmsg.list_msgs(package, rospack=self._rospack): - message_class = roslib.message.get_message_class(base_type_str) - if message_class is not None: - message_type_names.append(base_type_str) - - self.type_combo_box.setItems.emit(sorted(message_type_names)) - - # update topic_combo_box - _, _, topic_types = rospy.get_master().getTopicTypes() - self._topic_dict = dict(topic_types) - self.topic_combo_box.setItems.emit(sorted(self._topic_dict.keys())) - - @Slot() - def _update_finished(self): - self.type_combo_box.setEnabled(True) - self.topic_combo_box.setEnabled(True) - - @Slot(str) - def on_topic_combo_box_currentIndexChanged(self, topic_name): - if topic_name in self._topic_dict: - self.type_combo_box.setEditText(self._topic_dict[topic_name]) - - @Slot() - def on_add_publisher_button_clicked(self): - topic_name = str(self.topic_combo_box.currentText()) - type_name = str(self.type_combo_box.currentText()) - rate = float(self.frequency_combo_box.currentText()) - enabled = False - self.add_publisher.emit(topic_name, type_name, rate, enabled) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst deleted file mode 100644 index 97d29b38..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/CHANGELOG.rst +++ /dev/null @@ -1,108 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_py_common -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* added unittest for MessageTreeModel -* fixed message_tree_model to list individual elements of simple-type array elements -* fixed topic_completer to handle array subscriptions -* Contributors: Robert Haschke - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* improve topic helpers to make more things plottable (`#246 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* fix ui loading of plugin constainer widget - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Add common system messaging pane widget - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt deleted file mode 100644 index f40947d9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_py_common) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_python_setup() - -if(CATKIN_ENABLE_TESTING) - find_package(genmsg REQUIRED) - find_package(std_msgs REQUIRED) - add_message_files( - DIRECTORY test/msg - FILES ArrayVal.msg Val.msg - NOINSTALL - ) - generate_messages(DEPENDENCIES std_msgs) -endif() - -catkin_package() - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -if(CATKIN_ENABLE_TESTING) - catkin_add_nosetests(test) -endif() \ No newline at end of file diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/package.xml b/workspace/src/rqt_common_plugins/rqt_py_common/package.xml deleted file mode 100644 index 6a1deb7b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/package.xml +++ /dev/null @@ -1,42 +0,0 @@ - - rqt_py_common - 0.3.13 - - rqt_py_common provides common functionality for rqt plugins written in Python. - Despite no plugin is provided, this package is part of the rqt_common_plugins - repository to keep refactoring generic functionality from these common plugins - into this package as easy as possible. - - Functionality included in this package should cover generic ROS concepts and - should not introduce any special dependencies beside "ros_base". - - Dorian Scholz - BSD - - http://ros.org/wiki/rqt_py_common - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - Dorian Scholz - Isaac Saito - - catkin - - genmsg - std_msgs - - genpy - qt_gui - roslib - rospy - rostopic - - - actionlib - rosbag - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui b/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui deleted file mode 100644 index 555f7549..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/resource/plugin_container.ui +++ /dev/null @@ -1,78 +0,0 @@ - - - _plugincontainer_top_widget - - - - 0 - 0 - 658 - 523 - - - - - 200 - 250 - - - - PluginContainer Widget - - - - 0 - - - 0 - - - - - Qt::Vertical - - - 9 - - - - - QLayout::SetMaximumSize - - - - - - 200 - 75 - - - - <!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.0//EN" "http://www.w3.org/TR/REC-html40/strict.dtd"> -<html><head><meta name="qrichtext" content="1" /><style type="text/css"> -p, li { white-space: pre-wrap; } -</style></head><body style=" font-family:'Sans Serif'; font-size:9pt; font-weight:400; font-style:normal;"> -<p style=" margin-top:0px; margin-bottom:0px; margin-left:0px; margin-right:0px; -qt-block-indent:0; text-indent:0px;">(System message might be shown here when necessary)</p></body></html> - - - - - - - - 200 - 30 - - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/setup.py b/workspace/src/rqt_common_plugins/rqt_py_common/setup.py deleted file mode 100644 index 204762c5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_py_common'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/__init__.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py deleted file mode 100644 index abe7ef31..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/data_items.py +++ /dev/null @@ -1,45 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QStandardItem - - -class ReadonlyItem(QStandardItem): - def __init__(self, *args): - super(ReadonlyItem, self).__init__(*args) - self.setEditable(False) - - -class CheckableItem(ReadonlyItem): - def __init__(self, *args): - super(CheckableItem, self).__init__(*args) - self.setCheckable(True) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py deleted file mode 100644 index 48de4d9b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/extended_combo_box.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Qt, Signal, Slot -from python_qt_binding.QtGui import QComboBox, QCompleter, QSortFilterProxyModel - - -class ExtendedComboBox(QComboBox): - setItems = Signal(list) - - def __init__(self, parent=None): - super(ExtendedComboBox, self).__init__(parent) - - self.setFocusPolicy(Qt.StrongFocus) - self.setEditable(True) - - # add a filter model to filter matching items - self.filter_model = QSortFilterProxyModel(self) - self.filter_model.setFilterCaseSensitivity(Qt.CaseInsensitive) - self.filter_model.setSourceModel(self.model()) - - # add a completer, which uses the filter model - self.completer = QCompleter(self.filter_model, self) - # always show all (filtered) completions - self.completer.setCompletionMode(QCompleter.UnfilteredPopupCompletion) - self.setCompleter(self.completer) - - # connect signals - self.lineEdit().textEdited[unicode].connect(self.filter_model.setFilterFixedString) - self.completer.activated.connect(self.on_completer_activated) - self.setItems.connect(self.onSetItems) - - # on selection of an item from the completer, select the corresponding item from combobox - def on_completer_activated(self, text): - if text: - index = self.findText(text) - self.setCurrentIndex(index) - - # on model change, update the models of the filter and completer as well - def setModel(self, model): - super(ExtendedComboBox, self).setModel(model) - self.filter_model.setSourceModel(model) - self.completer.setModel(self.filter_model) - - # on model column change, update the model column of the filter and completer as well - def setModelColumn(self, column): - self.completer.setCompletionColumn(column) - self.filter_model.setFilterKeyColumn(column) - super(ExtendedComboBox, self).setModelColumn(column) - - @Slot(list) - def onSetItems(self, items): - self.clear() - self.addItems(items) - - -if __name__ == "__main__": - import sys - from python_qt_binding.QtGui import QApplication - - app = QApplication(sys.argv) - - string_list = ['hola muchachos', 'adios amigos', 'hello world', 'good bye'] - - combo = ExtendedComboBox() - - # either fill the standard model of the combobox - combo.addItems(string_list) - - # or use another model - #combo.setModel(QStringListModel(string_list)) - - combo.resize(300, 40) - combo.show() - - sys.exit(app.exec_()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py deleted file mode 100644 index 3cf40f1e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/ini_helper.py +++ /dev/null @@ -1,66 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - - -def pack(data): - """ - Packs 'data' into a form that can be easily and readably written to an ini file - :param data: A list of strings or a list of items with a 'text' method to be flattened into a string ''list'' - :return: A string suitable for output to ini files ''str'' - """ - if len(data) == 0: - return '' - - def _get_str(item): - try: - return item.text() - except: - return item - - data = [_get_str(value) for value in data] - if len(data) == 1: - return data[0] - return data - - -def unpack(data): - """ - Unpacks the values read from an ini file - :param data: An entry taken from an ini file ''list or string'' - :return: A list of strings ''list'' - """ - if data is None or data == '': - data = [] - elif isinstance(data, basestring): - data = [data] - return data - diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py deleted file mode 100644 index 478933e1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/item_delegates.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import QModelIndex -from python_qt_binding.QtGui import QDoubleSpinBox, QItemDelegate - - -class SpinBoxDelegate(QItemDelegate): - def __init__(self, min_value=0, max_value=100, decimals=2, *args): - self._min = min_value - self._max = max_value - self._decimals = decimals - super(SpinBoxDelegate, self).__init__(*args) - - def createEditor(self, parent, option, index): - editor = QDoubleSpinBox(parent) - editor.setDecimals(self._decimals) - editor.setMaximum(self._min) - editor.setMaximum(self._max) - return editor - - -class DelegateUtil(object): - """ - Find out the hierarchy level of the selected item. - see: http://stackoverflow.com/a/4208240/577001 - - :type model_index: QModelIndex - :rtype: int - - :author: Isaac Saito - """ - @staticmethod - def _get_hierarchy_level(model_index): - hierarchyLevel = 1 - seek_root = model_index - while(seek_root.parent() != QModelIndex()): - seek_root = seek_root.parent() - hierarchyLevel += 1 - return hierarchyLevel diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py deleted file mode 100644 index 6a38c5e7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/layout_util.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QWidgetItem -import roslib -import rospy - - -class LayoutUtil(object): - - @staticmethod - def alternate_color(list_widgets, colors_alter=[Qt.white, Qt.gray]): - """ - Alternate the background color of the widgets that are ordered - linearly, by the given list of colors. - - Originally intended for the elements of QHBoxLayout & QVBoxLayout. - - @type list_widgets: QtGui.QWidget[] - @type colors_alter: QtCore.Qt.GlobalColor[] - @param colors_alter: 1st element is used as initial/default color. - @rtype: void - - @author: Isaac Saito - """ - - colors_num = len(colors_alter) - i_widget = 0 - for w in list_widgets: - w.setAutoFillBackground(True) - p = w.palette() - - divisor = (i_widget + colors_num) % colors_num - i_widget += 1 - - rospy.logdebug('LayoutUtil divisor={} i_widget={} colors_num={}'.format( - divisor, - i_widget, - colors_num)) - - p.setColor(w.backgroundRole(), colors_alter[divisor]) - w.setPalette(p) - - @staticmethod - def clear_layout(layout): - """ - Clear all items in the given layout. Currently, only the instances of - QWidgetItem get cleared (ie. QSpaceItem is ignored). - - Originally taken from http://stackoverflow.com/a/9375273/577001 - - :type layout: QLayout - """ - for i in reversed(range(layout.count())): - item = layout.itemAt(i) - - if isinstance(item, QWidgetItem): - # print "widget" + str(item) - item.widget().close() - # or - # item.widget().setParent(None) - elif isinstance(item, QSpacerItem): - # print "spacer " + str(item) - continue - # no need to do extra stuff - else: - # print "layout " + str(item) - LayoutUtil.clear_layout(item.layout()) - - # remove the item from layout - layout.removeItem(item) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py deleted file mode 100644 index 3d6638f3..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_model.py +++ /dev/null @@ -1,137 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rospy -from python_qt_binding.QtGui import QStandardItem, QStandardItemModel -from .data_items import ReadonlyItem - - -class MessageTreeModel(QStandardItemModel): - - def __init__(self, parent=None): - # FIXME: why is this not working? should be the same as the following line... - #super(MessageTreeModel, self).__init__(parent) - QStandardItemModel.__init__(self, parent) - - def add_message(self, message_instance, message_name='', message_type='', message_path=''): - if message_instance is None: - return - self._recursive_create_items(self, message_instance, message_name, message_type, message_path) - - def _get_toplevel_items(self, index_list): - items = [self.itemFromIndex(index) for index in index_list] - uniqueItems = {} - for item in items: - while item.parent() is not None: - item = item.parent() - if item.row() not in uniqueItems: - uniqueItems[item.row()] = item - return uniqueItems.values() - - def _get_data_items_for_path(self, slot_name, slot_type_name, slot_path, **kwargs): - return (QStandardItem(slot_name), QStandardItem(slot_type_name), QStandardItem(slot_path)) - - def _recursive_create_items(self, parent, slot, slot_name, slot_type_name, slot_path, **kwargs): - row = [] - for item in self._get_data_items_for_path(slot_name, slot_type_name, slot_path, **kwargs): - item._path = slot_path - item._user_data = kwargs.get('user_data', None) - row.append(item) - - is_leaf_node = False - if hasattr(slot, '__slots__') and hasattr(slot, '_slot_types'): - for child_slot_name, child_slot_type in zip(slot.__slots__, slot._slot_types): - child_slot_path = slot_path + '/' + child_slot_name - child_slot = getattr(slot, child_slot_name) - self._recursive_create_items(row[0], child_slot, child_slot_name, child_slot_type, child_slot_path, **kwargs) - - elif type(slot) in (list, tuple) and (len(slot) > 0): - child_slot_type = slot_type_name[:slot_type_name.find('[')] - for index, child_slot in enumerate(slot): - child_slot_name = '[%d]' % index - child_slot_path = slot_path + child_slot_name - self._recursive_create_items(row[0], child_slot, child_slot_name, child_slot_type, child_slot_path, **kwargs) - - else: - is_leaf_node = True - - if parent is self and kwargs.get('top_level_row_number', None) is not None: - parent.insertRow(kwargs['top_level_row_number'], row) - else: - parent.appendRow(row) - - return (row, is_leaf_node) - - ''' - NOTE: I (Isaac Saito) suspect that this function might have same/similar - functionality with _recursive_create_items. - - @summary: Evaluate current node and the previous node on the same depth. - If the name of both nodes at the same depth is the same, - current name is added to the previous node. - If not, the current name gets added to the parent node. - At the end, this function calls itself recursively going down - 1 level deeper. - @param stditem_parent: QStandardItem. - @param names_on_branch: List of strings each of which - represents the name of the node. - Ex. If you have a hierarchy that looks like: - - /top_node/sub_node/subsub_node - - then this list would look like: - - [ top_node, sub_node, subsub_node ] - @author: Isaac Saito - ''' - @staticmethod - def _build_tree_recursive(stditem_parent, names_on_branch): - name_curr = names_on_branch.pop(0) - stditem_curr = ReadonlyItem(name_curr) - - row_index_parent = stditem_parent.rowCount() - 1 - # item at the bottom is your most recent node. - - name_prev = '' - stditem_prev = None - if not stditem_parent.child(row_index_parent) == None: - stditem_prev = stditem_parent.child(row_index_parent) - name_prev = stditem_prev.text() - - stditem = None - if not name_prev == name_curr: - stditem_parent.appendRow(stditem_curr) - stditem = stditem_curr - else: - stditem = stditem_prev - - rospy.logdebug('add_tree_node 1 name_curr=%s ' + '\n\t\t\t\t\tname_prev=%s row_index_parent=%d', name_curr, name_prev, row_index_parent) - if (0 < len(names_on_branch)): - MessageTreeModel._build_tree_recursive(stditem, names_on_branch) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py deleted file mode 100644 index 28919793..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/message_tree_widget.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtCore import Slot, QMimeData, QModelIndex, Qt, qWarning -from python_qt_binding.QtGui import QAction, QDrag, QHeaderView, QIcon, QMenu, QTreeView - - -class MessageTreeWidget(QTreeView): - - def __init__(self, parent=None): - super(MessageTreeWidget, self).__init__(parent) - self.setDragEnabled(True) - self.sortByColumn(0, Qt.AscendingOrder) - - self.header().setResizeMode(QHeaderView.ResizeToContents) - self.header().setContextMenuPolicy(Qt.CustomContextMenu) - self.header().customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested) - - self._action_item_expand = QAction(QIcon.fromTheme('zoom-in'), 'Expand Selected', self) - self._action_item_expand.triggered.connect(self._handle_action_item_expand) - self._action_item_collapse = QAction(QIcon.fromTheme('zoom-out'), 'Collapse Selected', self) - self._action_item_collapse.triggered.connect(self._handle_action_item_collapse) - self.customContextMenuRequested.connect(self.handle_customContextMenuRequested) - - def startDrag(self, supportedActions): - index = self.currentIndex() - if not index.isValid(): - return - - item = self.model().itemFromIndex(index) - path = getattr(item, '_path', None) - if path is None: - qWarning('MessageTreeWidget.startDrag(): no _path set on item %s' % item) - return - - data = QMimeData() - data.setText(item._path) - - drag = QDrag(self) - drag.setMimeData(data) - drag.exec_() - - @Slot('QPoint') - def handle_customContextMenuRequested(self, pos): - # show context menu - menu = QMenu(self) - self._context_menu_add_actions(menu, pos) - menu.exec_(self.mapToGlobal(pos)) - - def _context_menu_add_actions(self, menu, pos): - if self.selectionModel().hasSelection(): - menu.addAction(self._action_item_expand) - menu.addAction(self._action_item_collapse) - - def _handle_action_item_collapse(self): - self._handle_action_set_expanded(False) - - def _handle_action_item_expand(self): - self._handle_action_set_expanded(True) - - def _handle_action_set_expanded(self, expanded): - def recursive_set_expanded(index): - if index != QModelIndex(): - self.setExpanded(index, expanded) - recursive_set_expanded(index.child(0, 0)) - for index in self.selectedIndexes(): - recursive_set_expanded(index) - - @Slot('QPoint') - def handle_header_view_customContextMenuRequested(self, pos): - - # create context menu - menu = QMenu(self) - - action_toggle_auto_resize = menu.addAction('Auto-Resize') - action_toggle_auto_resize.setCheckable(True) - auto_resize_flag = (self.header().resizeMode(0) == QHeaderView.ResizeToContents) - action_toggle_auto_resize.setChecked(auto_resize_flag) - - action_toggle_sorting = menu.addAction('Sorting') - action_toggle_sorting.setCheckable(True) - action_toggle_sorting.setChecked(self.isSortingEnabled()) - - # show menu - action = menu.exec_(self.header().mapToGlobal(pos)) - - # evaluate user action - if action is action_toggle_auto_resize: - if auto_resize_flag: - self.header().setResizeMode(QHeaderView.Interactive) - else: - self.header().setResizeMode(QHeaderView.ResizeToContents) - - elif action is action_toggle_sorting: - self.setSortingEnabled(not self.isSortingEnabled()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py deleted file mode 100644 index 2fb65767..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/plugin_container_widget.py +++ /dev/null @@ -1,135 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2013, Open Source Robotics Foundation Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import rospkg -import rospy - - -class PluginContainerWidget(QWidget): - """ - This widget accommodates a plugin widget that needs an area to show system - message. A plugin widget is the pane that provides plugin's main - functionalities. PluginContainerWidget visually encapsulates a plugin - widget. - - In order to print msg in the msg pane provided by this class, plugin widget - MUST define and emit following signals: - - - sig_sysmsg - - sig_progress - - Having said that this architecture is based on signals, it is recommended - that exceptions raised in classes that are used in a plugin widget be - aggregated in it, so that only plugin widget is responsible for emitting - signals. - """ - - def __init__(self, plugin_widget, - on_sys_msg=True, on_sysprogress_bar=True): - """ - @param plugin_widget: The main widget of an rqt plugin. - @type plugin_widget: QWidget - @type on_sys_msg: bool - @param on_sys_msg: If True, a pane that accommodates str messages will - appear in the plugin's region. - @param on_sysprogress_bar: If True, a progress bar will appear in the - plugin's region. - """ - super(PluginContainerWidget, self).__init__() - - ui_file = os.path.join(rospkg.RosPack().get_path('rqt_py_common'), - 'resource', 'plugin_container.ui') - loadUi(ui_file, self) - - self._plugin_widget = plugin_widget - self._splitter.insertWidget(0, self._plugin_widget) - self.setWindowTitle(self._plugin_widget.windowTitle()) - - # Default is on for these sys status widgets. Only if the flag for them - # are 'False', hide them. - if on_sys_msg: - self._plugin_widget.sig_sysmsg.connect(self.set_sysmsg) - else: - self._sysmsg_widget.hide() - - if on_sysprogress_bar: - self._plugin_widget.sig_sysprogress.connect(self.set_sysprogress) - else: - self._sysprogress_bar.hide() - - def set_sysprogress(self, sysprogress): - #TODO: Pass progress value - pass - - def set_sysmsg(self, sysmsg): - """ - Set system msg that's supposed to be shown in sys msg pane. - @type sysmsg: str - """ - rospy.loginfo('PluginContainerWidget; {}'.format(sysmsg)) - #self._sysmsg_widget.setPlainText(sysmsg) - self._sysmsg_widget.append(sysmsg) - - def shutdown(self): - - #TODO: Is shutdown step necessary for PluginContainerWidget? - - self._plugin_widget.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - - #Save setting of PluginContainerWidget. - instance_settings.set_value('_splitter', self._splitter.saveState()) - - #Save setting of ContainED widget - self._plugin_widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - - # Restore the setting of PluginContainerWidget, separate from - # ContainED widget. - if instance_settings.contains('_splitter'): - self._splitter.restoreState(instance_settings.value('_splitter')) - else: - self._splitter.setSizes([100, 100, 200]) - - # Now restore the setting of ContainED widget - self._plugin_widget.restore_settings(plugin_settings, - instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py deleted file mode 100644 index 768ba1fa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rosaction.py +++ /dev/null @@ -1,848 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -# Author: Isaac Saito under the influence of rosmsg.__init__.py - -# TODO 3/11/2013 (Isaac) This is moved from rqt_action pkg due to circular -# dependency issue. This rosaction is supposed to be moved (pull requested) to -# actionlib in the future. - -""" -Modifying rosaction.__init__.py to add functionality for ROS Action. - -Implements rosaction command-line tools. - -The code API of the rosaction module is unstable (inheriting the status of -rosmsg) - -(2/4/2013) Most of codes are not tested with actinlib. There's -"#NOT_TESTED_FROM_HERE" sign in the code below. -""" - -from __future__ import print_function - -import collections -import inspect -import os -import sys -import yaml -from optparse import OptionParser - -import genmsg -# import genpy -import rosbag -import roslib -import roslib.message -import rospkg -import rospy - -class ROSActionException(Exception): pass -class ROSActionProtoException(Exception): pass -class RosActionProtoArgsException(Exception): pass - -# If flowtype chosen is default, instead use flow-style -# False except if meeting objects or arrays with more than -# this size of sub-elements -MAX_DEFAULT_NON_FLOW_ITEMS = 4 - -MODE_ACTION = '.action' - - -def rosaction_cmd_list(mode, full, argv=None): - if argv is None: - argv = sys.argv[1:] - parser = OptionParser(usage="usage: ros%s list" % mode[1:]) - options, args = parser.parse_args(argv[1:]) - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for iterate_packages: %s' % mode) - - rospack = rospkg.RosPack() - packs = sorted([x for x in iterate_packages(rospack, mode)]) - for (p, direc) in packs: - for file_ in _list_types(direc, subdir, mode): - rospy.loginfo("%s/%s" % (p, file_)) - - -def _get_action_class_genpy(type_str, message_type, reload_on_error=False): - """ - Taken from genpy.message._get_message_or_service_class - - Utility for retrieving message/service class instances. Used by - get_message_class and get_service_class. - :param type_str: 'msg' or 'srv', ``str`` - :param message_type: type name of message/service, ``str`` - :returns: Message/Service for message/service type or None, ``class`` - :raises: :exc:`ValueError` If message_type is invalidly specified - """ - # # parse package and local type name for import - package, base_type = genmsg.package_resource_name(message_type) - if not package: - if base_type == 'Header': - package = 'std_msgs' - else: - raise ValueError("message type is missing package name: %s" - % str(message_type)) - pypkg = val = None - try: - # import the package and return the class - pypkg = __import__('%s.%s' % (package, type_str)) - val = getattr(getattr(pypkg, type_str), base_type) - except ImportError: - val = None - except AttributeError: - val = None - -""" -Taken from genpy.message -cache for get_message_class -""" -_message_class_cache_genpy = {} - - -def get_message_class_genpy(message_type, reload_on_error=False): - """ - Taken from genpy.message.get_message_class - - Get the message class. NOTE: this function maintains a - local cache of results to improve performance. - :param message_type: type name of message, ``str`` - :param reload_on_error: (optional). Attempt to reload the Python - module if unable to load message the first time. Defaults to - False. This is necessary if messages are built after the first load. - :returns: Message class for message/service type, ``Message class`` - :raises :exc:`ValueError`: if message_type is invalidly specified - """ - if message_type in _message_class_cache_genpy: - return _message_class_cache_genpy[message_type] - cls = _get_action_class_genpy('action', message_type, - reload_on_error=reload_on_error) - if cls: - _message_class_cache_genpy[message_type] = cls - return cls - - -def _get_action_class(type_str, message_type, reload_on_error=False): - """ - Taken from roslib.message._get_message_or_service_class - """ - - # # parse package and local type name for import - package, base_type = genmsg.package_resource_name(message_type) - if not package: - if base_type == 'Header': - package = 'std_msgs' - else: - raise ValueError("message type is missing package name: %s" % - str(message_type)) - pypkg = val = None - try: - # bootstrap our sys.path - roslib.launcher.load_manifest(package) - - rospy.loginfo('package={} type_str={} base_type={}'.format( - package, type_str, base_type)) - - # import the package and return the class - pypkg = __import__('%s/%s' % (package, type_str)) - # pypkg = __import__('%s.%s'%(package, type_str)) - val = getattr(getattr(pypkg, type_str), base_type) - - except rospkg.ResourceNotFound: - val = None - rospy.loginfo('_get_action_class except 1') - except ImportError: - val = None - rospy.loginfo('_get_action_class except 2') - except AttributeError: - val = None - rospy.loginfo('_get_action_class except 3') - - # this logic is mainly to support rosh, so that a user doesn't - # have to exit a shell just because a message wasn't built yet - if val is None and reload_on_error: - try: - if pypkg: - reload(pypkg) - val = getattr(getattr(pypkg, type_str), base_type) - except: - val = None - return val - -""" -Taken from roslib.message -""" -# # cache for get_message_class -_action_class_cache = {} - - -def get_action_class(action_type, reload_on_error=False): - """ - Taken from roslib.message.get_action_class - """ - - if action_type in _action_class_cache: - return _action_class_cache[action_type] - # try w/o bootstrapping - cls = get_message_class_genpy(action_type, reload_on_error=reload_on_error) - if cls is None: - # try old loader w/ bootstrapping - cls = _get_action_class('action', action_type, - reload_on_error=reload_on_error) - if cls: - _action_class_cache[action_type] = cls - return cls - - -def iterate_packages(rospack, mode): - """ - Iterator for packages that contain actions - :param mode: .action, ``str`` - """ - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for iterate_packages: %s' % mode) - pkgs = rospack.list() - for p in pkgs: - d = os.path.join(rospack.get_path(p), subdir) - if os.path.isdir(d): - yield p, d - - -def _msg_filter(ext): - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - -def _list_resources(path, rfilter=os.path.isfile): - """ - List resources in a package directory within a particular - subdirectory. This is useful for listing messages, services, etc... - :param rfilter: resource filter function that returns true if filename is - the desired resource type, ``fn(filename)->bool`` - """ - resources = [] - if os.path.isdir(path): - resources = [f for f in os.listdir(path) if rfilter(os.path.join(path, f))] - else: - resources = [] - return resources - - -def _list_types(path, subdir, ext): - """ - List all messages in the specified package - :param package str: name of package to search - :param include_depends bool: if True, will also list messages in package - dependencies - :returns [str]: message type names - """ - types = _list_resources(path, _msg_filter(ext)) - result = [x[:-len(ext)] for x in types] - result.sort() - - rospy.loginfo('_list_types result={}'.format(result)) - - return result - - -def list_types(package, mode=MODE_ACTION): - """ - Lists msg/srvs contained in package - :param package: package name, ``str`` - :param mode: MODE_ACTION. Defaults to msgs, ``str`` - :returns: list of msgs/srv in package, ``[str]`` - """ - - rospack = rospkg.RosPack() - if mode == MODE_ACTION: - subdir = 'action' - else: - raise ValueError('Unknown mode for list_types: %s' % mode) - path = os.path.join(rospack.get_path(package), subdir) - - rospy.loginfo('list_types package={} mode={} path={}'.format(package, mode, - path)) - - return [genmsg.resource_name(package, t) - for t in _list_types(path, subdir, mode)] - - -def list_actions(package): - """ - List actions contained in package - :param package: package name, ``str`` - :returns: list of actions in package, ``[str]`` - """ - return list_types(package, mode=MODE_ACTION) - - -def rosactionmain(mode=MODE_ACTION): - """ - Main entry point for command-line tools (rosaction). - - rosaction can interact with either ros messages or ros services. The mode - param indicates which - :param mode: MODE_ACTION or MODE_SRV, ``str`` - """ - try: - if mode == MODE_ACTION: - ext, full = mode, "message type" - else: - raise ROSActionException("Invalid mode: %s" % mode) - - if len(sys.argv) == 1: - rospy.loginfo(fullusage('ros' + mode[1:])) - sys.exit(0) - - command = sys.argv[1] -# if command == 'show': -# rosaction_cmd_show(ext, full) -# elif command == 'package': -# rosaction_cmd_package(ext, full) -# elif command == 'packages': -# rosaction_cmd_packages(ext, full) - if command == 'list': - rosaction_cmd_list(ext, full) -# elif command == 'md5': -# rosaction_cmd_md5(ext, full) - elif command == '--help': - print(fullusage('ros' + mode[1:])) - sys.exit(0) - else: - print(fullusage('ros' + mode[1:])) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except KeyError as e: - print("Unknown message type: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except rospkg.ResourceNotFound as e: - print("Invalid package: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ValueError as e: - print("Invalid type: '%s'" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ROSActionException as e: - print(str(e), file=sys.stderr) - sys.exit(1) - except KeyboardInterrupt: - pass - -""" -"#NOT_TESTED_FROM_HERE"---------------------------------------- -From here are what are copied from __init__.py that I don't know yet -if they are necessary/useful. -""" - ## copied from the web, recipe for ordered yaml output ###### -def construct_ordered_mapping(self, node, deep=False): - if not isinstance(node, yaml.MappingNode): - raise yaml.constructor.ConstructorError(None, None, - "expected a mapping node, but found %s" % node.id, - node.start_mark) - mapping = collections.OrderedDict() - for key_node, value_node in node.value: - key = self.construct_object(key_node, deep=deep) - if not isinstance(key, collections.Hashable): - raise yaml.constructor.ConstructorError("while constructing a mapping", - node.start_mark, - "found unhashable key", key_node.start_mark) - value = self.construct_object(value_node, deep=deep) - mapping[key] = value - return mapping - -def construct_yaml_map_with_ordered_dict(self, node): - data = collections.OrderedDict() - yield data - value = self.construct_mapping(node) - data.update(value) - -def represent_ordered_mapping(self, tag, mapping, flow_style=None): - value = [] - node = yaml.MappingNode(tag, value, flow_style=flow_style) - if self.alias_key is not None: - self.represented_objects[self.alias_key] = node - best_style = True - if hasattr(mapping, 'items'): - mapping = list(mapping.items()) - for item_key, item_value in mapping: - node_key = self.represent_data(item_key) - node_value = self.represent_data(item_value) - if not (isinstance(node_key, yaml.ScalarNode) and not node_key.style): - best_style = False - if not (isinstance(node_value, yaml.ScalarNode) and not node_value.style): - best_style = False - value.append((node_key, node_value)) - if flow_style is None: - if self.default_flow_style is not None: - node.flow_style = self.default_flow_style - else: - node.flow_style = best_style - return node - -## end recipe for ordered yaml output ###### - - -def get_array_type_instance(field_type, default_package=None): - """ - returns a single instance of field_type, where field_type can be a - message or ros primitive or an flexible size array. - """ - field_type = field_type.strip().rstrip("[]") - if field_type == "empty": - return None - if not "/" in field_type: - # is either built-in, Header, or in same package - # it seems built-in types get a priority - if field_type in ['byte', 'int8', 'int16', 'int32', 'int64', \ - 'char', 'uint8', 'uint16', 'uint32', 'uint64']: - return 0 - elif field_type in ['float32', 'float64']: - return 0 - elif field_type in ['string']: - return "" - elif field_type == 'bool': - return False - elif field_type == 'time': - field_type = "std_msgs/Time" - elif field_type == 'duration': - field_type = "std_msgs/Duration" - elif field_type == 'Header': - field_type = "std_msgs/Header" - else: - if default_package is None: - return None - field_type = default_package + "/" + field_type - msg_class = roslib.message.get_message_class(field_type) - if (msg_class == None): - # not important enough to raise exception? - return None - instance = msg_class() - return instance - -def get_yaml_for_msg(msg, prefix='', time_offset=None, current_time=None, - field_filter=None, flow_style_=None, fill_arrays_=False): - """ - Builds a YAML string of message. - @param msg: A object, dict or array - @param flow_style_: if True, produces one line with brackets, if false uses multiple lines with indentation, if None uses both using heuristics - @param prefix: prefixes all lines with this string - @param fill_arrays_: if True, for all flexible size arrays an element will be generated - @param current_time: currently not used. Only provided for API compatibility. current_time passes in the current time with respect to the message. - @type current_time: Time - @param field_filter: filter the fields that are strified for Messages. - @type field_filter: fn(Message)->iter(str) - @type flow_style_: bool - @return: a string - """ - def object_representer(dumper, obj): - ndict = collections.OrderedDict() - index = 0 - # allow caller to select which fields of message are strified - if field_filter != None: - fields = list(field_filter(obj)) - else: - fields = obj.__slots__ - for key in fields: - if not key.startswith('_'): - val = getattr(obj, key) - if type(val) == list and len(val) > MAX_DEFAULT_NON_FLOW_ITEMS: - dumper.default_flow_style = flow_style_ - if time_offset is not None and isinstance(val, Time): - ndict[key] = val - time_offset - # create initial array element (e.g. for code completion) - elif fill_arrays_ == True and val == []: - message_type = obj._slot_types[index] - if (obj._type != None) and "/" in obj._type: - def_pack = obj._type.split("/")[0] - instance = get_array_type_instance(message_type, default_package=def_pack) - if instance == None: - # not important enough to raise exception? - ndict[key] = val - else: - ndict[key] = [instance] - elif not inspect.ismethod(val) and not inspect.isfunction(val): - ndict[key] = val - index += 1 - # as a hack, we improve the heuristics of pyyaml and say with less than 5 objects, no need for brackets - if len(ndict) > MAX_DEFAULT_NON_FLOW_ITEMS: - dumper.default_flow_style = flow_style_ - return dumper.represent_dict(ndict) - yaml.representer.SafeRepresenter.add_representer(None, object_representer) - - # we force False over None here and set the style in dumper, to - # avoid unecessary outer brackets pyyaml chooses e.g. to - # represent msg Int32 as "{data: 0}" - initial_flow_style = False - if flow_style_ == True: - initial_flow_style = True - - # need to set default flow style True for bash prototype - # means will generate one line with [] and {} brackets - # bash needs bracket notation for rostopic pub - txt = yaml.safe_dump(msg, - # None, True, False (None chooses a compromise) - default_flow_style=initial_flow_style, - # Can be None, '', '\'', '"', '|', '>'. - default_style='', - # indent=2, #>=2, indentation depth - # line_break=?, - # allow_unicode=?, - # if true writes plenty of tags - # canonical = False, - # version={}?, - # width=40, - # encoding=?, - # tags={}?, - # when True, produces --- at start - # explicit_start=False, - # when True, produces ... at end - # explicit_end=False - ) - if prefix != None and prefix != '': - result = prefix + ("\n" + prefix).join(txt.splitlines()) - else: - result = txt.rstrip('\n') - return result - - -def create_names_filter(names): - """ - returns a function to use as filter that returns all objects slots except those with names in list. - """ - return lambda obj : filter(lambda slotname : not slotname in names, obj.__slots__) - - -def init_rosaction_proto(): - if "OrderedDict" in collections.__dict__: - yaml.constructor.BaseConstructor.construct_mapping = construct_ordered_mapping - yaml.constructor.Constructor.add_constructor( - 'tag:yaml.org,2002:map', - construct_yaml_map_with_ordered_dict) - - yaml.representer.BaseRepresenter.represent_mapping = represent_ordered_mapping - yaml.representer.Representer.add_representer(collections.OrderedDict, - yaml.representer.SafeRepresenter.represent_dict) - -def rosaction_cmd_prototype(args): - init_rosaction_proto() - parser = OptionParser(usage="usage: rosactionproto msg/srv [options]", - description="Produces output or a msg or service request, intended for tab completion support.") - parser.add_option("-f", "--flow_style", - dest="flow_style", type="int", default=None, action="store", - help="if 1 always use brackets, if 0 never use brackets. Default is a heuristic mix.") - parser.add_option("-e", "--empty-arrays", - dest="empty_arrays", default=False, action="store_true", - help="if true flexible size arrays are not filled with default instance") - parser.add_option("-s", "--silent", - dest="silent", default=False, action="store_true", - help="if true supresses all error messages") - parser.add_option("-p", "--prefix", metavar="prefix", default="", - help="prefix to print before each line, can be used for indent") - parser.add_option("-H", "--no-hyphens", - dest="no_hyphens", default="", action="store_true", - help="if true output has no outer hyphens") - parser.add_option("-x", "--exclude-slots", metavar="exclude_slots", default="", - help="comma separated list of slot names not to print") - - options, args = parser.parse_args(args) - - try: - if len(args) < 2: - raise RosActionProtoArgsException("Insufficient arguments") - mode = ".%s" % args[0] - message_type = args[1] - field_filter = None - if options.exclude_slots != None and options.exclude_slots.strip() != "": - field_filter = create_names_filter(options.exclude_slots.split(',')) - - # possible extentions: options for - # - target language - # - initial values for standard types - # - get partial message (subtree) - - # try to catch the user specifying code-style types and error - if '::' in message_type: - if not options.silent: - parser.error("rosactionproto does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'.") - elif '.' in message_type: - if not options.silent: - parser.error("invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % message_type) - if not '/' in message_type: - # if only one such msg or srv exists, use it - results = [] - for found in rosaction_search(rospkg.RosPack(), mode, message_type): - results.append(found) - if len(results) > 1: - raise ROSActionProtoException("Ambiguous message name %s" % message_type) - elif len(results) < 1: - raise ROSActionProtoException("Unknown message name %s" % message_type) - else: - message_type = results[0] - - if mode == MODE_ACTION: - msg_class = roslib.message.get_message_class(message_type) - if (msg_class == None): - raise ROSActionProtoException("Unknown message class: %s" % message_type) - instance = msg_class() - else: - raise ROSActionProtoException("Invalid mode: %s" % mode) - txt = get_yaml_for_msg(instance, - prefix=options.prefix, - flow_style_=options.flow_style, - fill_arrays_=not options.empty_arrays, - field_filter=field_filter) - - if options.no_hyphens == True: - return txt - else: - return '"' + txt + '"' - - except KeyError as e: - if not options.silent: - sys.stderr.write("Unknown message type: %s" % e, file=sys.stderr) - sys.exit(getattr(os, 'EX_USAGE', 1)) - # except rospkg.InvalidROSPkgException as e: - # if not options.silent: - # print(file=sys.stderr, "Invalid package: '%s'"%e) - # sys.exit(getattr(os, 'EX_USAGE', 1)) - except ValueError, e: - if not options.silent: - sys.stderr.write("Invalid type: '%s'" % e) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except ROSActionProtoException as e: - if not options.silent: - sys.stderr.write(str(e)) - sys.exit(1) - except RosActionProtoArgsException as e: - if not options.silent: - sys.stderr.write("%s" % e) - sys.exit(getattr(os, 'EX_USAGE', 1)) - except KeyboardInterrupt: - pass - -#### Start of rosmsg #### - -try: - from cStringIO import StringIO # Python 2.x -except ImportError: - from io import StringIO # Python 3.x -def spec_to_str(action_context, spec, buff=None, indent=''): - """ - Convert spec into a string representation. Helper routine for MsgSpec. - :param indent: internal use only, ``str`` - :param buff: internal use only, ``StringIO`` - :returns: string representation of spec, ``str`` - """ - if buff is None: - buff = StringIO() - for c in spec.constants: - buff.write("%s%s %s=%s\n" % (indent, c.type, c.name, c.val_text)) - for type_, name in zip(spec.types, spec.names): - buff.write("%s%s %s\n" % (indent, type_, name)) - base_type = genmsg.msgs.bare_msg_type(type_) - if not base_type in genmsg.msgs.BUILTIN_TYPES: - subspec = msg_context.get_registered(base_type) - spec_to_str(msg_context, subspec, buff, indent + ' ') - return buff.getvalue() - -def get_msg_text(type_, raw=False, rospack=None): - """ - Get .msg file for type_ as text - :param type_: message type, ``str`` - :param raw: if True, include comments and whitespace (default False), ``bool`` - :returns: text of .msg file, ``str`` - :raises :exc:`ROSActionException` If type_ is unknown - """ - if rospack is None: - rospack = rospkg.RosPack() - search_path = {} - for p in rospack.list(): - search_path[p] = [os.path.join(rospack.get_path(p), 'msg')] - - context = genmsg.MsgContext.create_default() - try: - spec = genmsg.load_msg_by_type(context, type_, search_path) - genmsg.load_depends(context, spec, search_path) - except Exception as e: - raise ROSActionException("Unable to load msg [%s]: %s" % (type_, e)) - - if raw: - return spec.text - else: - return spec_to_str(context, spec) - -def _msg_filter(ext): - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - -def rosaction_search(rospack, mode, base_type): - """ - Iterator for all packages that contain a message matching base_type - - :param base_type: message base type to match, e.g. 'String' would match std_msgs/String, ``str`` - """ - for p, path in iterate_packages(rospack, mode): - if os.path.isfile(os.path.join(path, "%s%s" % (base_type, mode))): - yield genmsg.resource_name(p, base_type) - -def _stdin_arg(parser, full): - options, args = parser.parse_args(sys.argv[2:]) - # read in args from stdin pipe if not present - if not args: - arg = None - while not arg: - arg = sys.stdin.readline().strip() - return options, arg - else: - if len(args) > 1: - parser.error("you may only specify one %s" % full) - return options, args[0] - -def rosaction_cmd_show(mode, full): - cmd = "ros%s" % (mode[1:]) - parser = OptionParser(usage="usage: %s show [options] <%s>" % (cmd, full)) - parser.add_option("-r", "--raw", - dest="raw", default=False, action="store_true", - help="show raw message text, including comments") - parser.add_option("-b", "--bag", - dest="bag", default=None, - help="show message from .bag file", metavar="BAGFILE") - options, arg = _stdin_arg(parser, full) - if arg.endswith(mode): - arg = arg[:-(len(mode))] - - # try to catch the user specifying code-style types and error - if '::' in arg: - parser.error(cmd + " does not understand C++-style namespaces (i.e. '::').\nPlease refer to msg/srv types as 'package_name/Type'.") - elif '.' in arg: - parser.error("invalid message type '%s'.\nPlease refer to msg/srv types as 'package_name/Type'." % arg) - if options.bag: - bag_file = options.bag - if not os.path.exists(bag_file): - raise ROSActionException("ERROR: bag file [%s] does not exist" % bag_file) - for topic, msg, t in rosbag.Bag(bag_file).read_messages(raw=True): - datatype, _, _, _, pytype = msg - if datatype == arg: - print(get_msg_text(datatype, options.raw, pytype._full_text)) - break - else: - rospack = rospkg.RosPack() - if '/' in arg: # package specified - rosaction_debug(rospack, mode, arg, options.raw) - else: - for found in rosaction_search(rospack, mode, arg): - print("[%s]:" % found) - rosaction_debug(rospack, mode, found, options.raw) - -def rosaction_md5(mode, type_): - try: - if mode == MODE_ACTION: - msg_class = roslib.message.get_message_class(type_) - else: - msg_class = roslib.message.get_service_class(type_) - except ImportError: - raise IOError("cannot load [%s]" % (type_)) - if msg_class is not None: - return msg_class._md5sum - else: - raise IOError("cannot load [%s]" % (type_)) - -def rosaction_cmd_md5(mode, full): - parser = OptionParser(usage="usage: ros%s md5 <%s>" % (mode[1:], full)) - options, arg = _stdin_arg(parser, full) - - if '/' in arg: # package specified - try: - md5 = rosaction_md5(mode, arg) - print(md5) - except IOError: - print("Cannot locate [%s]" % arg, file=sys.stderr) - else: - rospack = rospkg.RosPack() - matches = [m for m in rosaction_search(rospack, mode, arg)] - for found in matches: - try: - md5 = rosaction_md5(mode, found) - print("[%s]: %s" % (found, md5)) - except IOError: - print("Cannot locate [%s]" % found, file=sys.stderr) - if not matches: - print("No messages matching the name [%s]" % arg, file=sys.stderr) - -def rosaction_cmd_package(mode, full): - parser = OptionParser(usage="usage: ros%s package " % mode[1:]) - parser.add_option("-s", - dest="single_line", default=False, action="store_true", - help="list all msgs on a single line") - options, arg = _stdin_arg(parser, full) - joinstring = '\n' - if options.single_line: - joinstring = ' ' - print(joinstring.join(list_types(arg, mode=mode))) - -def rosaction_cmd_packages(mode, full, argv=None): - if argv is None: - argv = sys.argv[1:] - parser = OptionParser(usage="usage: ros%s packages" % mode[1:]) - parser.add_option("-s", - dest="single_line", default=False, action="store_true", - help="list all packages on a single line") - options, args = parser.parse_args(argv[1:]) - rospack = rospkg.RosPack() - joinstring = '\n' - if options.single_line: - joinstring = ' ' - p1 = [p for p, _ in iterate_packages(rospack, mode)] - p1.sort() - print(joinstring.join(p1)) - -def rosaction_debug(rospack, mode, type_, raw=False): - """ - Prints contents of msg/srv file - :param mode: MODE_ACTION or MODE_SRV, ``str`` - """ - if mode == MODE_ACTION: - print(get_msg_text(type_, raw=raw, rospack=rospack)) - else: - raise ROSActionException("Invalid mode for debug: %s" % mode) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py deleted file mode 100644 index 87eae043..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_ros_graph.py +++ /dev/null @@ -1,163 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt -import rospy - - -class RqtRosGraph(object): - - DELIM_GRN = '/' - - @staticmethod - def get_full_grn(model_index): - """ - @deprecated: Not completed. - - Create full path format of GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). Build GRN by recursively transcending - parents & children of a given QModelIndex instance. - - A complete example of GRN: /wide_stereo/left/image_color/compressed - - Upon its very 1st call, the argument is the index where user clicks on - on the view object (here QTreeView is used but should work with other - View too. Not tested yet though). str_grn can be 0-length string. - - :type model_index: QModelIndex - :type str_grn: str - :param str_grn: This could be an incomplete or a complete GRN format. - :rtype: str - """ - - children_grn_list = RqtRosGraph.get_lower_grn_dfs(model_index) - parent_data = model_index.data() - rospy.logdebug('parent_data={}'.format(parent_data)) - if parent_data == None: # model_index is 1st-order node of a tree. - upper_grn = RqtRosGraph.get_upper_grn(model_index, '') - grn_list = [] - for child_grn in children_grn_list: - grn_full = upper_grn + child_grn - rospy.logdebug('grn_full={} upper_grn={} child_grn={}'.format( - grn_full, upper_grn, child_grn)) - grn_list.append(grn_full) - else: - grn_list = children_grn_list - - #Create a string where namespace is delimited by slash. - grn = '' - for s in grn_list: - grn += RqtRosGraph.DELIM_GRN + s - - return grn - - @staticmethod - def get_lower_grn_dfs(model_index, grn_prev=''): - """ - Traverse all children treenodes and returns a list of "partial" - GRNs. Partial means that this method returns names under current level. - - Ex. Consider a tree like this: - - Root - |--TopitemA - | |--1 - | |--2 - | |--3 - | |--4 - | |--5 - | |--6 - | |--7 - |--TopitemB - - Re-formatted in GRN (omitting root): - - TopitemA/1/2/3/4 - TopitemA/1/2/3/5/6 - TopitemA/1/2/3/5/7 - TopitemB - - Might not be obvious from tree representation but there are 4 nodes as - GRN form suggests. - - (doc from here TBD) - - :type model_index: QModelIndex - :type grn_prev: str - :rtype: str[] - """ - i_child = 0 - list_grn_children_all = [] - while True: # Loop per child. - grn_curr = grn_prev + RqtRosGraph.DELIM_GRN + str( - model_index.data()) - child_qmindex = model_index.child(i_child, 0) - - if (not child_qmindex.isValid()): - rospy.logdebug('!! DEADEND i_child=#{} grn_curr={}'.format( - i_child, grn_curr)) - if i_child == 0: - # Only when the current node has no children, add current - # GRN to the returning list. - list_grn_children_all.append(grn_curr) - return list_grn_children_all - - rospy.logdebug('Child#{} grn_curr={}'.format(i_child, grn_curr)) - - list_grn_children = RqtRosGraph.get_lower_grn_dfs(child_qmindex, - grn_curr) - for child_grn in list_grn_children: - child_grn = (grn_prev + - (RqtRosGraph.DELIM_GRN + grn_curr) + - (RqtRosGraph.DELIM_GRN + child_grn)) - - list_grn_children_all = list_grn_children_all + list_grn_children - rospy.logdebug('111 lennodes={} list_grn_children={}'.format( - len(list_grn_children_all), list_grn_children)) - rospy.logdebug('122 list_grn_children_all={}'.format( - list_grn_children_all)) - i_child += 1 - return list_grn_children_all - - @staticmethod - def get_upper_grn(model_index, str_grn): - if model_index.data(Qt.DisplayRole) == None: - return str_grn - str_grn = (RqtRosGraph.DELIM_GRN + - str(model_index.data(Qt.DisplayRole)) + - str_grn) - rospy.logdebug('get_full_grn_recur out str=%s', str_grn) - return RqtRosGraph.get_upper_grn(model_index.parent(), str_grn) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py deleted file mode 100644 index c9c3313a..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/rqt_roscomm_util.py +++ /dev/null @@ -1,215 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import os - -import genmsg -import roslaunch -from roslaunch import RLException -import rospkg -import rospy -import rostopic - - -class RqtRoscommUtil(object): - - @staticmethod - def load_parameters(config, caller_id): - """ - Load parameters onto the parameter server. - - Copied from ROSLaunchRunner. - - @type config: roslaunch.config.ROSLaunchConfig - @raise RLException: - """ - - # XMLRPC proxy for communicating with master, 'xmlrpclib.ServerProxy' - param_server = config.master.get() - - param = None - try: - # multi-call style xmlrpc - # According to API doc, get_multi() returns - # multicall XMLRPC proxy for communicating with master, - # "xmlrpclib.MultiCall" - param_server_multi = config.master.get_multi() - - # clear specified parameter namespaces - # #2468 unify clear params to prevent error - for param in roslaunch.launch._unify_clear_params( - config.clear_params): - if param_server.hasParam(caller_id, param)[2]: - param_server_multi.deleteParam(caller_id, param) - r = param_server_multi() - for code, msg, _ in r: - if code != 1: - raise RLException("Failed to clear parameter {}: ".format( - msg)) - except RLException: - raise - except Exception, e: - rospy.logerr("load_parameters: unable to set params " + - "(last param was [{}]): {}".format(param, e)) - raise # re-raise as this is fatal - - try: - # multi-call objects are not reusable - param_server_multi = config.master.get_multi() - for param in config.params.itervalues(): - # suppressing this as it causes too much spam - # printlog("setting parameter [%s]"%param.key) - param_server_multi.setParam(caller_id, param.key, param.value) - r = param_server_multi() - for code, msg, _ in r: - if code != 1: - raise RLException("Failed to set parameter: " + - "%s" % (msg)) - except RLException: - raise - except Exception, e: - print("load_parameters: unable to set params (last param was " + - "[%s]): %s" % (param, e)) - raise # re-raise as this is fatal - rospy.logdebug("... load_parameters complete") - - @staticmethod - def iterate_packages(subdir): - """ - Iterator for packages that contain the given subdir. - - This method is generalizing rosmsg.iterate_packages. - - @param subdir: eg. 'launch', 'msg', 'srv', 'action' - @type subdir: str - @raise ValueError: - """ - if subdir == None or subdir == '': - raise ValueError('Invalid package subdir = {}'.format(subdir)) - - rospack = rospkg.RosPack() - - pkgs = rospack.list() - rospy.logdebug('pkgs={}'.format(pkgs)) - for p in pkgs: - d = os.path.join(rospack.get_path(p), subdir) - rospy.logdebug('rospack dir={}'.format(d)) - if os.path.isdir(d): - yield p, d - - @staticmethod - def list_files(package, subdir, file_extension='.launch'): - """ - #TODO: Come up with better name of the method. - - Taken from rosmsg. - Lists files contained in the specified package - - @param package: package name, ``str`` - @param file_extension: Defaults to '.launch', ``str`` - :returns: list of msgs/srv in package, ``[str]`` - """ - if subdir == None or subdir == '': - raise ValueError('Invalid package subdir = {}'.format(subdir)) - - rospack = rospkg.RosPack() - - path = os.path.join(rospack.get_path(package), subdir) - - return [genmsg.resource_name(package, t) - for t in RqtRoscommUtil._list_types( - path, file_extension)] - - @staticmethod - def _list_types(path, ext): - """ - Taken from rosmsg - - List all messages in the specified package - :param package str: name of package to search - :param include_depends bool: if True, will also list messages in - package dependencies. - :returns [str]: message type names - """ - types = RqtRoscommUtil._list_resources(path, - RqtRoscommUtil._msg_filter(ext)) - - result = [x for x in types] - # result = [x[:-len(ext)] for x in types] # Remove extension - - result.sort() - return result - - @staticmethod - def _list_resources(path, rfilter=os.path.isfile): - """ - Taken from rosmsg._list_resources - - List resources in a package directory within a particular - subdirectory. This is useful for listing messages, services, etc... - :param rfilter: resource filter function that returns true if filename - is the desired resource type, ``fn(filename)->bool`` - """ - resources = [] - if os.path.isdir(path): - resources = [f for f - in os.listdir(path) if rfilter(os.path.join(path, f))] - else: - resources = [] - return resources - - @staticmethod - def _msg_filter(ext): - """ - Taken from rosmsg._msg_filter - """ - def mfilter(f): - """ - Predicate for filtering directory list. matches message files - :param f: filename, ``str`` - """ - return os.path.isfile(f) and f.endswith(ext) - return mfilter - - @staticmethod - def is_roscore_running(): - """ - @rtype: bool - """ - try: - # Checkif rosmaster is running or not. - rostopic.get_topic_class('/rosout') - return True - except rostopic.ROSTopicIOException as e: - return False diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py deleted file mode 100644 index f2eb145e..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_completer.py +++ /dev/null @@ -1,108 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - -from python_qt_binding.QtCore import qWarning - -from .message_tree_model import MessageTreeModel -from .tree_model_completer import TreeModelCompleter - - -class TopicCompleter(TreeModelCompleter): - - def __init__(self, parent=None): - super(TopicCompleter, self).__init__(parent) - self.setModel(MessageTreeModel()) - - def splitPath(self, path): - # to handle array subscriptions, e.g. /topic/field[1]/subfield[2] - # we need to separate array subscriptions by an additional / - return super(TopicCompleter,self).splitPath(path.replace('[','/[')) - - def update_topics(self): - self.model().clear() - topic_list = rospy.get_published_topics() - for topic_path, topic_type in topic_list: - topic_name = topic_path.strip('/') - message_class = roslib.message.get_message_class(topic_type) - if message_class is None: - qWarning('TopicCompleter.update_topics(): could not get message class for topic type "%s" on topic "%s"' % (topic_type, topic_path)) - continue - message_instance = message_class() - self.model().add_message(message_instance, topic_name, topic_type, topic_path) - - -if __name__ == '__main__': - import sys - from python_qt_binding.QtGui import QApplication, QComboBox, QLineEdit, QMainWindow, QTreeView, QVBoxLayout, QWidget - app = QApplication(sys.argv) - mw = QMainWindow() - widget = QWidget(mw) - layout = QVBoxLayout(widget) - - edit = QLineEdit() - edit_completer = TopicCompleter(edit) - #edit_completer.setCompletionMode(QCompleter.InlineCompletion) - edit.setCompleter(edit_completer) - - combo = QComboBox() - combo.setEditable(True) - combo_completer = TopicCompleter(combo) - #combo_completer.setCompletionMode(QCompleter.InlineCompletion) - combo.lineEdit().setCompleter(combo_completer) - - model_tree = QTreeView() - model_tree.setModel(combo_completer.model()) - model_tree.expandAll() - for column in range(combo_completer.model().columnCount()): - model_tree.resizeColumnToContents(column) - - completion_tree = QTreeView() - completion_tree.setModel(combo_completer.completionModel()) - completion_tree.expandAll() - for column in range(combo_completer.completionModel().columnCount()): - completion_tree.resizeColumnToContents(column) - - layout.addWidget(model_tree) - layout.addWidget(completion_tree) - layout.addWidget(edit) - layout.addWidget(combo) - layout.setStretchFactor(model_tree, 1) - widget.setLayout(layout) - mw.setCentralWidget(widget) - - mw.move(300, 0) - mw.resize(800, 900) - mw.show() - app.exec_() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py deleted file mode 100644 index 31c7bd78..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_dict.py +++ /dev/null @@ -1,66 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - - -class TopicDict(object): - - def __init__(self): - self.update_topics() - - def get_topics(self): - return self.topic_dict - - def update_topics(self): - self.topic_dict = {} - topic_list = rospy.get_published_topics() - for topic_name, topic_type in topic_list: - message = roslib.message.get_message_class(topic_type)() - self.topic_dict.update(self._recursive_create_field_dict(topic_name, message)) - - def _recursive_create_field_dict(self, topic_name, field): - field_dict = {} - field_dict[topic_name] = { - 'type': type(field), - 'children': {}, - } - - if hasattr(field, '__slots__'): - for slot_name in field.__slots__: - field_dict[topic_name]['children'].update(self._recursive_create_field_dict(slot_name, getattr(field, slot_name))) - - return field_dict - - -if __name__ == '__main__': - import pprint - pprint.pprint(TopicDict().get_topics()) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py deleted file mode 100644 index 7e27be54..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_helpers.py +++ /dev/null @@ -1,115 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import roslib.msgs -from rostopic import get_topic_type -from python_qt_binding.QtCore import qDebug - -def get_type_class(type_name): - if roslib.msgs.is_valid_constant_type(type_name): - if type_name == 'string': - return str - elif type_name == 'bool': - return bool - else: - return type(roslib.msgs._convert_val(type_name, 0)) - else: - return roslib.message.get_message_class(type_name) - -def get_field_type(topic_name): - """ - Get the Python type of a specific field in the given registered topic. - If the field is an array, the type of the array's values are returned and the is_array flag is set to True. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file' - :returns: field_type, is_array - """ - # get topic_type and message_evaluator - topic_type, real_topic_name, _ = get_topic_type(topic_name) - if topic_type is None: - #qDebug('topic_helpers.get_field_type(%s): get_topic_type failed' % (topic_name)) - return None, False - - message_class = roslib.message.get_message_class(topic_type) - if message_class is None: - qDebug('topic_helpers.get_field_type(%s): get_message_class(%s) failed' % (topic_name, topic_type)) - return None, False - - slot_path = topic_name[len(real_topic_name):] - return get_slot_type(message_class, slot_path) - - -def get_slot_type(message_class, slot_path): - """ - Get the Python type of a specific slot in the given message class. - If the field is an array, the type of the array's values are returned and the is_array flag is set to True. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param message_class: message class type, ``type``, usually inherits from genpy.message.Message - :param slot_path: path to the slot inside the message class, ``str``, i.e. 'header/seq' - :returns: field_type, is_array - """ - is_array = False - fields = [f for f in slot_path.split('/') if f] - for field_name in fields: - try: - field_name, _, field_index = roslib.msgs.parse_type(field_name) - except roslib.msgs.MsgSpecException: - return None, False - if field_name not in getattr(message_class, '__slots__', []): - #qDebug('topic_helpers.get_slot_type(%s, %s): field not found: %s' % (message_class, slot_path, field_name)) - return None, False - slot_type = message_class._slot_types[message_class.__slots__.index(field_name)] - slot_type, slot_is_array, _ = roslib.msgs.parse_type(slot_type) - is_array = slot_is_array and field_index is None - - message_class = get_type_class(slot_type) - return message_class, is_array - - -def is_slot_numeric(topic_name): - """ - Check is a slot in the given topic is numeric, or an array of numeric values. - This is a static type check, so it works for unpublished topics and with empty arrays. - - :param topic_name: name of field of a registered topic, ``str``, i.e. '/rosout/file' - :returns: is_numeric, is_array, description - """ - field_type, is_array = get_field_type(topic_name) - if field_type in (int, float): - if is_array: - message = 'topic "%s" is numeric array: %s[]' % (topic_name, field_type) - else: - message = 'topic "%s" is numeric: %s' % (topic_name, field_type) - return True, is_array, message - - return False, is_array, 'topic "%s" is NOT numeric: %s' % (topic_name, field_type) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py deleted file mode 100644 index 29b808ab..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/topic_tree_model.py +++ /dev/null @@ -1,49 +0,0 @@ -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import roslib -import rospy - -from .message_tree_model import MessageTreeModel - - -class TopicTreeModel(MessageTreeModel): - - def __init__(self, parent=None): - super(TopicTreeModel, self).__init__(parent) - self.refresh() - - def refresh(self): - self.clear() - topic_list = rospy.get_published_topics() - for topic_path, topic_type in topic_list: - topic_name = topic_path.strip('/') - message_instance = roslib.message.get_message_class(topic_type)() - self.add_message(message_instance, topic_name, topic_type, topic_path) diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py b/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py deleted file mode 100644 index 8cedc387..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/src/rqt_py_common/tree_model_completer.py +++ /dev/null @@ -1,48 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QCompleter - - -class TreeModelCompleter(QCompleter): - separator = '/' - - def __init__(self, parent=None): - super(TreeModelCompleter, self).__init__(parent) - - def splitPath(self, path): - path = path.lstrip(self.separator) - path_list = path.split(self.separator) - return path_list - - def pathFromIndex(self, index): - return self.model().itemFromIndex(index)._path diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg b/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg deleted file mode 100644 index 515ee2c8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/ArrayVal.msg +++ /dev/null @@ -1 +0,0 @@ -Val[5] vals diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg b/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg deleted file mode 100644 index b0ae1084..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/msg/Val.msg +++ /dev/null @@ -1 +0,0 @@ -float64[5] floats diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py deleted file mode 100644 index a3ad2b15..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_common_unit.py +++ /dev/null @@ -1,54 +0,0 @@ -#!/usr/bin/env python -# Software License Agreement (BSD License) -# -# Copyright (c) 2015, Robert Haschke -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import unittest - -class TestMessageTreeModel(unittest.TestCase): - def test_path_names(self): - from rqt_py_common.message_tree_model import MessageTreeModel - from rqt_py_common.msg import Val, ArrayVal - m = MessageTreeModel() - m.add_message(ArrayVal()) - root = m.item(0).child(0) - self.assertEqual(root._path, '/vals') - for i in range(0,5): - child = root.child(i) - self.assertEqual(child._path, '/vals[%s]' % i) - child = child.child(0) - self.assertEqual(child._path, '/vals[%s]/floats' % i) - for j in range(0,5): - self.assertEqual(child.child(j)._path, '/vals[%s]/floats[%s]' % (i,j)) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py deleted file mode 100755 index 64756c46..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_ros_graph.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from python_qt_binding.QtGui import QStandardItem, QStandardItemModel -from rqt_reconfigure.rqt_ros_graph import RqtRosGraph - - -class TestRqtRosGraph(unittest.TestCase): - """ - :author: Isaac Saito - """ - - def setUp(self): - unittest.TestCase.setUp(self) - - self._model = QStandardItemModel() - node1 = QStandardItem('node1') - self._node1_1 = QStandardItem('node1_1') - self._node1_1_1 = QStandardItem('node1_1_1') - node1_1_2 = QStandardItem('node1_1_2') - node1_2 = QStandardItem('node1_2') - - node1.appendRow(self._node1_1) - self._node1_1.appendRow(self._node1_1_1) - self._node1_1.appendRow(node1_1_2) - node1.appendRow(node1_2) - - self._model.appendRow(node1) - - #node_list = [node1, node1_1, self._node1_1_1, node1_1_2, node1_2] - #self._model.appendRow(node_list) - - self._grn_node1_1_1 = '/node1/node1_1/node1_1_1' - self._len_lower_grn_node1_1 = 2 - - def tearDown(self): - unittest.TestCase.tearDown(self) - del self._model - - def test_get_upper_grn(self): - self.assertEqual(RqtRosGraph.get_upper_grn(self._node1_1_1.index(), ''), - self._grn_node1_1_1) - - def test_get_lower_grn_dfs(self): - self.assertEqual(len(RqtRosGraph.get_lower_grn_dfs( - self._node1_1.index(), - '')), - self._len_lower_grn_node1_1) - - def test_get_full_grn(self): - self.assertEqual(RqtRosGraph.get_full_grn(self._node1_1_1.index()), - self._grn_node1_1_1) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py b/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py deleted file mode 100755 index 792d17de..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_common/test/test_rqt_roscomm_util.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from rqt_py_common.rqt_roscomm_util import RqtRoscommUtil - - -class TestRqtRoscommUtil(unittest.TestCase): - """ - @author: Isaac Saito - """ - - def setUp(self): - unittest.TestCase.setUp(self) - - self._totalnum_pkg_contains_launch = 41 # Varies depending on system. - - def tearDown(self): - unittest.TestCase.tearDown(self) - #del self._model - - def test_iterate_packages(self): - """ - Not a very good test because the right answer that is hardcoded varies - depending on the system where this unittest runs. - """ - pkg_num_sum = 0 - for pkg in RqtRoscommUtil.iterate_packages('launch'): - pkg_num_sum += 1 - print 'pkg={}'.format(pkg) - - print pkg_num_sum - self.assertEqual(pkg_num_sum, self._totalnum_pkg_contains_launch) - - def test_list_files(self): - """ - Not a very good test because the right answer that is hardcoded varies - depending on the system where this unittest runs. - """ - file_num = 0 - pkg_name = 'pr2_moveit_config' - _totalnum_launches_pkg_contains = 15 - subdir = 'launch' - file_ext = '.launch' - files = RqtRoscommUtil.list_files(pkg_name, subdir, file_ext) - for file in files: - file_num += 1 - print 'file={}'.format(file) - - print file_num - self.assertEqual(file_num, _totalnum_launches_pkg_contains) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst deleted file mode 100644 index 90c3c900..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_py_console -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt deleted file mode 100644 index d3d1f7fa..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_py_console) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_py_console - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox deleted file mode 100644 index b540ebdd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_py_console is a Python GUI plugin providing an interactive Python console. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/package.xml b/workspace/src/rqt_common_plugins/rqt_py_console/package.xml deleted file mode 100644 index 4111943f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - rqt_py_console - 0.3.13 - rqt_py_console is a Python GUI plugin providing an interactive Python console. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_py_console - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui - qt_gui_py_common - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml b/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml deleted file mode 100644 index 121e2eb1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin providing an interactive Python console. - - - - - folder - Plugins related to miscellaneous tools. - - - applications-python - A Python GUI plugin providing an interactive Python console. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui b/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui deleted file mode 100644 index 12810f1c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/resource/py_console_widget.ui +++ /dev/null @@ -1,53 +0,0 @@ - - - PyConsole - - - - 0 - 0 - 276 - 212 - - - - PyConsole - - - - 0 - - - 0 - - - 0 - - - 3 - - - 0 - - - - - 0 - - - - - - - - - - - PyConsoleTextEdit - QTextEdit -
rqt_py_console.py_console_text_edit
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console b/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console deleted file mode 100755 index 8f9bfcb8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/scripts/rqt_py_console +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_py_console.py_console.PyConsole')) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/setup.py b/workspace/src/rqt_common_plugins/rqt_py_console/setup.py deleted file mode 100644 index 6ebf4557..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_py_console'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/__init__.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py deleted file mode 100644 index 5eb37dbe..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console.py +++ /dev/null @@ -1,103 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QVBoxLayout, QWidget -from qt_gui.plugin import Plugin -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog -from py_console_widget import PyConsoleWidget - -try: - from spyder_console_widget import SpyderConsoleWidget - _has_spyderlib = True -except ImportError: - _has_spyderlib = False - - -class PyConsole(Plugin): - """ - Plugin providing an interactive Python console - """ - def __init__(self, context): - super(PyConsole, self).__init__(context) - self.setObjectName('PyConsole') - - self._context = context - self._use_spyderlib = _has_spyderlib - self._console_widget = None - self._widget = QWidget() - self._widget.setLayout(QVBoxLayout()) - self._widget.layout().setContentsMargins(0, 0, 0, 0) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - self._context.add_widget(self._widget) - - def _switch_console_widget(self): - self._widget.layout().removeWidget(self._console_widget) - self.shutdown_console_widget() - - if _has_spyderlib and self._use_spyderlib: - self._console_widget = SpyderConsoleWidget(self._context) - self._widget.setWindowTitle('SpyderConsole') - else: - self._console_widget = PyConsoleWidget(self._context) - self._widget.setWindowTitle('PyConsole') - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - - self._widget.layout().addWidget(self._console_widget) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('use_spyderlib', self._use_spyderlib) - - def restore_settings(self, plugin_settings, instance_settings): - self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true']) - self._switch_console_widget() - - def trigger_configuration(self): - options = [ - {'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib}, - {'title': 'PyConsole', 'description': 'Simple Python console.'}, - ] - dialog = SimpleSettingsDialog(title='PyConsole Options') - dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib)) - console_type = dialog.get_settings()[0] - new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib) - if self._use_spyderlib != new_use_spyderlib: - self._use_spyderlib = new_use_spyderlib - self._switch_console_widget() - - def shutdown_console_widget(self): - if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'): - self._console_widget.shutdown() - - def shutdown_plugin(self): - self.shutdown_console_widget() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py deleted file mode 100644 index 4f933700..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_text_edit.py +++ /dev/null @@ -1,68 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import sys -from code import InteractiveInterpreter -from exceptions import SystemExit - -from python_qt_binding import QT_BINDING, QT_BINDING_VERSION -from python_qt_binding.QtCore import Qt, Signal - -from qt_gui_py_common.console_text_edit import ConsoleTextEdit - - -class PyConsoleTextEdit(ConsoleTextEdit): - _color_stdin = Qt.darkGreen - _multi_line_char = ':' - _multi_line_indent = ' ' - _prompt = ('>>> ', '... ') # prompt for single and multi line - exit = Signal() - - def __init__(self, parent=None): - super(PyConsoleTextEdit, self).__init__(parent) - - self._interpreter_locals = {} - self._interpreter = InteractiveInterpreter(self._interpreter_locals) - - self._comment_writer.write('Python %s on %s\n' % (sys.version.replace('\n', ''), sys.platform)) - self._comment_writer.write('Qt bindings: %s version %s\n' % (QT_BINDING, QT_BINDING_VERSION)) - - self._add_prompt() - - def update_interpreter_locals(self, newLocals): - self._interpreter_locals.update(newLocals) - - def _exec_code(self, code): - try: - self._interpreter.runsource(code) - except SystemExit: # catch sys.exit() calls, so they don't close the whole gui - self.exit.emit() diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py deleted file mode 100644 index 97beee5d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/py_console_widget.py +++ /dev/null @@ -1,54 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import py_console_text_edit - - -class PyConsoleWidget(QWidget): - def __init__(self, context=None): - super(PyConsoleWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_py_console'), 'resource', 'py_console_widget.ui') - loadUi(ui_file, self, {'PyConsoleTextEdit': py_console_text_edit.PyConsoleTextEdit}) - self.setObjectName('PyConsoleWidget') - - my_locals = { - 'context': context - } - self.py_console.update_interpreter_locals(my_locals) - self.py_console.print_message('The variable "context" is set to the PluginContext of this plugin.') - self.py_console.exit.connect(context.close_plugin) diff --git a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py b/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py deleted file mode 100644 index 7c86d285..00000000 --- a/workspace/src/rqt_common_plugins/rqt_py_console/src/rqt_py_console/spyder_console_widget.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QFont - -from spyderlib.widgets.internalshell import InternalShell -from spyderlib.utils.module_completion import moduleCompletion - - -class SpyderConsoleWidget(InternalShell): - def __init__(self, context=None): - my_locals = { - 'context': context - } - super(SpyderConsoleWidget, self).__init__(namespace=my_locals) - self.setObjectName('SpyderConsoleWidget') - self.set_pythonshell_font(QFont('Mono')) - self.interpreter.restore_stds() - - def get_module_completion(self, objtxt): - """Return module completion list associated to object name""" - return moduleCompletion(objtxt) - - def run_command(self, *args): - self.interpreter.redirect_stds() - super(SpyderConsoleWidget, self).run_command(*args) - self.flush() - self.interpreter.restore_stds() - - def shutdown(self): - self.exit_interpreter() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst deleted file mode 100644 index 63c7145c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/CHANGELOG.rst +++ /dev/null @@ -1,125 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_reconfigure -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- -* Added refresh button to re-scan reconfigure server list -* Now retains functioning nodes when refreshing -* Contributors: Kei Okada, Scott K Logan - -0.3.11 (2015-04-30) -------------------- -* restore support for parameter groups (`#162 `_) -* fix background colors for dark themes (`#293 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* fix slider bar, add context menus for common operations (`#251 `_) -* fix bug in float range calculations (`#241 `_) -* remove experimental suffix from rqt_reconfigure (`#256 `_) -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- -* remove unnecessary margins to improve usability on small screens (`#228 `_) - -0.3.5 (2014-05-07) ------------------- -* numerous improvements and bug fixes (`#209 `_, `#210 `_) -* add option to open list of names from command line (`#214 `_) - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* mark rqt_launch and rqt_reconfigure as experimental (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix updating range limits (`#108 `_) -* fix layout quirks (`#150 `_) -* fix icon for closing a node (`#48 `_) -* fix handling of enum parameters with strings - -0.2.17 (2013-07-04) -------------------- -* Improvement; "GUI hangs for awhile or completely, when any one of nodes doesn't return any value" (`#81 `_) - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- -* Fix; Segmentation fault using integer slider (`#63 `_) - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- -* Improve performance significantly upon launch (`#45 `_) - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- -* Add feature to delete of shown nodes feature - -0.2.8 (2013-01-11) ------------------- -* Fix; No Interaction with Boolean values (`#2 `_) - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* renamed rqt_param to rqt_reconfigure (added missing file) -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt deleted file mode 100644 index b4ea6679..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_reconfigure) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_reconfigure - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml b/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml deleted file mode 100644 index f126309d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/package.xml +++ /dev/null @@ -1,38 +0,0 @@ - - rqt_reconfigure - 0.3.13 - This rqt plugin succeeds former dynamic_reconfigure's GUI - (reconfigure_gui), and provides the way to view and edit the parameters - that are accessible via dynamic_reconfigure.
-
- (12/27/2012) In the future, arbitrary parameters that are not associated - with any nodes (which are not handled by dynamic_reconfigure) might - become handled. - However, currently as the name indicates, this pkg solely is dependent - on dynamic_reconfigure that allows access to only those params latched - to nodes. - - Scott K Logan - - BSD - - http://ros.org/wiki/rqt_reconfigure - https://github.com/ros-visualization/rqt_common_plugins/rqt_reconfigure - https://github.com/ros-visualization/rqt_common_plugins/issues - - Isaac Saito - Ze'ev Klapow - - catkin - - dynamic_reconfigure - rospy - rqt_console - rqt_gui - rqt_gui_py - rqt_py_common - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml b/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml deleted file mode 100644 index bd1bcc50..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Python GUI to view and edit dynamic_reconfigure parameters. - - - - - folder - Plugins related to configuration. - - - accessories-text-editor - A Python GUI for viewing and editing dynamic_reconfigure parameters. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui deleted file mode 100644 index ac50dccb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_bool.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 25 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui deleted file mode 100644 index ec2ee66d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_enum.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 30 - - - - Param - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui deleted file mode 100644 index 42da28e5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_number.ui +++ /dev/null @@ -1,108 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param - - - - 0 - - - - - - - param_name - - - - - - - - 30 - 0 - - - - min - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 130 - 0 - - - - 1 - - - Qt::Horizontal - - - - - - - - 30 - 0 - - - - max - - - - - - - - 0 - 0 - - - - - 75 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui deleted file mode 100644 index c23bf3e8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/editor_string.ui +++ /dev/null @@ -1,51 +0,0 @@ - - - _widget_top - - - - 0 - 0 - 393 - 50 - - - - - 300 - 20 - - - - Param string - - - - 0 - - - - - - - TextLabel - - - - - - - - 30 - 20 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui deleted file mode 100644 index 9a129107..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/node_selector.ui +++ /dev/null @@ -1,148 +0,0 @@ - - - _node_selector_pane - - - - 0 - 0 - 581 - 479 - - - - - 0 - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - &Collapse all - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - - 50 - false - - - - &Expand all - - - false - - - false - - - false - - - false - - - - - - - - - - 0 - 0 - - - - QAbstractItemView::MultiSelection - - - true - - - true - - - false - - - - - - - - 0 - 0 - - - - - 75 - 0 - - - - - 50 - false - - - - &Refresh - - - false - - - false - - - false - - - false - - - - - - - - NodeSelectorWidget - QWidget -
rqt_reconfigure.node_selector_widget
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui deleted file mode 100644 index 4ba54196..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_main.ui +++ /dev/null @@ -1,58 +0,0 @@ - - - ParamWidget - - - - 0 - 0 - 513 - 430 - - - - - 0 - 0 - - - - - 300 - 200 - - - - Dynamic Reconfigure - - - - 0 - - - 0 - - - - - - 0 - 0 - - - - QFrame::Panel - - - Qt::Horizontal - - - 2 - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui deleted file mode 100644 index 7a8424ad..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/param_timeline.ui +++ /dev/null @@ -1,31 +0,0 @@ - - - ParamEditWidget - - - - 0 - 0 - 858 - 235 - - - - Param Edit - - - - - - 6 - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui deleted file mode 100644 index 5a5d65ac..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/paramedit_pane.ui +++ /dev/null @@ -1,59 +0,0 @@ - - - _paramedit_widget - - - - 0 - 0 - 666 - 555 - - - - - 0 - 300 - - - - Param - - - - 0 - - - - - - - true - - - - - 0 - 0 - 644 - 533 - - - - - - - - - - - - ParameditWidget - QWidget -
rqt_reconfigure.paramedit_widget
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui deleted file mode 100644 index 97e31025..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/singlenode_parameditor.ui +++ /dev/null @@ -1,484 +0,0 @@ - - - _node_widget - - - - 0 - 0 - 488 - 391 - - - - - 0 - 0 - - - - - 300 - 30 - - - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 170 - 170 - 170 - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 127 - 127 - 127 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 255 - - - - - - - 0 - 0 - 0 - - - - - - - 255 - 255 - 255 - - - - - - - 255 - 255 - 220 - - - - - - - 0 - 0 - 0 - - - - - - - - - 10 - - - - Dynamic Reconfigure - - - - - - true - - - - 75 - true - true - - - - nodename - - - Qt::AlignHCenter|Qt::AlignTop - - - - - - - - - - - GroupWidget - QWidget -
rqt_reconfigure.param_groups
- 1 - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui b/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui deleted file mode 100644 index dc7156da..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/resource/text_filter_widget.ui +++ /dev/null @@ -1,32 +0,0 @@ - - - Form - - - - 0 - 0 - 833 - 30 - - - - Form - - - - 0 - - - - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure b/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure deleted file mode 100755 index 24b15366..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/scripts/rqt_reconfigure +++ /dev/null @@ -1,10 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main -from rqt_reconfigure.param_plugin import ParamPlugin - -plugin = 'rqt_reconfigure.param_plugin.ParamPlugin' -main = Main(filename=plugin) -sys.exit(main.main(sys.argv, standalone=plugin, plugin_argument_provider=ParamPlugin.add_arguments)) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py deleted file mode 100644 index 4e8baeeb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_reconfigure'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/__init__.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py deleted file mode 100644 index e6bc13e5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/dynreconf_client_widget.py +++ /dev/null @@ -1,104 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import rospy - -from .param_editors import EditorWidget -from .param_groups import GroupWidget, find_cfg -from .param_updater import ParamUpdater - - -class DynreconfClientWidget(GroupWidget): - """ - A wrapper of dynamic_reconfigure.client instance. - Represents a widget where users can view and modify ROS params. - """ - - def __init__(self, reconf, node_name): - """ - :type reconf: dynamic_reconfigure.client - :type node_name: str - """ - - group_desc = reconf.get_group_descriptions() - rospy.logdebug('DynreconfClientWidget.group_desc=%s', group_desc) - super(DynreconfClientWidget, self).__init__(ParamUpdater(reconf), - group_desc, node_name) - - self.setMinimumWidth(150) - - self.reconf = reconf - self.updater.start() - self.reconf.config_callback = self.config_callback - self._node_grn = node_name - - def get_node_grn(self): - - return self._node_grn - - def config_callback(self, config): - - #TODO: Think about replacing callback architecture with signals. - - if config: - # TODO: should use config.keys but this method doesnt exist - - names = [name for name, v in config.items()] - # v isn't used but necessary to get key and put it into dict. - rospy.logdebug('config_callback name={} v={}'.format(name, v)) - - for widget in self.editor_widgets: - if isinstance(widget, EditorWidget): - if widget.param_name in names: - rospy.logdebug('EDITOR widget.param_name=%s', - widget.param_name) - widget.update_value(config[widget.param_name]) - elif isinstance(widget, GroupWidget): - cfg = find_cfg(config, widget.param_name) - rospy.logdebug('GROUP widget.param_name=%s', - widget.param_name) - widget.update_group(cfg) - - def close(self): - self.reconf.close() - self.updater.stop() - - for w in self.editor_widgets: - w.close() - - self.deleteLater() - - def filter_param(self, filter_key): - #TODO impl - pass diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py deleted file mode 100644 index c4b6f5a0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/filter_children_model.py +++ /dev/null @@ -1,187 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QSortFilterProxyModel -import rospy - -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem - - -class FilterChildrenModel(QSortFilterProxyModel): - """ - Extending QSortFilterProxyModel, this provides methods to filter children - tree nodes. - - QSortFilterProxyModel filters top-down direction starting from the - top-level of tree, and once a node doesn't hit the query it gets disabled. - Filtering with this class reflects the result from the bottom node. - - Ex. - #TODO example needed here - """ - - # Emitted when parameters filtered. int indicates the order/index of - # params displayed. - sig_filtered = Signal(int) - - def __init__(self, parent): - super(FilterChildrenModel, self).__init__(parent) - - # :Key: Internal ID of QModelIndex of each treenode. - # :Value: TreenodeStatus - # self._treenodes = OrderedDict() - - self._parent = parent - self._toplv_parent_prev = None - - def filterAcceptsRow(self, src_row, src_parent_qmindex): - """ - Overridden. - - Terminology: - "Treenode" is deliberately used to avoid confusion with "Node" in ROS. - - :type src_row: int - :type src_parent_qmindex: QModelIndex - """ - rospy.logdebug('filerAcceptRow 1') - return self._filter_row_recur(src_row, src_parent_qmindex) - - def _filter_row_recur(self, src_row, src_parent_qmindex): - """ - :type src_row: int - :type src_parent_qmindex: QModelIndex - """ - _src_model = self.sourceModel() - curr_qmindex = _src_model.index(src_row, 0, src_parent_qmindex) - curr_qitem = _src_model.itemFromIndex(curr_qmindex) - - if isinstance(curr_qitem, TreenodeQstdItem): - # If selectable ROS Node, get GRN name - nodename_fullpath = curr_qitem.get_raw_param_name() - text_filter_target = nodename_fullpath - rospy.logdebug(' Nodename full={} '.format(nodename_fullpath)) - else: - # If ReadonlyItem, this means items are the parameters, not a part - # of node name. So, get param name. - text_filter_target = curr_qitem.data(Qt.DisplayRole) - - regex = self.filterRegExp() - pos_hit = regex.indexIn(text_filter_target) - if pos_hit >= 0: # Query hit. - rospy.logdebug('curr data={} row={} col={}'.format( - curr_qmindex.data(), - curr_qmindex.row(), - curr_qmindex.column())) - - # Set all subsequent treenodes True - rospy.logdebug(' FCModel.filterAcceptsRow src_row={}'.format( - src_row) + - ' parent row={} data={}'.format( - src_parent_qmindex.row(), - src_parent_qmindex.data()) + - ' filterRegExp={}'.format(regex)) - - # If the index is the terminal treenode, parameters that hit - # the query are displayed at the root tree. - _child_index = curr_qmindex.child(0, 0) - if ((not _child_index.isValid()) and - (isinstance(curr_qitem, TreenodeQstdItem))): - self._show_params_view(src_row, curr_qitem) - - # Once we find a treenode that hits the query, no need to further - # traverse since what this method wants to know with the given - # index is whether the given index is supposed to be shown or not. - # Thus, just return True here. - return True - - if not isinstance(curr_qitem, TreenodeQstdItem): - return False # If parameters, no need for recursive filtering. - - # Evaluate children recursively. - row_child = 0 - while True: - child_qmindex = curr_qmindex.child(row_child, 0) - if child_qmindex.isValid(): - flag = self._filter_row_recur(row_child, curr_qmindex) - if flag: - return True - else: - return False - row_child += 1 - return False - - def _show_params_view(self, src_row, curr_qitem): - """ - :type curr_qitem: QStandardItem - """ - - rospy.logdebug('_show_params_view data={}'.format( - curr_qitem.data(Qt.DisplayRole))) - curr_qitem.enable_param_items() - - def _get_toplevel_parent_recur(self, qmindex): - p = qmindex.parent() - if p.isValid(): - self._get_toplevel_parent(p) - return p - - def filterAcceptsColumn(self, source_column, source_parent): - """ - Overridden. - - Doing nothing really since columns are not in use. - - :type source_column: int - :type source_parent: QModelIndex - """ - rospy.logdebug('FCModel.filterAcceptsCol source_col={} '.format( - source_column) + 'parent col={} row={} data={}'.format( - source_parent.column(), source_parent.row(), source_parent.data())) - return True - - def set_filter(self, filter_): - self._filter = filter_ - - # If filtered text is '' (0-length str), invalidate current - # filtering, in the hope of making filtering process faster. - if filter_.get_text == '': - self.invalidate() - rospy.loginfo('filter invalidated.') - - # By calling setFilterRegExp, filterAccepts* methods get kicked. - self.setFilterRegExp(self._filter.get_regexp()) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py deleted file mode 100644 index 7dda8dc4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/node_selector_widget.py +++ /dev/null @@ -1,477 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from collections import OrderedDict -import os -import time - -import dynamic_reconfigure as dyn_reconf -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import (QHeaderView, QItemSelectionModel, - QWidget) -import rospy -from rospy.exceptions import ROSException -import rosservice - -from rqt_py_common.rqt_ros_graph import RqtRosGraph -from rqt_reconfigure.filter_children_model import FilterChildrenModel -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem -from rqt_reconfigure.treenode_item_model import TreenodeItemModel - -from rqt_reconfigure.dynreconf_client_widget import DynreconfClientWidget - - -class NodeSelectorWidget(QWidget): - _COL_NAMES = ['Node'] - - # public signal - sig_node_selected = Signal(DynreconfClientWidget) - - def __init__(self, parent, rospack, signal_msg=None): - """ - @param signal_msg: Signal to carries a system msg that is shown on GUI. - @type signal_msg: QtCore.Signal - """ - super(NodeSelectorWidget, self).__init__() - self._parent = parent - self.stretch = None - self._signal_msg = signal_msg - - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource', - 'node_selector.ui') - loadUi(ui_file, self) - - # List of the available nodes. Since the list should be updated over - # time and we don't want to create node instance per every update - # cycle, This list instance should better be capable of keeping track. - self._nodeitems = OrderedDict() - # Dictionary. 1st elem is node's GRN name, - # 2nd is TreenodeQstdItem instance. - # TODO: Needs updated when nodes list updated. - - # Setup treeview and models - self._item_model = TreenodeItemModel() - self._rootitem = self._item_model.invisibleRootItem() # QStandardItem - - self._nodes_previous = None - - # Calling this method updates the list of the node. - # Initially done only once. - self._update_nodetree_pernode() - - # TODO(Isaac): Needs auto-update function enabled, once another - # function that updates node tree with maintaining - # collapse/expansion state. http://goo.gl/GuwYp can be a - # help. - - self._collapse_button.pressed.connect( - self._node_selector_view.collapseAll) - self._expand_button.pressed.connect(self._node_selector_view.expandAll) - self._refresh_button.pressed.connect(self._refresh_nodes) - - # Filtering preparation. - self._proxy_model = FilterChildrenModel(self) - self._proxy_model.setDynamicSortFilter(True) - self._proxy_model.setSourceModel(self._item_model) - self._node_selector_view.setModel(self._proxy_model) - self._filterkey_prev = '' - - # This 1 line is needed to enable horizontal scrollbar. This setting - # isn't available in .ui file. - # Ref. http://stackoverflow.com/a/6648906/577001 - self._node_selector_view.header().setResizeMode( - 0, QHeaderView.ResizeToContents) - - # Setting slot for when user clicks on QTreeView. - self.selectionModel = self._node_selector_view.selectionModel() - # Note: self.selectionModel.currentChanged doesn't work to deselect - # a treenode as expected. Need to use selectionChanged. - self.selectionModel.selectionChanged.connect( - self._selection_changed_slot) - - def node_deselected(self, grn): - """ - Deselect the index that corresponds to the given GRN. - - :type grn: str - """ - - # Obtain the corresponding index. - qindex_tobe_deselected = self._item_model.get_index_from_grn(grn) - rospy.logdebug('NodeSelWidt node_deselected qindex={} data={}'.format( - qindex_tobe_deselected, - qindex_tobe_deselected.data(Qt.DisplayRole))) - - # Obtain all indices currently selected. - indexes_selected = self.selectionModel.selectedIndexes() - for index in indexes_selected: - grn_from_selectedindex = RqtRosGraph.get_upper_grn(index, '') - rospy.logdebug(' Compare given grn={} grn from selected={}'.format( - grn, grn_from_selectedindex)) - # If GRN retrieved from selected index matches the given one. - if grn == grn_from_selectedindex: - # Deselect the index. - self.selectionModel.select(index, QItemSelectionModel.Deselect) - - def node_selected(self, grn): - """ - Select the index that corresponds to the given GRN. - - :type grn: str - """ - - # Obtain the corresponding index. - qindex_tobe_selected = self._item_model.get_index_from_grn(grn) - rospy.logdebug('NodeSelWidt node_selected qindex={} data={}'.format( - qindex_tobe_selected, - qindex_tobe_selected.data(Qt.DisplayRole))) - - - # Select the index. - if qindex_tobe_selected: - self.selectionModel.select(qindex_tobe_selected, QItemSelectionModel.Select) - - def _selection_deselected(self, index_current, rosnode_name_selected): - """ - Intended to be called from _selection_changed_slot. - """ - self.selectionModel.select(index_current, QItemSelectionModel.Deselect) - - try: - reconf_widget = self._nodeitems[ - rosnode_name_selected].get_dynreconf_widget() - except ROSException as e: - raise e - - # Signal to notify other pane that also contains node widget. - self.sig_node_selected.emit(reconf_widget) - #self.sig_node_selected.emit(self._nodeitems[rosnode_name_selected]) - - def _selection_selected(self, index_current, rosnode_name_selected): - """Intended to be called from _selection_changed_slot.""" - rospy.logdebug('_selection_changed_slot row={} col={} data={}'.format( - index_current.row(), index_current.column(), - index_current.data(Qt.DisplayRole))) - - # Determine if it's terminal treenode. - found_node = False - for nodeitem in self._nodeitems.itervalues(): - name_nodeitem = nodeitem.data(Qt.DisplayRole) - name_rosnode_leaf = rosnode_name_selected[ - rosnode_name_selected.rfind(RqtRosGraph.DELIM_GRN) + 1:] - - # If name of the leaf in the given name & the name taken from - # nodeitem list matches. - if ((name_nodeitem == rosnode_name_selected) and - (name_nodeitem[name_nodeitem.rfind(RqtRosGraph.DELIM_GRN) + 1:] - == name_rosnode_leaf)): - - rospy.logdebug('terminal str {} MATCH {}'.format( - name_nodeitem, name_rosnode_leaf)) - found_node = True - break - if not found_node: # Only when it's NOT a terminal we deselect it. - self.selectionModel.select(index_current, - QItemSelectionModel.Deselect) - return - - # Only when it's a terminal we move forward. - - item_child = self._nodeitems[rosnode_name_selected] - item_widget = None - try: - item_widget = item_child.get_dynreconf_widget() - except ROSException as e: - raise e - rospy.logdebug('item_selected={} child={} widget={}'.format( - index_current, item_child, item_widget)) - self.sig_node_selected.emit(item_widget) - - # Show the node as selected. - #selmodel.select(index_current, QItemSelectionModel.SelectCurrent) - - def _selection_changed_slot(self, selected, deselected): - """ - Sends "open ROS Node box" signal ONLY IF the selected treenode is the - terminal treenode. - Receives args from signal QItemSelectionModel.selectionChanged. - - :param selected: All indexs where selected (could be multiple) - :type selected: QItemSelection - :type deselected: QItemSelection - """ - - ## Getting the index where user just selected. Should be single. - if not selected.indexes() and not deselected.indexes(): - rospy.logerr('Nothing selected? Not ideal to reach here') - return - - index_current = None - if selected.indexes(): - index_current = selected.indexes()[0] - elif len(deselected.indexes()) == 1: - # Setting length criteria as 1 is only a workaround, to avoid - # Node boxes on right-hand side disappears when filter key doesn't - # match them. - # Indeed this workaround leaves another issue. Question for - # permanent solution is asked here http://goo.gl/V4DT1 - index_current = deselected.indexes()[0] - - rospy.logdebug(' - - - index_current={}'.format(index_current)) - - rosnode_name_selected = RqtRosGraph.get_upper_grn(index_current, '') - - # If retrieved node name isn't in the list of all nodes. - if not rosnode_name_selected in self._nodeitems.keys(): - # De-select the selected item. - self.selectionModel.select(index_current, - QItemSelectionModel.Deselect) - return - - if selected.indexes(): - try: - self._selection_selected(index_current, rosnode_name_selected) - except ROSException as e: - #TODO: print to sysmsg pane - err_msg = e.message + '. Connection to node=' + \ - format(rosnode_name_selected) + ' failed' - self._signal_msg.emit(err_msg) - rospy.logerr(err_msg) - - elif deselected.indexes(): - try: - self._selection_deselected(index_current, - rosnode_name_selected) - except ROSException as e: - rospy.logerr(e.message) - #TODO: print to sysmsg pane - - def get_paramitems(self): - """ - :rtype: OrderedDict 1st elem is node's GRN name, - 2nd is TreenodeQstdItem instance - """ - return self._nodeitems - - def _update_nodetree_pernode(self): - """ - """ - - # TODO(Isaac): 11/25/2012 dynamic_reconfigure only returns params that - # are associated with nodes. In order to handle independent - # params, different approach needs taken. - try: - nodes = dyn_reconf.find_reconfigure_services() - except rosservice.ROSServiceIOException as e: - rospy.logerr("Reconfigure GUI cannot connect to master.") - raise e # TODO Make sure 'raise' here returns or finalizes func. - - if not nodes == self._nodes_previous: - i_node_curr = 1 - num_nodes = len(nodes) - elapsedtime_overall = 0.0 - for node_name_grn in nodes: - # Skip this grn if we already have it - if node_name_grn in self._nodeitems: - i_node_curr += 1 - continue - - time_siglenode_loop = time.time() - - ####(Begin) For DEBUG ONLY; skip some dynreconf creation -# if i_node_curr % 2 != 0: -# i_node_curr += 1 -# continue - #### (End) For DEBUG ONLY. #### - - # Instantiate QStandardItem. Inside, dyn_reconf client will - # be generated too. - treenodeitem_toplevel = TreenodeQstdItem( - node_name_grn, TreenodeQstdItem.NODE_FULLPATH) - _treenode_names = treenodeitem_toplevel.get_treenode_names() - - try: - treenodeitem_toplevel.connect_param_server() - except rospy.exceptions.ROSException as e: - rospy.logerr(e.message) - #Skip item that fails to connect to its node. - continue - #TODO: Needs to show err msg on GUI too. - - # Using OrderedDict here is a workaround for StdItemModel - # not returning corresponding item to index. - self._nodeitems[node_name_grn] = treenodeitem_toplevel - - self._add_children_treenode(treenodeitem_toplevel, - self._rootitem, _treenode_names) - - time_siglenode_loop = time.time() - time_siglenode_loop - elapsedtime_overall += time_siglenode_loop - - _str_progress = 'reconf ' + \ - 'loading #{}/{} {} / {}sec node={}'.format( - i_node_curr, num_nodes, round(time_siglenode_loop, 2), - round(elapsedtime_overall, 2), node_name_grn) - - # NOT a debug print - please DO NOT remove. This print works - # as progress notification when loading takes long time. - rospy.logdebug(_str_progress) - i_node_curr += 1 - - def _add_children_treenode(self, treenodeitem_toplevel, - treenodeitem_parent, child_names_left): - """ - Evaluate current treenode and the previous treenode at the same depth. - If the name of both nodes is the same, current node instance is - ignored (that means children will be added to the same parent). If not, - the current node gets added to the same parent node. At the end, this - function gets called recursively going 1 level deeper. - - :type treenodeitem_toplevel: TreenodeQstdItem - :type treenodeitem_parent: TreenodeQstdItem. - :type child_names_left: List of str - :param child_names_left: List of strings that is sorted in hierarchical - order of params. - """ - # TODO(Isaac): Consider moving this method to rqt_py_common. - - name_currentnode = child_names_left.pop(0) - grn_curr = treenodeitem_toplevel.get_raw_param_name() - stditem_currentnode = TreenodeQstdItem(grn_curr, - TreenodeQstdItem.NODE_FULLPATH) - - # item at the bottom is your most recent node. - row_index_parent = treenodeitem_parent.rowCount() - 1 - - # Obtain and instantiate prev node in the same depth. - name_prev = '' - stditem_prev = None - if treenodeitem_parent.child(row_index_parent): - stditem_prev = treenodeitem_parent.child(row_index_parent) - name_prev = stditem_prev.text() - - stditem = None - # If the name of both nodes is the same, current node instance is - # ignored (that means children will be added to the same parent) - if name_prev != name_currentnode: - stditem_currentnode.setText(name_currentnode) - - # Arrange alphabetically by display name - insert_index = 0 - while insert_index < treenodeitem_parent.rowCount() and treenodeitem_parent.child(insert_index).text() < name_currentnode: - insert_index += 1 - - treenodeitem_parent.insertRow(insert_index, stditem_currentnode) - stditem = stditem_currentnode - else: - stditem = stditem_prev - - if child_names_left: - # TODO: Model is closely bound to a certain type of view (treeview) - # here. Ideally isolate those two. Maybe we should split into 2 - # class, 1 handles view, the other does model. - self._add_children_treenode(treenodeitem_toplevel, stditem, - child_names_left) - else: # Selectable ROS Node. - #TODO: Accept even non-terminal treenode as long as it's ROS Node. - self._item_model.set_item_from_index(grn_curr, stditem.index()) - - def _prune_nodetree_pernode(self): - try: - nodes = dyn_reconf.find_reconfigure_services() - except rosservice.ROSServiceIOException as e: - rospy.logerr("Reconfigure GUI cannot connect to master.") - raise e # TODO Make sure 'raise' here returns or finalizes func. - - for i in reversed(range(0, self._rootitem.rowCount())): - candidate_for_removal = self._rootitem.child(i).get_raw_param_name() - if not candidate_for_removal in nodes: - rospy.logdebug('Removing {} because the server is no longer available.'.format( - candidate_for_removal)) - self._nodeitems[candidate_for_removal].disconnect_param_server() - self._rootitem.removeRow(i) - self._nodeitems.pop(candidate_for_removal) - - def _refresh_nodes(self): - self._prune_nodetree_pernode() - self._update_nodetree_pernode() - - def close_node(self): - rospy.logdebug(" in close_node") - # TODO(Isaac) Figure out if dynamic_reconfigure needs to be closed. - - def set_filter(self, filter_): - """ - Pass fileter instance to the child proxymodel. - :type filter_: BaseFilter - """ - self._proxy_model.set_filter(filter_) - - def _test_sel_index(self, selected, deselected): - """ - Method for Debug only - """ - #index_current = self.selectionModel.currentIndex() - src_model = self._item_model - index_current = None - index_deselected = None - index_parent = None - curr_qstd_item = None - if selected.indexes(): - index_current = selected.indexes()[0] - index_parent = index_current.parent() - curr_qstd_item = src_model.itemFromIndex(index_current) - elif deselected.indexes(): - index_deselected = deselected.indexes()[0] - index_parent = index_deselected.parent() - curr_qstd_item = src_model.itemFromIndex(index_deselected) - - if selected.indexes() > 0: - rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format( - index_current, index_parent, index_deselected, - index_current.data(Qt.DisplayRole), - index_parent.data(Qt.DisplayRole),) - + ' desel.d={} cur.item={}'.format( - None, # index_deselected.data(Qt.DisplayRole) - curr_qstd_item)) - elif deselected.indexes(): - rospy.logdebug('sel={} par={} desel={} sel.d={} par.d={}'.format( - index_current, index_parent, index_deselected, - None, index_parent.data(Qt.DisplayRole)) + - ' desel.d={} cur.item={}'.format( - index_deselected.data(Qt.DisplayRole), - curr_qstd_item)) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py deleted file mode 100644 index 55196fc4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_editors.py +++ /dev/null @@ -1,446 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import math -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Signal -from python_qt_binding.QtGui import (QDoubleValidator, QIntValidator, QLabel, - QMenu, QWidget) -from decimal import Decimal -import rospkg -import rospy - -EDITOR_TYPES = { - 'bool': 'BooleanEditor', - 'str': 'StringEditor', - 'int': 'IntegerEditor', - 'double': 'DoubleEditor', -} - -# These .ui files are frequently loaded multiple times. Since file access -# costs a lot, only load each file once. -rp = rospkg.RosPack() -ui_bool = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_bool.ui') -ui_str = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_string.ui') -ui_num = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_number.ui') -ui_int = ui_num -ui_enum = os.path.join(rp.get_path('rqt_reconfigure'), 'resource', - 'editor_enum.ui') - - -class EditorWidget(QWidget): - ''' - This class is abstract -- its child classes should be instantiated. - - There exist two kinds of "update" methods: - - _update_paramserver for Parameter Server. - - update_value for the value displayed on GUI. - ''' - - def __init__(self, updater, config): - ''' - @param updater: A class that extends threading.Thread. - @type updater: rqt_reconfigure.param_updater.ParamUpdater - ''' - - super(EditorWidget, self).__init__() - - self._updater = updater - self.param_name = config['name'] - self.param_default = config['default'] - self.param_description = config['description'] - - self.old_value = None - - self.cmenu = QMenu() - self.cmenu.addAction(self.tr('Set to Default')).triggered.connect(self._set_to_default) - - def _update_paramserver(self, value): - ''' - Update the value on Parameter Server. - ''' - if value != self.old_value: - self.update_configuration(value) - self.old_value = value - - def update_value(self, value): - ''' - To be implemented in subclass, but still used. - - Update the value that's displayed on the arbitrary GUI component - based on user's input. - - This method is not called from the GUI thread, so any changes to - QObjects will need to be done through a signal. - ''' - self.old_value = value - - def update_configuration(self, value): - self._updater.update({self.param_name: value}) - - def display(self, grid): - ''' - Should be overridden in subclass. - - :type grid: QFormLayout - ''' - self._paramname_label.setText(self.param_name) -# label_paramname = QLabel(self.param_name) -# label_paramname.setWordWrap(True) - self._paramname_label.setMinimumWidth(100) - grid.addRow(self._paramname_label, self) - self.setToolTip(self.param_description) - self._paramname_label.setToolTip(self.param_description) - self._paramname_label.contextMenuEvent = self.contextMenuEvent - - def close(self): - ''' - Should be overridden in subclass. - ''' - pass - - def _set_to_default(self): - self._update_paramserver(self.param_default) - - def contextMenuEvent(self, e): - self.cmenu.exec_(e.globalPos()) - - -class BooleanEditor(EditorWidget): - _update_signal = Signal(bool) - - def __init__(self, updater, config): - super(BooleanEditor, self).__init__(updater, config) - loadUi(ui_bool, self) - - # Initialize to default - self.update_value(config['default']) - - # Make checkbox update param server - self._checkbox.stateChanged.connect(self._box_checked) - - # Make param server update checkbox - self._update_signal.connect(self._checkbox.setChecked) - - def _box_checked(self, value): - self._update_paramserver(bool(value)) - - def update_value(self, value): - super(BooleanEditor, self).update_value(value) - self._update_signal.emit(value) - - -class StringEditor(EditorWidget): - _update_signal = Signal(str) - - def __init__(self, updater, config): - super(StringEditor, self).__init__(updater, config) - loadUi(ui_str, self) - - self._paramval_lineedit.setText(config['default']) - - # Update param server when cursor leaves the text field - # or enter is pressed. - self._paramval_lineedit.editingFinished.connect(self.edit_finished) - - # Make param server update text field - self._update_signal.connect(self._paramval_lineedit.setText) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Empty String')).triggered.connect(self._set_to_empty) - - def update_value(self, value): - super(StringEditor, self).update_value(value) - rospy.logdebug('StringEditor update_value={}'.format(value)) - self._update_signal.emit(value) - - def edit_finished(self): - rospy.logdebug('StringEditor edit_finished val={}'.format( - self._paramval_lineedit.text())) - self._update_paramserver(self._paramval_lineedit.text()) - - def _set_to_empty(self): - self._update_paramserver('') - - -class IntegerEditor(EditorWidget): - _update_signal = Signal(int) - - def __init__(self, updater, config): - super(IntegerEditor, self).__init__(updater, config) - loadUi(ui_int, self) - - # Set ranges - self._min = int(config['min']) - self._max = int(config['max']) - self._min_val_label.setText(str(self._min)) - self._max_val_label.setText(str(self._max)) - self._slider_horizontal.setRange(self._min, self._max) - - # TODO: Fix that the naming of _paramval_lineEdit instance is not - # consistent among Editor's subclasses. - self._paramval_lineEdit.setValidator(QIntValidator(self._min, - self._max, self)) - - # Initialize to default - self._paramval_lineEdit.setText(str(config['default'])) - self._slider_horizontal.setValue(int(config['default'])) - - # Make slider update text (locally) - self._slider_horizontal.sliderMoved.connect(self._slider_moved) - - # Make keyboard input change slider position and update param server - self._paramval_lineEdit.editingFinished.connect(self._text_changed) - - # Make slider update param server - # Turning off tracking means this isn't called during a drag - self._slider_horizontal.setTracking(False) - self._slider_horizontal.valueChanged.connect(self._slider_changed) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max) - self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min) - - def _slider_moved(self): - # This is a "local" edit - only change the text - self._paramval_lineEdit.setText(str( - self._slider_horizontal.sliderPosition())) - - def _text_changed(self): - # This is a final change - update param server - # No need to update slider... update_value() will - self._update_paramserver(int(self._paramval_lineEdit.text())) - - def _slider_changed(self): - # This is a final change - update param server - # No need to update text... update_value() will - self._update_paramserver(self._slider_horizontal.value()) - - def update_value(self, value): - super(IntegerEditor, self).update_value(value) - self._update_signal.emit(int(value)) - - def _update_gui(self, value): - # Block all signals so we don't loop - self._slider_horizontal.blockSignals(True) - # Update the slider value - self._slider_horizontal.setValue(value) - # Make the text match - self._paramval_lineEdit.setText(str(value)) - self._slider_horizontal.blockSignals(False) - - def _set_to_max(self): - self._update_paramserver(self._max) - - def _set_to_min(self): - self._update_paramserver(self._min) - - -class DoubleEditor(EditorWidget): - _update_signal = Signal(float) - - def __init__(self, updater, config): - super(DoubleEditor, self).__init__(updater, config) - loadUi(ui_num, self) - - # Handle unbounded doubles nicely - if config['min'] != -float('inf'): - self._min = float(config['min']) - self._min_val_label.setText(str(self._min)) - else: - self._min = -1e10000 - self._min_val_label.setText('-inf') - - if config['max'] != float('inf'): - self._max = float(config['max']) - self._max_val_label.setText(str(self._max)) - else: - self._max = 1e10000 - self._max_val_label.setText('inf') - - if config['min'] != -float('inf') and config['max'] != float('inf'): - self._func = lambda x: x - self._ifunc = self._func - else: - self._func = lambda x: math.atan(x) - self._ifunc = lambda x: math.tan(x) - - # If we have no range, disable the slider - self.scale = (self._func(self._max) - self._func(self._min)) - if self.scale <= 0: - self.scale = 0 - self.setDisabled(True) - else: - self.scale = 100 / self.scale - - # Set ranges - self._slider_horizontal.setRange(self._get_value_slider(self._min), - self._get_value_slider(self._max)) - self._paramval_lineEdit.setValidator(QDoubleValidator( - self._min, self._max, - 8, self)) - - # Initialize to defaults - self._paramval_lineEdit.setText(str(config['default'])) - self._slider_horizontal.setValue( - self._get_value_slider(config['default'])) - - # Make slider update text (locally) - self._slider_horizontal.sliderMoved.connect(self._slider_moved) - - # Make keyboard input change slider position and update param server - self._paramval_lineEdit.editingFinished.connect(self._text_changed) - - # Make slider update param server - # Turning off tracking means this isn't called during a drag - self._slider_horizontal.setTracking(False) - self._slider_horizontal.valueChanged.connect(self._slider_changed) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Add special menu items - self.cmenu.addAction(self.tr('Set to Maximum')).triggered.connect(self._set_to_max) - self.cmenu.addAction(self.tr('Set to Minimum')).triggered.connect(self._set_to_min) - self.cmenu.addAction(self.tr('Set to NaN')).triggered.connect(self._set_to_nan) - - def _slider_moved(self): - # This is a "local" edit - only change the text - self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str( - self._get_value_textfield())))) - - def _text_changed(self): - # This is a final change - update param server - # No need to update slider... update_value() will - self._update_paramserver(float(self._paramval_lineEdit.text())) - - def _slider_changed(self): - # This is a final change - update param server - # No need to update text... update_value() will - self._update_paramserver(self._get_value_textfield()) - - def _get_value_textfield(self): - '''@return: Current value in text field.''' - return self._ifunc(self._slider_horizontal.sliderPosition() / - self.scale) if self.scale else 0 - - def _get_value_slider(self, value): - ''' - @rtype: double - ''' - return int(round((self._func(value)) * self.scale)) - - def update_value(self, value): - super(DoubleEditor, self).update_value(value) - self._update_signal.emit(float(value)) - - def _update_gui(self, value): - # Block all signals so we don't loop - self._slider_horizontal.blockSignals(True) - # Update the slider value if not NaN - if not math.isnan(value): - self._slider_horizontal.setValue(self._get_value_slider(value)) - elif not math.isnan(self.param_default): - self._slider_horizontal.setValue(self._get_value_slider(self.param_default)) - # Make the text match - self._paramval_lineEdit.setText('{0:f}'.format(Decimal(str(value)))) - self._slider_horizontal.blockSignals(False) - - def _set_to_max(self): - self._update_paramserver(self._max) - - def _set_to_min(self): - self._update_paramserver(self._min) - - def _set_to_nan(self): - self._update_paramserver(float('NaN')) - - -class EnumEditor(EditorWidget): - _update_signal = Signal(int) - - def __init__(self, updater, config): - super(EnumEditor, self).__init__(updater, config) - - loadUi(ui_enum, self) - - try: - enum = eval(config['edit_method'])['enum'] - except: - rospy.logerr("reconfig EnumEditor) Malformed enum") - return - - # Setup the enum items - self.names = [item['name'] for item in enum] - self.values = [item['value'] for item in enum] - - items = ["%s (%s)" % (self.names[i], self.values[i]) - for i in range(0, len(self.names))] - - # Add items to the combo box - self._combobox.addItems(items) - - # Initialize to the default - self._combobox.setCurrentIndex(self.values.index(config['default'])) - - # Make selection update the param server - self._combobox.currentIndexChanged['int'].connect(self.selected) - - # Make the param server update selection - self._update_signal.connect(self._update_gui) - - # Bind the context menu - self._combobox.contextMenuEvent = self.contextMenuEvent - - def selected(self, index): - self._update_paramserver(self.values[index]) - - def update_value(self, value): - super(EnumEditor, self).update_value(value) - self._update_signal.emit(self.values.index(value)) - - def _update_gui(self, idx): - # Block all signals so we don't loop - self._combobox.blockSignals(True) - self._combobox.setCurrentIndex(idx) - self._combobox.blockSignals(False) - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py deleted file mode 100644 index b05bf241..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_groups.py +++ /dev/null @@ -1,329 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import time - -from python_qt_binding.QtCore import QSize, Qt, Signal, QMargins -from python_qt_binding.QtGui import (QFont, QFormLayout, QHBoxLayout, QIcon, - QGroupBox, QLabel, QPushButton, - QTabWidget, QVBoxLayout, QWidget) -import rospy - -# *Editor classes that are not explicitly used within this .py file still need -# to be imported. They are invoked implicitly during runtime. -from .param_editors import BooleanEditor, DoubleEditor, EditorWidget, \ - EDITOR_TYPES, EnumEditor, IntegerEditor, \ - StringEditor - -_GROUP_TYPES = { - '': 'BoxGroup', - 'collapse': 'CollapseGroup', - 'tab': 'TabGroup', - 'hide': 'HideGroup', - 'apply': 'ApplyGroup', -} - - -def find_cfg(config, name): - ''' - (Ze'ev) reaaaaallly cryptic function which returns the config object for - specified group. - ''' - cfg = None - for k, v in config.items(): - try: - if k.lower() == name.lower(): - cfg = v - return cfg - else: - try: - cfg = find_cfg(v, name) - if cfg: - return cfg - except Exception as exc: - raise exc - except AttributeError: - pass - except Exception as exc: - raise exc - return cfg - - -class GroupWidget(QWidget): - ''' - (Isaac's guess as of 12/13/2012) - This class bonds multiple Editor instances that are associated with - a single node as a group. - ''' - - # public signal - sig_node_disabled_selected = Signal(str) - sig_node_state_change = Signal(bool) - - def __init__(self, updater, config, nodename): - ''' - :param config: - :type config: Dictionary? defined in dynamic_reconfigure.client.Client - :type nodename: str - ''' - - super(GroupWidget, self).__init__() - self.state = config['state'] - self.param_name = config['name'] - self._toplevel_treenode_name = nodename - - # TODO: .ui file needs to be back into usage in later phase. -# ui_file = os.path.join(rp.get_path('rqt_reconfigure'), -# 'resource', 'singlenode_parameditor.ui') -# loadUi(ui_file, self) - - verticalLayout = QVBoxLayout(self) - verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0)) - - _widget_nodeheader = QWidget() - _h_layout_nodeheader = QHBoxLayout(_widget_nodeheader) - _h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0)) - - self.nodename_qlabel = QLabel(self) - font = QFont('Trebuchet MS, Bold') - font.setUnderline(True) - font.setBold(True) - - # Button to close a node. - _icon_disable_node = QIcon.fromTheme('window-close') - _bt_disable_node = QPushButton(_icon_disable_node, '', self) - _bt_disable_node.setToolTip('Hide this node') - _bt_disable_node_size = QSize(36, 24) - _bt_disable_node.setFixedSize(_bt_disable_node_size) - _bt_disable_node.pressed.connect(self._node_disable_bt_clicked) - - _h_layout_nodeheader.addWidget(self.nodename_qlabel) - _h_layout_nodeheader.addWidget(_bt_disable_node) - - self.nodename_qlabel.setAlignment(Qt.AlignCenter) - font.setPointSize(10) - self.nodename_qlabel.setFont(font) - grid_widget = QWidget(self) - self.grid = QFormLayout(grid_widget) - verticalLayout.addWidget(_widget_nodeheader) - verticalLayout.addWidget(grid_widget, 1) - # Again, these UI operation above needs to happen in .ui file. - - self.tab_bar = None # Every group can have one tab bar - self.tab_bar_shown = False - - self.updater = updater - - self.editor_widgets = [] - self._param_names = [] - - self._create_node_widgets(config) - - rospy.logdebug('Groups node name={}'.format(nodename)) - self.nodename_qlabel.setText(nodename) - - # Labels should not stretch - #self.grid.setColumnStretch(1, 1) - #self.setLayout(self.grid) - - def collect_paramnames(self, config): - pass - - def _create_node_widgets(self, config): - ''' - :type config: Dict? - ''' - i_debug = 0 - for param in config['parameters']: - begin = time.time() * 1000 - editor_type = '(none)' - - if param['edit_method']: - widget = EnumEditor(self.updater, param) - elif param['type'] in EDITOR_TYPES: - rospy.logdebug('GroupWidget i_debug=%d param type =%s', - i_debug, - param['type']) - editor_type = EDITOR_TYPES[param['type']] - widget = eval(editor_type)(self.updater, param) - - self.editor_widgets.append(widget) - self._param_names.append(param['name']) - - rospy.logdebug('groups._create_node_widgets num editors=%d', - i_debug) - - end = time.time() * 1000 - time_elap = end - begin - rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format( - editor_type, i_debug, time_elap)) - i_debug += 1 - - for name, group in config['groups'].items(): - if group['type'] == 'tab': - widget = TabGroup(self, self.updater, group, self._toplevel_treenode_name) - elif group['type'] in _GROUP_TYPES.keys(): - widget = eval(_GROUP_TYPES[group['type']])(self.updater, group, self._toplevel_treenode_name) - else: - widget = eval(_GROUP_TYPES[''])(self.updater, group, self._toplevel_treenode_name) - - self.editor_widgets.append(widget) - rospy.logdebug('groups._create_node_widgets ' + - 'name=%s', - name) - - for i, ed in enumerate(self.editor_widgets): - ed.display(self.grid) - - rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d', - len(self.editor_widgets)) - - def display(self, grid): - grid.addRow(self) - - def update_group(self, config): - if 'state' in config: - old_state = self.state - self.state = config['state'] - if self.state != old_state: - self.sig_node_state_change.emit(self.state) - - names = [name for name in config.keys()] - - for widget in self.editor_widgets: - if isinstance(widget, EditorWidget): - if widget.param_name in names: - widget.update_value(config[widget.param_name]) - elif isinstance(widget, GroupWidget): - cfg = find_cfg(config, widget.param_name) - widget.update_group(cfg) - - def close(self): - for w in self.editor_widgets: - w.close() - - def get_treenode_names(self): - ''' - :rtype: str[] - ''' - return self._param_names - - def _node_disable_bt_clicked(self): - rospy.logdebug('param_gs _node_disable_bt_clicked') - self.sig_node_disabled_selected.emit(self._toplevel_treenode_name) - - -class BoxGroup(GroupWidget): - def __init__(self, updater, config, nodename): - super(BoxGroup, self).__init__(updater, config, nodename) - - self.box = QGroupBox(self.param_name) - self.box.setLayout(self.grid) - - def display(self, grid): - grid.addRow(self.box) - - -class CollapseGroup(BoxGroup): - def __init__(self, updater, config, nodename): - super(CollapseGroup, self).__init__(updater, config, nodename) - self.box.setCheckable(True) - self.box.clicked.connect(self.click_cb) - self.sig_node_state_change.connect(self.box.setChecked) - - for child in self.box.children(): - if child.isWidgetType(): - self.box.toggled.connect(child.setShown) - - self.box.setChecked(self.state) - - def click_cb(self, on): - self.updater.update({'groups': {self.param_name: on}}) - - -class HideGroup(BoxGroup): - def __init__(self, updater, config, nodename): - super(HideGroup, self).__init__(updater, config, nodename) - self.box.setShown(self.state) - self.sig_node_state_change.connect(self.box.setShown) - - -class TabGroup(GroupWidget): - def __init__(self, parent, updater, config, nodename): - super(TabGroup, self).__init__(updater, config, nodename) - self.parent = parent - - if not self.parent.tab_bar: - self.parent.tab_bar = QTabWidget() - - self.wid = QWidget() - self.wid.setLayout(self.grid) - - parent.tab_bar.addTab(self.wid, self.param_name) - - def display(self, grid): - if not self.parent.tab_bar_shown: - grid.addRow(self.parent.tab_bar) - self.parent.tab_bar_shown = True - - def close(self): - super(TabGroup, self).close() - self.parent.tab_bar = None - self.parent.tab_bar_shown = False - - -class ApplyGroup(BoxGroup): - class ApplyUpdater: - def __init__(self, updater, loopback): - self.updater = updater - self.loopback = loopback - self._configs_pending = {} - - def update(self, config): - for name, value in config.items(): - self._configs_pending[name] = value - self.loopback(config) - - def apply_update(self): - self.updater.update(self._configs_pending) - self._configs_pending = {} - - def __init__(self, updater, config, nodename): - self.updater = ApplyGroup.ApplyUpdater(updater, self.update_group) - super(ApplyGroup, self).__init__(self.updater, config, nodename) - - self.button = QPushButton('Apply %s' % self.param_name) - self.button.clicked.connect(self.updater.apply_update) - - self.grid.addRow(self.button) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py deleted file mode 100644 index 5ca3c670..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_plugin.py +++ /dev/null @@ -1,71 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from rqt_gui_py.plugin import Plugin -from rqt_py_common.plugin_container_widget import PluginContainerWidget - -from .param_widget import ParamWidget - - -class ParamPlugin(Plugin): - - def __init__(self, context): - """ - :type context: qt_gui.PluginContext - """ - - super(ParamPlugin, self).__init__(context) - self.setObjectName('Dynamic Reconfigure') - - self._plugin_widget = ParamWidget(context) - self._widget = PluginContainerWidget(self._plugin_widget, True, False) - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + - (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.shutdown() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - @staticmethod - def add_arguments(parser): - group = parser.add_argument_group('Options for rqt_reconfigure plugin') - group.add_argument('node_name', nargs='*', default=[], help='Node(s) to open automatically') - diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py deleted file mode 100644 index d71a18de..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_updater.py +++ /dev/null @@ -1,107 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import threading -import time - -import rospy - - -class ParamUpdater(threading.Thread): - ''' - Using dynamic_reconfigure that is passed in __init__, this thread updates - the Dynamic Reconfigure server's value. - - This class works for a single element in a single parameter. - ''' - - #TODO: Modify variable names to the ones that's more intuitive. - - def __init__(self, reconf): - """ - :type reconf: dynamic_reconfigure - """ - super(ParamUpdater, self).__init__() - self.setDaemon(True) - - self._reconf = reconf - self._condition_variable = threading.Condition() - self._configs_pending = {} - self._timestamp_last_pending = None - self._stop_flag = False - - def run(self): - _timestamp_last_commit = None - - rospy.logdebug(' ParamUpdater started') - - while not self._stop_flag: - if _timestamp_last_commit >= self._timestamp_last_pending: - with self._condition_variable: - rospy.logdebug(' ParamUpdater loop 1.1') - self._condition_variable.wait() - rospy.logdebug(' ParamUpdater loop 1.2') - rospy.logdebug(' ParamUpdater loop 2') - - if self._stop_flag: - return - - _timestamp_last_commit = time.time() - configs_tobe_updated = self._configs_pending.copy() - self._configs_pending = {} - - rospy.logdebug(' run last_commit={}, last_pend={}'.format( - _timestamp_last_commit, self._timestamp_last_pending)) - - try: - self._reconf.update_configuration(configs_tobe_updated) - except rospy.ServiceException as ex: - rospy.logdebug('Could not update configs due to {}'.format( - ex.value)) - except Exception as exc: - raise exc - - def update(self, config): - with self._condition_variable: - for name, value in config.items(): - self._configs_pending[name] = value - - self._timestamp_last_pending = time.time() - - self._condition_variable.notify() - - def stop(self): - self._stop_flag = True - with self._condition_variable: - self._condition_variable.notify() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py deleted file mode 100644 index 63218951..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/param_widget.py +++ /dev/null @@ -1,175 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import rospkg -import sys - -from python_qt_binding.QtCore import Signal, QMargins -from python_qt_binding.QtGui import (QLabel, QHBoxLayout, QSplitter, - QVBoxLayout, QWidget, QItemSelectionModel) - -from rqt_reconfigure.node_selector_widget import NodeSelectorWidget -from rqt_reconfigure.paramedit_widget import ParameditWidget -from rqt_reconfigure.text_filter import TextFilter -from rqt_reconfigure.text_filter_widget import TextFilterWidget -import rospy - -class ParamWidget(QWidget): - _TITLE_PLUGIN = 'Dynamic Reconfigure' - - # To be connected to PluginContainerWidget - sig_sysmsg = Signal(str) - sig_sysprogress = Signal(str) - - # To make selections from CLA - sig_selected = Signal(str) - - def __init__(self, context, node=None): - """ - This class is intended to be called by rqt plugin framework class. - Currently (12/12/2012) the whole widget is splitted into 2 panes: - one on left allows you to choose the node(s) you work on. Right side - pane lets you work with the parameters associated with the node(s) you - select on the left. - - (12/27/2012) Despite the pkg name is changed to rqt_reconfigure to - reflect the available functionality, file & class names remain - 'param', expecting all the parameters will become handle-able. - """ - - super(ParamWidget, self).__init__() - self.setObjectName(self._TITLE_PLUGIN) - self.setWindowTitle(self._TITLE_PLUGIN) - - rp = rospkg.RosPack() - - #TODO: .ui file needs to replace the GUI components declaration - # below. For unknown reason, referring to another .ui files - # from a .ui that is used in this class failed. So for now, - # I decided not use .ui in this class. - # If someone can tackle this I'd appreciate. - _hlayout_top = QHBoxLayout(self) - _hlayout_top.setContentsMargins(QMargins(0, 0, 0, 0)) - self._splitter = QSplitter(self) - _hlayout_top.addWidget(self._splitter) - - _vlayout_nodesel_widget = QWidget() - _vlayout_nodesel_side = QVBoxLayout() - _hlayout_filter_widget = QWidget(self) - _hlayout_filter = QHBoxLayout() - self._text_filter = TextFilter() - self.filter_lineedit = TextFilterWidget(self._text_filter, rp) - self.filterkey_label = QLabel("&Filter key:") - self.filterkey_label.setBuddy(self.filter_lineedit) - _hlayout_filter.addWidget(self.filterkey_label) - _hlayout_filter.addWidget(self.filter_lineedit) - _hlayout_filter_widget.setLayout(_hlayout_filter) - self._nodesel_widget = NodeSelectorWidget(self, rp, self.sig_sysmsg) - _vlayout_nodesel_side.addWidget(_hlayout_filter_widget) - _vlayout_nodesel_side.addWidget(self._nodesel_widget) - _vlayout_nodesel_side.setSpacing(1) - _vlayout_nodesel_widget.setLayout(_vlayout_nodesel_side) - - reconf_widget = ParameditWidget(rp) - - self._splitter.insertWidget(0, _vlayout_nodesel_widget) - self._splitter.insertWidget(1, reconf_widget) - # 1st column, _vlayout_nodesel_widget, to minimize width. - # 2nd col to keep the possible max width. - self._splitter.setStretchFactor(0, 0) - self._splitter.setStretchFactor(1, 1) - - # Signal from paramedit widget to node selector widget. - reconf_widget.sig_node_disabled_selected.connect( - self._nodesel_widget.node_deselected) - # Pass name of node to editor widget - self._nodesel_widget.sig_node_selected.connect( - reconf_widget.show_reconf) - - if not node: - title = self._TITLE_PLUGIN - else: - title = self._TITLE_PLUGIN + ' %s' % node - self.setObjectName(title) - - #Connect filter signal-slots. - self._text_filter.filter_changed_signal.connect( - self._filter_key_changed) - - # Open any clients indicated from command line - self.sig_selected.connect(self._nodesel_widget.node_selected) - for rn in [rospy.resolve_name(c) for c in context.argv()]: - if rn in self._nodesel_widget.get_paramitems(): - self.sig_selected.emit(rn) - else: - rospy.logwarn('Could not find a dynamic reconfigure client named \'%s\'', str(rn)) - - def shutdown(self): - #TODO: Needs implemented. Trigger dynamic_reconfigure to unlatch - # subscriber. - pass - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('splitter', self._splitter.saveState()) - - def restore_settings(self, plugin_settings, instance_settings): - if instance_settings.contains('splitter'): - self._splitter.restoreState(instance_settings.value('splitter')) - else: - self._splitter.setSizes([100, 100, 200]) - - def get_filter_text(self): - """ - :rtype: QString - """ - return self.filter_lineedit.text() - - def _filter_key_changed(self): - self._nodesel_widget.set_filter(self._text_filter) - - #TODO: This method should be integrated into common architecture. I just - # can't think of how to do so within current design. - def emit_sysmsg(self, msg_str): - self.sig_sysmsg.emit(msg_str) - - -if __name__ == '__main__': - # main should be used only for debug purpose. - # This launches this QWidget as a standalone rqt gui. - from rqt_gui.main import Main - - main = Main() - sys.exit(main.main(sys.argv, standalone='rqt_reconfigure')) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py deleted file mode 100644 index aeb0c201..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/paramedit_widget.py +++ /dev/null @@ -1,167 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito, Ze'ev Klapow - -import os -from collections import OrderedDict - -import dynamic_reconfigure.client -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Signal -from python_qt_binding.QtGui import QVBoxLayout, QWidget, QWidgetItem -from rqt_py_common.layout_util import LayoutUtil -import rospy - -from .dynreconf_client_widget import DynreconfClientWidget - - -class ParameditWidget(QWidget): - """ - This class represents a pane where parameter editor widgets of multiple - nodes are shown. In rqt_reconfigure, this pane occupies right half of the - entire visible area. - """ - - # public signal - sig_node_disabled_selected = Signal(str) - - def __init__(self, rospack): - """""" - super(ParameditWidget, self).__init__() - - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), - 'resource', 'paramedit_pane.ui') - loadUi(ui_file, self, {'ParameditWidget': ParameditWidget}) - - self._dynreconf_clients = OrderedDict() - - # Adding the list of Items - self.vlayout = QVBoxLayout(self.scrollarea_holder_widget) - - #self._set_index_widgets(self.listview, paramitems_dict) # causes error - self.destroyed.connect(self.close) - - def _set_index_widgets(self, view, paramitems_dict): - """ - @deprecated: Causes error - """ - i = 0 - for p in paramitems_dict: - view.setIndexWidget(i, p) - i += 1 - - def show_reconf(self, dynreconf_widget): - """ - Callback when user chooses a node. - - @param dynreconf_widget: - """ - node_grn = dynreconf_widget.get_node_grn() - rospy.logdebug('ParameditWidget.show str(node_grn)=%s', str(node_grn)) - - if not node_grn in self._dynreconf_clients.keys(): - # Add dynreconf widget if there isn't already one. - - # Client gets renewed every time different node_grn was clicked. - - self._dynreconf_clients.__setitem__(node_grn, dynreconf_widget) - self.vlayout.addWidget(dynreconf_widget) - dynreconf_widget.sig_node_disabled_selected.connect( - self._node_disabled) - - else: # If there has one already existed, remove it. - self._remove_node(node_grn) - #LayoutUtil.clear_layout(self.vlayout) - - # Re-add the rest of existing items to layout. - #for k, v in self._dynreconf_clients.iteritems(): - # rospy.loginfo('added to layout k={} v={}'.format(k, v)) - # self.vlayout.addWidget(v) - - # Add color to alternate the rim of the widget. - LayoutUtil.alternate_color(self._dynreconf_clients.itervalues(), - [self.palette().background().color().lighter(125), - self.palette().background().color().darker(125)]) - - def close(self): - for dc in self._dynreconf_clients: - # Clear out the old widget - dc.close() - dc = None - - self._paramedit_scrollarea.deleteLater() - - def filter_param(self, filter_key): - """ - :type filter_key: - """ - - #TODO Pick nodes that match filter_key. - - #TODO For the nodes that are kept in previous step, call - # DynreconfWidget.filter_param for all of its existing - # instances. - pass - - def _remove_node(self, node_grn): - try: - i = self._dynreconf_clients.keys().index(node_grn) - except ValueError: - # ValueError occurring here means that the specified key is not - # found, most likely already removed, which is possible in the - # following situation/sequence: - # - # Node widget on ParameditWidget removed by clicking disable button - # --> Node deselected on tree widget gets updated - # --> Tree widget detects deselection - # --> Tree widget emits deselection signal, which is captured by - # ParameditWidget's slot. Thus reaches this method again. - return - - item = self.vlayout.itemAt(i) - if isinstance(item, QWidgetItem): - item.widget().close() - w = self._dynreconf_clients.pop(node_grn) - - rospy.logdebug('popped={} Len of left clients={}'.format( - w, len(self._dynreconf_clients))) - - def _node_disabled(self, node_grn): - rospy.logdebug('paramedit_w _node_disabled grn={}'.format(node_grn)) - - # Signal to notify other GUI components (eg. nodes tree pane) that - # a node widget is disabled. - self.sig_node_disabled_selected.emit(node_grn) - - # Remove the selected node widget from the internal list of nodes. - self._remove_node(node_grn) diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py deleted file mode 100644 index fb86bfd0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter.py +++ /dev/null @@ -1,106 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to stoporse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from python_qt_binding.QtCore import QRegExp, Qt -from rqt_console.filters.message_filter import MessageFilter - - -class TextFilter(MessageFilter): - """ - Provides a filtering feature for text set by set_text. - - Inheriting rqt_console.filters.MessageFilter, this class provides timeout - effect to the input widget (eg. QLineEdit) that contains this class. - """ - - def __init__(self, qregexp=None): - super(TextFilter, self).__init__() - self._regexp = qregexp - - def test_message(self, text): - """ - Overridden. - - :param message: the message to be tested against the filters. - :type message: str. - :rtype: bool - """ - - _hit = False - - if (self.is_enabled() and - self._text != '' and - not self._qregexp == None # If None, init process isn't done yet - # and we can ignore the call to this - # method. - ): - pos_hit = self._qregexp.indexIn(text) - if pos_hit >= 0: - _hit = True - else: - _hit = False - return _hit - -# def set_regexp(self, qregexp): -# """ -# Setter for self._qregexp. Need not be used if _qregexp is already set -# via __init__. Calling this method overwrites the existing regex -# instance. -# -# Do not mix up self._qregexp with MessageFilter._regex that is boolean. -# Instead, this method sets QRegExp instance, that test_message method -# uses. -# -# :type qregexp: QRegExp -# """ -# self._qregexp = qregexp - - def get_regexp(self): - return self._regex - - def set_text(self, text): - """ - Setter for _text - :param text: text to set ''str'' - :emits filter_changed_signal: If _enabled is true - """ - super(TextFilter, self).set_text(text) - - syntax_nr = QRegExp.RegExp - syntax = QRegExp.PatternSyntax(syntax_nr) - self.regex = QRegExp(text, Qt.CaseInsensitive, syntax) - self.set_regex(self.regex) - - def get_text(self): - return self._text diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py deleted file mode 100644 index 8cceb221..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/text_filter_widget.py +++ /dev/null @@ -1,91 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget - - -class TextFilterWidget(QWidget): - """ - Taken from rqt_console.TextFilterWidget. Only modification from it is .ui - file in use that takes more generic form (only textfiedl). - """ - def __init__(self, parentfilter, rospack, display_list_args=None): - """ - Widget for displaying interactive data related to text filtering. - - Taken from rqt_console and simplified to be usable in broader - situations. - - :type parentfilter: BaseFilter - :param parentfilter: buddy filter were data is stored, ''TimeFilter'' - :param display_list_args: empty list, ''list'' - """ - super(TextFilterWidget, self).__init__() - ui_file = os.path.join(rospack.get_path('rqt_reconfigure'), 'resource', - 'text_filter_widget.ui') - loadUi(ui_file, self) - self.setObjectName('TextFilterWidget') - # When data is changed it is stored in the parent filter - self._parentfilter = parentfilter - - self.text_edit.textChanged.connect(self.handle_text_changed) - - self.handle_text_changed() - - def set_text(self, text): - """ - Setter for the text edit widget - :param text: text to be placed in text_edit, ''str'' - """ - self.text_edit.setText(text) - - def handle_text_changed(self): - self._parentfilter.set_text(self.text_edit.text()) - - def repopulate(self): - """ - Stub function. - If the widget had any dynamically adjustable data it would requery it - in this function. - """ - pass - - def save_settings(self, settings): - settings.set_value('text', self._parentfilter._text) - - def restore_settings(self, settings): - text = settings.value('text', '') - self.set_text(text) - self.handle_text_changed() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py deleted file mode 100644 index ddacdeff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_item_model.py +++ /dev/null @@ -1,77 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QStandardItemModel -import rospy - - -class TreenodeItemModel(QStandardItemModel): - """ - This class is made only for this purpose; to hold QStandardItem instances - associated with QModelIndex. QStandardItemModel has methods to return it - by index called itemFromIndex, but in some cases the method doesn't work - for unknown reasons. Ref. question asked: - http://stackoverflow.com/questions/14646979/strange-index-values-from-qstandarditemmodel - - :author: Isaac Saito - """ - - def __init__(self, parent=None): - super(TreenodeItemModel, self).__init__(parent) - self._parent = parent - - self._indexes = {} # { str : QPersistentModelIndex } - - def get_index_from_grn(self, grn): - """ - - :type grn: str - :rtype: QPersistentModelIndex. None if the corresponding item isn't - found. - """ - rospy.logdebug('get_index_from_grn all item={}'.format( - self._indexes)) - return self._indexes.get(grn) - - def set_item_from_index(self, grn, qpindex): - """ - :type grn: str - :type qpindex: QPersistentModelIndex - """ - rospy.logdebug('set_item_from_index grn={} qpindex={}'.format( - grn, qpindex)) - self._indexes[grn] = qpindex diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py deleted file mode 100644 index 8da677f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_qstditem.py +++ /dev/null @@ -1,265 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -from __future__ import division - -import threading -import time - -import dynamic_reconfigure.client -from python_qt_binding.QtCore import Qt -from python_qt_binding.QtGui import QBrush, QStandardItem -import rospy -from rospy.exceptions import ROSException -from rqt_py_common.data_items import ReadonlyItem - -from .dynreconf_client_widget import DynreconfClientWidget - - -class ParamserverConnectThread(threading.Thread): - def __init__(self, parent, param_name_raw): - super(ParamserverConnectThread, self).__init__() - self._parent = parent - self._param_name_raw = param_name_raw - - def run(self): - dynreconf_client = None - try: - dynreconf_client = dynamic_reconfigure.client.Client( - str(self._param_name_raw), timeout=5.0) - rospy.logdebug('ParamserverConnectThread dynreconf_client={}'. \ - format(dynreconf_client)) - self._parent.set_dynreconf_client(dynreconf_client) - except rospy.exceptions.ROSException as e: - raise type(e)(e.message + - "TreenodeQstdItem. Couldn't connect to {}".format( - self._param_name_raw)) - - -class TreenodeQstdItem(ReadonlyItem): - """ - Extending ReadonlyItem - the display content of this item shouldn't be - modified. - """ - - NODE_FULLPATH = 1 - - def __init__(self, *args): - """ - :param args[0]: str (will become 1st arg of QStandardItem) - :param args[1]: integer value that indicates whether this class - is node that has GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). This can be None - """ - grn_current_treenode = args[0] - self._param_name_raw = grn_current_treenode - self._set_param_name(grn_current_treenode) - super(TreenodeQstdItem, self).__init__(grn_current_treenode) - - # dynamic_reconfigure.client.Client - self._dynreconf_client = None - # DynreconfClientWidget - self._dynreconfclient_widget = None - - self._is_rosnode = False - - self._lock = None - self._paramserver_connect_thread = None - - try: - if args[1]: - self._is_rosnode = True - except IndexError: # tuple index out of range etc. - rospy.logerr('TreenodeQstdItem IndexError') - - def set_dynreconf_client(self, dynreconf_client): - """ - @param dynreconf_client: dynamic_reconfigure.client.Client - """ - self._dynreconf_client = dynreconf_client - rospy.logdebug('Qitem set dynreconf_client={} param={}'.format( - self._dynreconf_client, - self._param_name_raw)) - - def clear_dynreconf_client(self): - if self._dynreconf_client is not None: - self._dynreconf_client.close() - del self._dynreconf_client - self._dynreconf_client = None - - def get_dynreconf_widget(self): - """ - @rtype: DynreconfClientWidget (QWidget) - @return: None if dynreconf_client is not yet generated. - @raise ROSException: - """ - - if not self._dynreconfclient_widget: - rospy.logdebug('get dynreconf_client={}'.format( - self._dynreconf_client)) - rospy.logdebug('In get_dynreconf_widget 1') - if not self._dynreconf_client: - self.connect_param_server() - rospy.logdebug('In get_dynreconf_widget 2') - - timeout = 3 * 100 - loop = 0 - # Loop until _dynreconf_client is set. self._dynreconf_client gets - # set from different thread (in ParamserverConnectThread). - while self._dynreconf_client == None: - #Avoid deadlock - if timeout < loop: - #Make itself unclickable - self.setEnabled(False) - raise ROSException('dynreconf client failed') - - time.sleep(0.01) - loop += 1 - rospy.logdebug('In get_dynreconf_widget loop#{}'.format(loop)) - - rospy.logdebug('In get_dynreconf_widget 4') - self._dynreconfclient_widget = DynreconfClientWidget( - self._dynreconf_client, - self._param_name_raw) - # Creating the DynreconfClientWidget transfers ownership of the _dynreconf_client - # to it. If it is destroyed from Qt, we need to clear our reference to it and - # stop the param server thread we had. - self._dynreconfclient_widget.destroyed.connect(self.clear_dynreconfclient_widget) - self._dynreconfclient_widget.destroyed.connect(self.disconnect_param_server) - rospy.logdebug('In get_dynreconf_widget 5') - - else: - pass - return self._dynreconfclient_widget - - def clear_dynreconfclient_widget(self): - self._dynreconfclient_widget = None - - def connect_param_server(self): - """ - Connect to parameter server using dynamic_reconfigure client. - Behavior is delegated to a private method _connect_param_server, and - its return value, client, is set to member variable. - - @return void - @raise ROSException: - """ - # If the treenode doesn't represent ROS Node, return None. - if not self._is_rosnode: - rospy.logerr('connect_param_server failed due to missing ' + - 'ROS Node. Return with nothing.') - return - - if not self._dynreconf_client: - if self._paramserver_connect_thread: - self.disconnect_param_server() - self._paramserver_connect_thread = ParamserverConnectThread( - self, self._param_name_raw) - self._paramserver_connect_thread.start() - - def disconnect_param_server(self): - if self._paramserver_connect_thread: - # Try to stop the thread - if self._paramserver_connect_thread.isAlive(): - self._paramserver_connect_thread.join(1) - del self._paramserver_connect_thread - self._paramserver_connect_thread = None - self.clear_dynreconf_client() - - def enable_param_items(self): - """ - Create QStdItem per parameter and addColumn them to myself. - :rtype: None if _dynreconf_client is not initiated. - """ - if not self._dynreconfclient_widget: - return None - paramnames = self._dynreconfclient_widget.get_treenode_names() - paramnames_items = [] - brush = QBrush(Qt.lightGray) - for paramname in paramnames: - item = ReadonlyItem(paramname) - item.setBackground(brush) - paramnames_items.append(item) - rospy.logdebug('enable_param_items len of paramnames={}'.format( - len(paramnames_items))) - self.appendColumn(paramnames_items) - - def _set_param_name(self, param_name): - """ - :param param_name: A string formatted as GRN (Graph Resource Names, see - http://www.ros.org/wiki/Names). - Example: /paramname/subpara/subsubpara/... - """ - rospy.logdebug('_set_param_name param_name={} '.format(param_name)) - - # separate param_name by forward slash - self._list_treenode_names = param_name.split('/') - - # Deleting the 1st elem which is zero-length str. - del self._list_treenode_names[0] - - self._toplevel_treenode_name = self._list_treenode_names[0] - - rospy.logdebug('paramname={} nodename={} _list_params[-1]={}'.format( - param_name, self._toplevel_treenode_name, - self._list_treenode_names[-1])) - - def get_param_name_toplv(self): - """ - :rtype: String of the top level param name. - """ - - return self._name_top - - def get_raw_param_name(self): - return self._param_name_raw - - def get_treenode_names(self): - """ - :rtype: List of string. Null if param - """ - - #TODO: what if self._list_treenode_names is empty or null? - return self._list_treenode_names - - def get_node_name(self): - """ - :return: A value of single tree node (ie. NOT the fullpath node name). - Ex. suppose fullpath name is /top/sub/subsub/subsubsub and you - are at 2nd from top, the return value is subsub. - """ - return self._toplevel_treenode_name - - def type(self): - return QStandardItem.UserType diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py deleted file mode 100644 index 675eed16..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/src/rqt_reconfigure/treenode_status.py +++ /dev/null @@ -1,89 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - - -from python_qt_binding.QtGui import QModelIndex - - -class TreenodeStatus(QModelIndex): - """ - - This class contains very similar information with - rqt_reconfigure.ParameterItem. The purpose of this class is to enable - FilterChildrenModel (subclassing QSortFilterProxyModel) to look up each - node, which, afaik, is not possible via QSortFilterProxyModel and that's - why I created this class. - - That said, to store an info about each treenode: - - - ParameterItem should be used to show on view. - - This class should be used when you need to keep track from - QAbstractProxyModel - - :author: Isaac Saito - """ - - def __init__(self, nodename_full=None, qmindex=None): - """ - :param index_id: default value is -1, which indicates "not set". This - can be set. - :param nodename_full: default value is None, which indicates "not set". - This can be set. - :type index_id: qint64 - :type nodename_full: str - :type qmindex: QModelIndex - """ - super(TreenodeStatus, self).__init__(qmindex) - - self._is_eval_done = False - self._shows = False - self._nodename_full = nodename_full - - def set_nodename_full(self, nodename_full): - self._nodename_full = nodename_full - - def get_nodename_full(self): - return self._nodename_full - - def set_is_eval_done(self, v): - self._is_eval_done = v - - def get_is_eval_done(self): - return self._is_eval_done - - def set_shows(self, v): - self._shows = v - - def get_shows(self): - return self._shows diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/__init__.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py deleted file mode 100755 index 0d35524f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_text_filter.py +++ /dev/null @@ -1,67 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from python_qt_binding.QtCore import Qt, QRegExp -from rqt_reconfigure.text_filter import TextFilter - - -class MyTest(unittest.TestCase): - _query_text = 'filter' - _filtered_text = 'filtered_text' - - def setUp(self): - unittest.TestCase.setUp(self) - - syntax_nr = QRegExp.RegExp - syntax = QRegExp.PatternSyntax(syntax_nr) - self._regExp = QRegExp(self._query_text, Qt.CaseInsensitive, syntax) - - self._filter = TextFilter(self._regExp) - self._filter.set_text(self._query_text) - - def tearDown(self): - unittest.TestCase.tearDown(self) - - def test_test_message(self): - """Testing test_message method.""" - result_regex = self._filter.test_message(self._filtered_text) - print 'result_regex={}'.format(result_regex) - self.assertEqual(result_regex, - True # Both _query_text & filtered_text overlaps. - ) - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py b/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py deleted file mode 100755 index 3ff17d7f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_reconfigure/test/test_treenode_qstditem.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Author: Isaac Saito - -import unittest - -from rqt_reconfigure.treenode_qstditem import TreenodeQstdItem - - -class TestTreenodeQstdItem(unittest.TestCase): - """ - :author: Isaac Saito - """ - - _nodename_raw = '/base_hokuyo_node' - _nodename_extracted = 'base_hokuyo_node' - - def setUp(self): - unittest.TestCase.setUp(self) - #self._item = TreenodeQstdItem(self._nodename_raw, 0) # For unknown reason - #this stops operation. - self._item = TreenodeQstdItem(self._nodename_raw) - - def tearDown(self): - unittest.TestCase.tearDown(self) - del self._item - - def test_get_node_name(self): - self.assertEqual(self._item.get_node_name(), - self._nodename_extracted) - -# def test_get_node_name(self): -# self.assertEqual(self._item.get_widget().show()) - - -if __name__ == '__main__': - unittest.main() diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst deleted file mode 100644 index f309ce56..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_service_caller -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt deleted file mode 100644 index 519ee830..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_service_caller) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_service_caller - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox deleted file mode 100644 index 4968e513..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_publisher provides a GUI plugin for publishing arbitrary messages with fixed or computed field values. -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml b/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml deleted file mode 100644 index 98e8b347..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_service_caller - 0.3.13 - rqt_service_caller provides a GUI plugin for calling arbitrary services. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_service_caller - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - rosservice - rqt_gui - rqt_gui_py - rqt_py_common - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml b/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml deleted file mode 100644 index e29bafa8..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin calling ROS services. - - - - - folder - Plugins related to ROS services. - - - media-playback-start - A Python GUI plugin for calling ROS services. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui b/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui deleted file mode 100644 index 66e56bd0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/resource/ServiceCaller.ui +++ /dev/null @@ -1,187 +0,0 @@ - - - ServiceCallerWidget - - - - 0 - 0 - 904 - 359 - - - - Service Caller - - - - - - - - Refresh service list - - - - - - - 16 - 16 - - - - - - - - - 0 - 0 - - - - Service - - - - - - - - 0 - 0 - - - - false - - - - - - - Call selected service - - - Call - - - - 16 - 16 - - - - - - - - - - Qt::Horizontal - - - - - - - Request - - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - - Topic - - - - - Type - - - - - Expression - - - - - - - - - - - - Response - - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - QAbstractItemView::ExtendedSelection - - - - Field - - - - - Type - - - - - Value - - - - - - - - - - - - - ExtendedComboBox - QComboBox -
rqt_py_common.extended_combo_box
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller b/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller deleted file mode 100755 index 49fc7cbc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/scripts/rqt_service_caller +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_service_caller.service_caller.ServiceCaller')) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py b/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py deleted file mode 100644 index 7dc34e1d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_service_caller'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/__init__.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py deleted file mode 100644 index 6719e53b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller.py +++ /dev/null @@ -1,55 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_gui_py.plugin import Plugin -from .service_caller_widget import ServiceCallerWidget - - -class ServiceCaller(Plugin): - - def __init__(self, context): - super(ServiceCaller, self).__init__(context) - self.setObjectName('ServiceCaller') - - self._widget = ServiceCallerWidget() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - - def trigger_configuration(self): - self._widget.trigger_configuration() diff --git a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py b/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py deleted file mode 100644 index 6934470c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_service_caller/src/rqt_service_caller/service_caller_widget.py +++ /dev/null @@ -1,284 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import math -import os -import random -import time - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, Slot, qWarning -from python_qt_binding.QtGui import QIcon, QMenu, QTreeWidgetItem, QWidget - -import rospkg -import rospy -import genpy -import rosservice - -from rqt_py_common.extended_combo_box import ExtendedComboBox - - -class ServiceCallerWidget(QWidget): - column_names = ['service', 'type', 'expression'] - - def __init__(self): - super(ServiceCallerWidget, self).__init__() - self.setObjectName('ServiceCallerWidget') - - # create context for the expression eval statement - self._eval_locals = {} - for module in (math, random, time): - self._eval_locals.update(module.__dict__) - self._eval_locals['genpy'] = genpy - del self._eval_locals['__name__'] - del self._eval_locals['__doc__'] - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_service_caller'), 'resource', 'ServiceCaller.ui') - loadUi(ui_file, self, {'ExtendedComboBox': ExtendedComboBox}) - self.refresh_services_button.setIcon(QIcon.fromTheme('view-refresh')) - self.call_service_button.setIcon(QIcon.fromTheme('call-start')) - - self._column_index = {} - for column_name in self.column_names: - self._column_index[column_name] = len(self._column_index) - - self._service_info = None - self.on_refresh_services_button_clicked() - - self.request_tree_widget.itemChanged.connect(self.request_tree_widget_itemChanged) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('splitter_orientation', self.splitter.orientation()) - - def restore_settings(self, plugin_settings, instance_settings): - if int(instance_settings.value('splitter_orientation', Qt.Vertical)) == int(Qt.Vertical): - self.splitter.setOrientation(Qt.Vertical) - else: - self.splitter.setOrientation(Qt.Horizontal) - - def trigger_configuration(self): - new_orientation = Qt.Vertical if self.splitter.orientation() == Qt.Horizontal else Qt.Horizontal - self.splitter.setOrientation(new_orientation) - - @Slot() - def on_refresh_services_button_clicked(self): - service_names = rosservice.get_service_list() - self._services = {} - for service_name in service_names: - try: - self._services[service_name] = rosservice.get_service_class_by_name(service_name) - #qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name])) - except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e: - qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e)) - - self.service_combo_box.clear() - self.service_combo_box.addItems(sorted(service_names)) - - @Slot(str) - def on_service_combo_box_currentIndexChanged(self, service_name): - self.request_tree_widget.clear() - self.response_tree_widget.clear() - service_name = str(service_name) - if not service_name: - return - - self._service_info = {} - self._service_info['service_name'] = service_name - self._service_info['service_class'] = self._services[service_name] - self._service_info['service_proxy'] = rospy.ServiceProxy(service_name, self._service_info['service_class']) - self._service_info['expressions'] = {} - self._service_info['counter'] = 0 - - # recursively create widget items for the service request's slots - request_class = self._service_info['service_class']._request_class - top_level_item = self._recursive_create_widget_items(None, service_name, request_class._type, request_class()) - - # add top level item to tree widget - self.request_tree_widget.addTopLevelItem(top_level_item) - - # resize columns - self.request_tree_widget.expandAll() - for i in range(self.request_tree_widget.columnCount()): - self.request_tree_widget.resizeColumnToContents(i) - - def _recursive_create_widget_items(self, parent, topic_name, type_name, message, is_editable=True): - item = QTreeWidgetItem(parent) - if is_editable: - item.setFlags(item.flags() | Qt.ItemIsEditable) - else: - item.setFlags(item.flags() & (~Qt.ItemIsEditable)) - - if parent is None: - # show full topic name with preceding namespace on toplevel item - topic_text = topic_name - else: - topic_text = topic_name.split('/')[-1] - - item.setText(self._column_index['service'], topic_text) - item.setText(self._column_index['type'], type_name) - - item.setData(0, Qt.UserRole, topic_name) - - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name, type_name in zip(message.__slots__, message._slot_types): - self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name), is_editable) - - elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'): - type_name = type_name.split('[', 1)[0] - for index, slot in enumerate(message): - self._recursive_create_widget_items(item, topic_name + '[%d]' % index, type_name, slot, is_editable) - - else: - item.setText(self._column_index['expression'], repr(message)) - - return item - - @Slot('QTreeWidgetItem*', int) - def request_tree_widget_itemChanged(self, item, column): - column_name = self.column_names[column] - new_value = str(item.text(column)) - #qDebug('ServiceCaller.request_tree_widget_itemChanged(): %s : %s' % (column_name, new_value)) - - if column_name == 'expression': - topic_name = str(item.data(0, Qt.UserRole)) - self._service_info['expressions'][topic_name] = new_value - #qDebug('ServiceCaller.request_tree_widget_itemChanged(): %s expression: %s' % (topic_name, new_value)) - - def fill_message_slots(self, message, topic_name, expressions, counter): - if not hasattr(message, '__slots__'): - return - for slot_name in message.__slots__: - slot_key = topic_name + '/' + slot_name - - # if no expression exists for this slot_key, continue with it's child slots - if slot_key not in expressions: - self.fill_message_slots(getattr(message, slot_name), slot_key, expressions, counter) - continue - - expression = expressions[slot_key] - if len(expression) == 0: - continue - - # get slot type - slot = getattr(message, slot_name) - if hasattr(slot, '_type'): - slot_type = slot._type - else: - slot_type = type(slot) - - self._eval_locals['i'] = counter - value = self._evaluate_expression(expression, slot_type) - if value is not None: - setattr(message, slot_name, value) - - def _evaluate_expression(self, expression, slot_type): - successful_eval = True - successful_conversion = True - - try: - # try to evaluate expression - value = eval(expression, {}, self._eval_locals) - except Exception: - # just use expression-string as value - value = expression - successful_eval = False - - try: - # try to convert value to right type - value = slot_type(value) - except Exception: - successful_conversion = False - - if successful_conversion: - return value - elif successful_eval: - qWarning('ServiceCaller.fill_message_slots(): can not convert expression to slot type: %s -> %s' % (type(value), slot_type)) - else: - qWarning('ServiceCaller.fill_message_slots(): failed to evaluate expression: %s' % (expression)) - - return None - - @Slot() - def on_call_service_button_clicked(self): - self.response_tree_widget.clear() - - request = self._service_info['service_class']._request_class() - self.fill_message_slots(request, self._service_info['service_name'], self._service_info['expressions'], self._service_info['counter']) - try: - response = self._service_info['service_proxy'](request) - except rospy.ServiceException as e: - qWarning('ServiceCaller.on_call_service_button_clicked(): request:\n%r' % (request)) - qWarning('ServiceCaller.on_call_service_button_clicked(): error calling service "%s":\n%s' % (self._service_info['service_name'], e)) - top_level_item = QTreeWidgetItem() - top_level_item.setText(self._column_index['service'], 'ERROR') - top_level_item.setText(self._column_index['type'], 'rospy.ServiceException') - top_level_item.setText(self._column_index['expression'], str(e)) - else: - #qDebug('ServiceCaller.on_call_service_button_clicked(): response: %r' % (response)) - top_level_item = self._recursive_create_widget_items(None, '/', response._type, response, is_editable=False) - - self.response_tree_widget.addTopLevelItem(top_level_item) - # resize columns - self.response_tree_widget.expandAll() - for i in range(self.response_tree_widget.columnCount()): - self.response_tree_widget.resizeColumnToContents(i) - - @Slot('QPoint') - def on_request_tree_widget_customContextMenuRequested(self, pos): - self._show_context_menu(self.request_tree_widget.itemAt(pos), self.request_tree_widget.mapToGlobal(pos)) - - @Slot('QPoint') - def on_response_tree_widget_customContextMenuRequested(self, pos): - self._show_context_menu(self.response_tree_widget.itemAt(pos), self.response_tree_widget.mapToGlobal(pos)) - - def _show_context_menu(self, item, global_pos): - if item is None: - return - - # show context menu - menu = QMenu(self) - action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), "Expand All Children") - action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), "Collapse All Children") - action = menu.exec_(global_pos) - - # evaluate user action - if action in (action_item_expand, action_item_collapse): - expanded = (action is action_item_expand) - - def recursive_set_expanded(item): - item.setExpanded(expanded) - for index in range(item.childCount()): - recursive_set_expanded(item.child(index)) - recursive_set_expanded(item) - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst deleted file mode 100644 index c39eb8ec..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/CHANGELOG.rst +++ /dev/null @@ -1,103 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_shell -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt deleted file mode 100644 index 456cf787..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_shell) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_shell - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox deleted file mode 100644 index bb2d90a0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_shell is a Python GUI plugin providing an interactive shell. - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_shell/package.xml b/workspace/src/rqt_common_plugins/rqt_shell/package.xml deleted file mode 100644 index a35c84ff..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_shell - 0.3.13 - rqt_shell is a Python GUI plugin providing an interactive shell. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_shell - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - qt_gui - qt_gui_py_common - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml b/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml deleted file mode 100644 index e52722fd..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin providing an interactive shell. - - - - - folder - Plugins related to miscellaneous tools. - - - terminal - A Python GUI plugin providing an interactive shell. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui b/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui deleted file mode 100644 index 02403179..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/resource/shell_widget.ui +++ /dev/null @@ -1,35 +0,0 @@ - - - Shell - - - - 0 - 0 - 276 - 212 - - - - Shell - - - - - - - - - - - - - - ShellTextEdit - QTextEdit -
rqt_shell.shell_text_edit
- - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell b/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell deleted file mode 100755 index 57d86771..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/scripts/rqt_shell +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_shell.shell.Shell')) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/setup.py b/workspace/src/rqt_common_plugins/rqt_shell/setup.py deleted file mode 100644 index 86521efb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_shell'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/__init__.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py deleted file mode 100644 index 655d34e0..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell.py +++ /dev/null @@ -1,124 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin -from qt_gui_py_common.simple_settings_dialog import SimpleSettingsDialog - -from shell_widget import ShellWidget - -try: - from xterm_widget import XTermWidget, is_xterm_available - _has_xterm = is_xterm_available() -except ImportError: - XTermWidget = None - _has_xterm = False - -try: - from spyder_shell_widget import SpyderShellWidget - _has_spyderlib = True -except ImportError: - SpyderShellWidget = None - _has_spyderlib = False - - -class Shell(Plugin): - """ - Plugin providing an interactive shell - """ - # shell types in order of priority - shell_types = [ - { - 'title': 'XTerm', - 'widget_class': XTermWidget, - 'description': 'Fully functional embedded XTerm (needs xterm and only works on X11).', - 'enabled': _has_xterm, - }, - { - 'title': 'SpyderShell', - 'widget_class': SpyderShellWidget, - 'description': 'Advanced shell (needs spyderlib).', - 'enabled': _has_spyderlib, - }, - { - 'title': 'SimpleShell', - 'widget_class': ShellWidget, - 'description': 'Simple shell for executing non-interactive finite commands.', - 'enabled': True, - }, - ] - - def __init__(self, context): - super(Shell, self).__init__(context) - self._context = context - self.setObjectName('Shell') - - self._widget = None - - def _switch_shell_widget(self): - # check for available shell type - while not self.shell_types[self._shell_type_index]['enabled']: - self._shell_type_index += 1 - selected_shell = self.shell_types[self._shell_type_index] - - if self._widget is not None: - if hasattr(self._widget, 'close_signal'): - self._widget.close_signal.disconnect(self._context.close_plugin) - self._context.remove_widget(self._widget) - self._widget.close() - - self._widget = selected_shell['widget_class']() - self._widget.setWindowTitle(selected_shell['title']) - if self._context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number())) - self._context.add_widget(self._widget) - if hasattr(self._widget, 'close_signal'): - self._widget.close_signal.connect(self._context.close_plugin) - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('shell_type', self._shell_type_index) - - def restore_settings(self, plugin_settings, instance_settings): - self._shell_type_index = int(instance_settings.value('shell_type', 0)) - self._switch_shell_widget() - - def trigger_configuration(self): - dialog = SimpleSettingsDialog(title='Shell Options') - dialog.add_exclusive_option_group(title='Shell Type', options=self.shell_types, selected_index=self._shell_type_index) - shell_type = dialog.get_settings()[0] - if shell_type is not None and self._shell_type_index != shell_type['selected_index']: - self._shell_type_index = shell_type['selected_index'] - self._context.reload_plugin() - - def shutdown_plugin(self): - if self._widget is not None and hasattr(self._widget, 'shutdown'): - self._widget.shutdown() - diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py deleted file mode 100644 index 5671d6ee..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_text_edit.py +++ /dev/null @@ -1,51 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import subprocess - -from qt_gui_py_common.console_text_edit import ConsoleTextEdit - - -class ShellTextEdit(ConsoleTextEdit): - - def __init__(self, parent=None): - super(ShellTextEdit, self).__init__(parent) - self._add_prompt() - - def _exec_code(self, code): - try: - self._pipe = subprocess.Popen([code], shell=True, stdout=subprocess.PIPE, stderr=subprocess.PIPE) - out, err = self._pipe.communicate() - self._stdout.write(out) - self._stderr.write(err) - except Exception, e: - self._stderr.write(str(e) + '\n') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py deleted file mode 100644 index 49177360..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/shell_widget.py +++ /dev/null @@ -1,47 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QWidget -import shell_text_edit - - -class ShellWidget(QWidget): - def __init__(self, parent=None): - super(ShellWidget, self).__init__(parent=parent) - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_shell'), 'resource', 'shell_widget.ui') - loadUi(ui_file, self, {'ShellTextEdit': shell_text_edit.ShellTextEdit}) - self.setObjectName('ShellWidget') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py deleted file mode 100644 index 2801a40c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/spyder_shell_widget.py +++ /dev/null @@ -1,121 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from python_qt_binding.QtGui import QFont, QIcon -from python_qt_binding.QtCore import QProcess, SIGNAL, QTextCodec, Signal - -from spyderlib.widgets.externalshell.baseshell import ExternalShellBase -from spyderlib.widgets.shell import TerminalWidget - - -class SpyderShellWidget(ExternalShellBase): - """Spyder Shell Widget: execute a shell in a separate process using spyderlib's ExternalShellBase""" - SHELL_CLASS = TerminalWidget - close_signal = Signal() - - def __init__(self, parent=None): - ExternalShellBase.__init__(self, parent=parent, fname=None, wdir='.', - history_filename='.history', - light_background=True, - menu_actions=None, - show_buttons_inside=False, - show_elapsed_time=False) - - self.setObjectName('SpyderShellWidget') - - # capture tab key - #self.shell._key_tab = self._key_tab - - self.shell.set_pythonshell_font(QFont('Mono')) - - # Additional python path list - self.path = [] - - # For compatibility with the other shells that can live in the external console - self.is_ipython_kernel = False - self.connection_file = None - - self.create_process() - - def get_icon(self): - return QIcon() - - def create_process(self): - self.shell.clear() - - self.process = QProcess(self) - self.process.setProcessChannelMode(QProcess.MergedChannels) - - env = [unicode(key_val_pair) for key_val_pair in self.process.systemEnvironment()] - env.append('TERM=xterm') - env.append('COLORTERM=gnome-terminal') - self.process.setEnvironment(env) - - # Working directory - if self.wdir is not None: - self.process.setWorkingDirectory(self.wdir) - - self.process.readyReadStandardOutput.connect(self.write_output) - self.process.finished.connect(self.finished) - self.process.finished.connect(self.close_signal) - - self.process.start('/bin/bash', ['-i']) - - running = self.process.waitForStarted() - self.set_running_state(running) - if not running: - self.shell.addPlainText("Process failed to start") - else: - self.shell.setFocus() - self.emit(SIGNAL('started()')) - - return self.process - - def shutdown(self): - self.process.kill() - self.process.waitForFinished() - - def _key_tab(self): - self.process.write('\t') - self.process.waitForBytesWritten(-1) - self.write_output() - - def send_to_process(self, text): - if not isinstance(text, basestring): - text = unicode(text) - if not text.endswith('\n'): - text += '\n' - self.process.write(QTextCodec.codecForLocale().fromUnicode(text)) - self.process.waitForBytesWritten(-1) - - def keyboard_interrupt(self): - self.send_ctrl_to_process('c') diff --git a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py b/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py deleted file mode 100644 index 5c733463..00000000 --- a/workspace/src/rqt_common_plugins/rqt_shell/src/rqt_shell/xterm_widget.py +++ /dev/null @@ -1,71 +0,0 @@ -#!/usr/bin/python -# -*- coding: utf-8 -*- -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Dorian Scholz -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os - -from python_qt_binding.QtCore import QProcess, QTimer, Signal -from python_qt_binding.QtGui import QX11EmbedContainer - - -class XTermWidget(QX11EmbedContainer): - xterm_cmd = '/usr/bin/xterm' - close_signal = Signal() - - def __init__(self, parent=None): - super(XTermWidget, self).__init__(parent) - self.setObjectName('XTermWidget') - self._process = QProcess(self) - self._process.finished.connect(self.close_signal) - # let the widget finish init before embedding xterm - QTimer.singleShot(100, self._embed_xterm) - - def _embed_xterm(self): - args = ['-into', str(self.winId())] - self._process.start(self.xterm_cmd, args) - if self._process.error() == QProcess.FailedToStart: - print "failed to execute '%s'" % self.xterm_cmd - - def shutdown(self): - self._process.kill() - self._process.waitForFinished() - - -def is_xterm_available(): - return os.path.isfile(XTermWidget.xterm_cmd) - -if __name__ == '__main__': - from PyQt4.QtGui import QApplication - app = QApplication([]) - xt = XTermWidget() - app.exec_() diff --git a/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst deleted file mode 100644 index da098058..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_srv -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- -* Eliminated a duplicated module with rqt_msg. Now depends on a common module (rqt_msg.MessageWidget) - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt deleted file mode 100644 index 9649d6c4..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/CMakeLists.txt +++ /dev/null @@ -1,14 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_srv) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_srv - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox deleted file mode 100644 index 7a0f5839..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_srv - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_srv/package.xml b/workspace/src/rqt_common_plugins/rqt_srv/package.xml deleted file mode 100644 index 847de737..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - rqt_srv - 0.3.13 - A Python GUI plugin for introspecting available ROS message types. - Note that the srvs available through this plugin is the ones that are stored - on your machine, not on the ROS core your rqt instance connects to. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_srv - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - rosmsg - rospy - rqt_gui - rqt_gui_py - rqt_msg - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml b/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml deleted file mode 100644 index 3c540290..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin for browsing available ROS service types. - - - - - folder - Plugins related to ROS services. - - - edit-copy - A Python GUI plugin for browsing available ROS service types. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv b/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv deleted file mode 100755 index 80ce74a7..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/scripts/rqt_srv +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_srv.services.Services')) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/setup.py b/workspace/src/rqt_common_plugins/rqt_srv/setup.py deleted file mode 100644 index e2225d32..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_srv'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/__init__.py b/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py b/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py deleted file mode 100644 index caeb7b31..00000000 --- a/workspace/src/rqt_common_plugins/rqt_srv/src/rqt_srv/services.py +++ /dev/null @@ -1,60 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import rosmsg - -from qt_gui.plugin import Plugin -from rqt_msg.messages_widget import MessagesWidget - - -class Services(Plugin): -#TODO fix the rosmsg.get_service_class function to return a class with slots like rosmsg.get_message_class does so that the recursive functions used to create tree_view elements will work. - def __init__(self, context): - super(Services, self).__init__(context) - self.setObjectName('servicess') - self._widget = MessagesWidget(rosmsg.MODE_SRV) - self._widget.setWindowTitle('Service Type Browser') - self._widget.type_label.setText('Service:') - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.cleanup_browsers_on_close() - - def save_settings(self, plugin_settings, instance_settings): - # instance_settings.set_value(k, v) - pass - - def restore_settings(self, plugin_settings, instance_settings): - # v = instance_settings.value(k) - pass diff --git a/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst deleted file mode 100644 index 53c9233c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/CHANGELOG.rst +++ /dev/null @@ -1,61 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_top -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* rqt_top: fix exception catching -* Contributors: Benjamin Chrétien - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- -* fix information when nodes are restarted, remove dead nodes from memory (`#294 `_) -* fix rqt_top script (`#303 `_) - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- -* fix sort order for numerical fields (`#205 `_) - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* fix an error caused by SIGKILLing nodes - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* remove copy of psutil module and implement missing function (`#105 `_) - -0.2.17 (2013-07-06) -------------------- -* Embeds python-psutil in the package in order to be enabled on Ubuntu Precise -* first release of this package into hydro - diff --git a/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt deleted file mode 100644 index ba292ce1..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_top) -# Load catkin and all dependencies required for this package -#find_package(catkin REQUIRED COMPONENTS python-psutil) -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_top - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox deleted file mode 100644 index ab3ee1cc..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/mainpage.dox +++ /dev/null @@ -1,14 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b top - - - ---> - - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_top/package.xml b/workspace/src/rqt_common_plugins/rqt_top/package.xml deleted file mode 100644 index e387302c..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - rqt_top - 0.3.13 - RQT plugin for monitoring ROS processes. - - Dan Lazewatsky - Dan Lazewatsky - BSD - - http://ros.org/wiki/rqt_top - https://github.com/ros-visualization/rqt_top - https://github.com/ros-visualization/rqt_top/issues - - catkin - - python-psutil - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_top/plugin.xml b/workspace/src/rqt_common_plugins/rqt_top/plugin.xml deleted file mode 100644 index e46960f9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - Provides information similar to that provided in UNIX top for running ROS nodes. - - - - - folder - Plugins related to introspection. - - - utilities-system-monitor - A Python GUI plugin for viewing process information about running ROS processes. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top b/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top deleted file mode 100755 index ff42031b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/scripts/rqt_top +++ /dev/null @@ -1,34 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_top.top_plugin.Top')) diff --git a/workspace/src/rqt_common_plugins/rqt_top/setup.py b/workspace/src/rqt_common_plugins/rqt_top/setup.py deleted file mode 100644 index 1887fb73..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_top'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/__init__.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py deleted file mode 100644 index 3024b89b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/node_info.py +++ /dev/null @@ -1,120 +0,0 @@ -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -import rosnode -import rospy -import xmlrpclib -import psutil - -ID = '/NODEINFO' - -class NodeInfo(object): - nodes = dict() - - def get_node_info(self, node_name, skip_cache=False): - node_api = rosnode.get_api_uri(rospy.get_master(), node_name, skip_cache=skip_cache) - try: - code, msg, pid = xmlrpclib.ServerProxy(node_api[2]).getPid(ID) - if node_name in self.nodes: - return self.nodes[node_name] - else: - try: - p = psutil.Process(pid) - self.nodes[node_name] = p - return p - except: - return False - except xmlrpclib.socket.error: - if not skip_cache: - return self.get_node_info(node_name, skip_cache=True) - else: - return False - - def get_all_node_info(self): - infos = [] - self.remove_dead_nodes() - for node_name in rosnode.get_node_names(): - info = self.get_node_info(node_name) - if info is not False: infos.append((node_name, info)) - return infos - - def get_all_node_fields(self, fields): - processes = self.get_all_node_info() - infos = [] - for name, p in processes: - infos.append(self.as_dict(p, fields + ['cmdline', 'get_memory_info'])) - infos[-1]['node_name'] = name - return infos - - def remove_dead_nodes(self): - running_nodes = rosnode.get_node_names() - dead_nodes = [node_name for node_name in self.nodes if node_name not in running_nodes] - for node_name in dead_nodes: - self.nodes.pop(node_name, None) - - def kill_node(self, node_name): - success, fail = rosnode.kill_nodes([node_name]) - return node_name in success - - def as_dict(self, p, attrs=[], ad_value=None): - # copied code from psutil.__init__ from a newer version - excluded_names = set(['send_signal', 'suspend', 'resume', 'terminate', - 'kill', 'wait', 'is_running', 'as_dict', 'parent', - 'get_children', 'nice']) - retdict = dict() - for name in set(attrs or dir(p)): - if name.startswith('_'): - continue - if name.startswith('set_'): - continue - if name in excluded_names: - continue - try: - attr = getattr(p, name) - if callable(attr): - if name == 'get_cpu_percent': - ret = attr(interval=0) - else: - ret = attr() - else: - ret = attr - except psutil.AccessDenied: - ret = ad_value - except NotImplementedError: - # in case of not implemented functionality (may happen - # on old or exotic systems) we want to crash only if - # the user explicitly asked for that particular attr - if attrs: - raise - continue - if name.startswith('get'): - if name[3] == '_': - name = name[4:] - elif name == 'getcwd': - name = 'cwd' - retdict[name] = ret - return retdict diff --git a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py b/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py deleted file mode 100644 index 4cdc898d..00000000 --- a/workspace/src/rqt_common_plugins/rqt_top/src/rqt_top/top_plugin.py +++ /dev/null @@ -1,192 +0,0 @@ -#!/usr/bin/env python -# Copyright (c) 2013, Oregon State University -# All rights reserved. - -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# * Neither the name of the Oregon State University nor the -# names of its contributors may be used to endorse or promote products -# derived from this software without specific prior written permission. - -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND -# ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -# DISCLAIMED. IN NO EVENT SHALL OREGON STATE UNIVERSITY BE LIABLE FOR ANY -# DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES -# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND -# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - -# Author Dan Lazewatsky/lazewatd@engr.orst.edu - -from __future__ import division - -from qt_gui.plugin import Plugin -from python_qt_binding import loadUi -from python_qt_binding.QtGui import QLabel, QTreeWidget, QTreeWidgetItem, QVBoxLayout, QCheckBox, QWidget, QToolBar, QLineEdit, QPushButton -from python_qt_binding.QtCore import Qt, QTimer - -from rqt_top.node_info import NodeInfo -import re -from threading import RLock -import textwrap - - -class TopWidgetItem(QTreeWidgetItem): - def __init__(self, parent=None): - super(TopWidgetItem, self).__init__(parent) - - def __lt__(self, other): - col = self.treeWidget().sortColumn() - dtype = Top.SORT_TYPE[col] - return dtype(self.text(col)) < dtype(other.text(col)) - - -class Top(Plugin): - - NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'] - OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ] - FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ] - NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ] - SORT_TYPE = [str, str, float, float, float ] - TOOLTIPS = { - 0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))), - 3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20))) - } - - _node_info = NodeInfo() - - name_filter = re.compile('') - - def __init__(self, context): - super(Top, self).__init__(context) - # Give QObjects reasonable names - self.setObjectName('Top') - - # Process standalone plugin command-line arguments - from argparse import ArgumentParser - parser = ArgumentParser() - # Add argument(s) to the parser. - parser.add_argument("-q", "--quiet", action="store_true", - dest="quiet", - help="Put plugin in silent mode") - args, unknowns = parser.parse_known_args(context.argv()) - # if not args.quiet: - # print 'arguments: ', args - # print 'unknowns: ', unknowns - - self._selected_node = '' - self._selected_node_lock = RLock() - - # Setup the toolbar - self._toolbar = QToolBar() - self._filter_box = QLineEdit() - self._regex_box = QCheckBox() - self._regex_box.setText('regex') - self._toolbar.addWidget(QLabel('Filter')) - self._toolbar.addWidget(self._filter_box) - self._toolbar.addWidget(self._regex_box) - - self._filter_box.returnPressed.connect(self.update_filter) - self._regex_box.stateChanged.connect(self.update_filter) - - # Create a container widget and give it a layout - self._container = QWidget() - self._container.setWindowTitle('Process Monitor') - self._layout = QVBoxLayout() - self._container.setLayout(self._layout) - - self._layout.addWidget(self._toolbar) - - # Create the table widget - self._table_widget = QTreeWidget() - self._table_widget.setObjectName('TopTable') - self._table_widget.setColumnCount(len(self.NODE_LABELS)) - self._table_widget.setHeaderLabels(self.NODE_LABELS) - self._table_widget.itemClicked.connect(self._tableItemClicked) - self._table_widget.setSortingEnabled(True) - self._table_widget.setAlternatingRowColors(True) - - self._layout.addWidget(self._table_widget) - context.add_widget(self._container) - - # Add a button for killing nodes - self._kill_button = QPushButton('Kill Node') - self._layout.addWidget(self._kill_button) - self._kill_button.clicked.connect(self._kill_node) - - # Update twice since the first cpu% lookup will always return 0 - self.update_table() - self.update_table() - - self._table_widget.resizeColumnToContents(0) - - # Start a timer to trigger updates - self._update_timer = QTimer() - self._update_timer.setInterval(1000) - self._update_timer.timeout.connect(self.update_table) - self._update_timer.start() - - def _tableItemClicked(self, item, column): - with self._selected_node_lock: - self._selected_node = item.text(0) - - def update_filter(self, *args): - if self._regex_box.isChecked(): - expr = self._filter_box.text() - else: - expr = re.escape(self._filter_box.text()) - self.name_filter = re.compile(expr) - self.update_table() - - def _kill_node(self): - self._node_info.kill_node(self._selected_node) - - def update_one_item(self, row, info): - twi = TopWidgetItem() - for col, field in enumerate(self.OUT_FIELDS): - val = info[field] - twi.setText(col, self.FORMAT_STRS[col] % val) - self._table_widget.insertTopLevelItem(row, twi) - - for col, (key, func) in self.TOOLTIPS.iteritems(): - twi.setToolTip(col, func(info[key])) - - with self._selected_node_lock: - if twi.text(0) == self._selected_node: - twi.setSelected(True) - - self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0) - - def update_table(self): - self._table_widget.clear() - infos = self._node_info.get_all_node_fields(self.NODE_FIELDS) - for nx, info in enumerate(infos): - self.update_one_item(nx, info) - - def shutdown_plugin(self): - self._update_timer.stop() - - def save_settings(self, plugin_settings, instance_settings): - instance_settings.set_value('filter_text', self._filter_box.text()) - instance_settings.set_value('is_regex', int(self._regex_box.checkState())) - - def restore_settings(self, plugin_settings, instance_settings): - self._filter_box.setText(instance_settings.value('filter_text')) - is_regex_int = instance_settings.value('is_regex') - if is_regex_int: - self._regex_box.setCheckState(Qt.CheckState(is_regex_int)) - else: - self._regex_box.setCheckState(Qt.CheckState(0)) - self.update_filter() - - #def trigger_configuration(self): - # Comment in to signal that the plugin has a way to configure it - # Usually used to open a configuration dialog diff --git a/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst deleted file mode 100644 index 79bd94d5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/CHANGELOG.rst +++ /dev/null @@ -1,113 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_topic -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- -* check for divide by zero and data failures -* Contributors: Aaron Blasdel - -0.3.12 (2015-07-24) -------------------- -* Save/Restore of headers added -* Contributors: Aaron Blasdel - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) -* catch and show exceptions `#198 `_ - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- -* improve rqt_topic initialization time (`#62 `_) -* modified toggling topics to use checkbox instead of context menu (`#75 `_) - -0.3.0 (2013-08-28) ------------------- -* fix cleaning old data in rqt_topic (fix `#74 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- -* Improve API (now either name or msg type are select-able in order to select which topics to monitor). -* API change to accept a list of the topics that this plugin watches. - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt deleted file mode 100644 index a57e51f5..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_topic) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_topic - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/package.xml b/workspace/src/rqt_common_plugins/rqt_topic/package.xml deleted file mode 100644 index 6c64d57b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_topic - 0.3.13 - rqt_topic provides a GUI plugin for displaying debug information about ROS topics including publishers, subscribers, publishing rate, and ROS Messages. - Dorian Scholz - - BSD - - http://ros.org/wiki/rqt_topic - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Dorian Scholz - - catkin - - python-rospkg - rostopic - rqt_gui - rqt_gui_py - std_msgs - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml b/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml deleted file mode 100644 index 901c99f2..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin to introspect ROS topics. - - - - - folder - Plugins related to ROS topics. - - - system-search - A Python GUI plugin for monitoring ROS topics. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui b/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui deleted file mode 100644 index 67430126..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/resource/TopicWidget.ui +++ /dev/null @@ -1,65 +0,0 @@ - - - TopicWidget - - - - 0 - 0 - 731 - 412 - - - - Topic Monitor - - - - - - Qt::CustomContextMenu - - - Right click on item for more options. - - - true - - - QAbstractItemView::DragOnly - - - true - - - - Topic - - - - - Type - - - - - Bandwidth - - - - - Hz - - - - - Value - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic b/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic deleted file mode 100755 index 34fe7339..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/scripts/rqt_topic +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_topic.topic.Topic')) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/setup.py b/workspace/src/rqt_common_plugins/rqt_topic/setup.py deleted file mode 100644 index 44482c44..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_topic'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/__init__.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py deleted file mode 100644 index 721bb9d6..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from rqt_gui_py.plugin import Plugin - -from .topic_widget import TopicWidget - - -class Topic(Plugin): - - def __init__(self, context): - super(Topic, self).__init__(context) - self.setObjectName('Topic') - - self._widget = TopicWidget(self) - - self._widget.start() - if context.serial_number() > 1: - self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._widget) - - def shutdown_plugin(self): - self._widget.shutdown_plugin() - - def save_settings(self, plugin_settings, instance_settings): - self._widget.save_settings(plugin_settings, instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - self._widget.restore_settings(plugin_settings, instance_settings) - diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py deleted file mode 100644 index bc28d17b..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_info.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division, with_statement -from StringIO import StringIO - -from python_qt_binding.QtCore import qWarning - -import roslib -import rospy -from rostopic import ROSTopicHz - - -class TopicInfo(ROSTopicHz): - - def __init__(self, topic_name, topic_type): - super(TopicInfo, self).__init__(100) - self._topic_name = topic_name - self.error = None - self._subscriber = None - self.monitoring = False - self._reset_data() - self.message_class = None - try: - self.message_class = roslib.message.get_message_class(topic_type) - except Exception as e: - self.message_class = None - qWarning('TopicInfo.__init__(): %s' % (e)) - - if self.message_class is None: - self.error = 'can not get message class for type "%s"' % topic_type - qWarning('TopicInfo.__init__(): topic "%s": %s' % (topic_name, self.error)) - - def _reset_data(self): - self.last_message = None - self.times = [] - self.timestamps = [] - self.sizes = [] - - def toggle_monitoring(self): - if self.monitoring: - self.stop_monitoring() - else: - self.start_monitoring() - - def start_monitoring(self): - if self.message_class is not None: - self.monitoring = True - # FIXME: subscribing to class AnyMsg breaks other subscribers on same node - self._subscriber = rospy.Subscriber(self._topic_name, self.message_class, self.message_callback) - - def stop_monitoring(self): - self.monitoring = False - self._reset_data() - if self._subscriber is not None: - self._subscriber.unregister() - - def message_callback(self, message): - ROSTopicHz.callback_hz(self, message) - with self.lock: - self.timestamps.append(rospy.get_time()) - - # FIXME: this only works for message of class AnyMsg - #self.sizes.append(len(message._buff)) - # time consuming workaround... - buff = StringIO() - message.serialize(buff) - self.sizes.append(buff.len) - - if len(self.timestamps) > self.window_size - 1: - self.timestamps.pop(0) - self.sizes.pop(0) - assert(len(self.timestamps) == len(self.sizes)) - - self.last_message = message - - def get_bw(self): - if len(self.timestamps) < 2: - return None, None, None, None - current_time = rospy.get_time() - if current_time <= self.timestamps[0]: - return None, None, None, None - - with self.lock: - total = sum(self.sizes) - bytes_per_s = total / (current_time - self.timestamps[0]) - mean_size = total / len(self.timestamps) - max_size = max(self.sizes) - min_size = min(self.sizes) - return bytes_per_s, mean_size, min_size, max_size - - def get_hz(self): - if not self.times: - return None, None, None, None - with self.lock: - n = len(self.times) - mean = sum(self.times) / n - rate = 1. / mean if mean > 0. else 0 - min_delta = min(self.times) - max_delta = max(self.times) - return rate, mean, min_delta, max_delta diff --git a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py b/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py deleted file mode 100644 index 8055132f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_topic/src/rqt_topic/topic_widget.py +++ /dev/null @@ -1,374 +0,0 @@ -#!/usr/bin/env python - -# Copyright (c) 2011, Dorian Scholz, TU Darmstadt -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of the TU Darmstadt nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from __future__ import division -import os - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QTimer, Signal, Slot -from python_qt_binding.QtGui import QHeaderView, QIcon, QMenu, QTreeWidgetItem, QWidget -import roslib -import rospkg -import rospy -from rospy.exceptions import ROSException - -from .topic_info import TopicInfo - - -class TopicWidget(QWidget): - """ - main class inherits from the ui window class. - - You can specify the topics that the topic pane. - - TopicWidget.start must be called in order to update topic pane. - """ - - SELECT_BY_NAME = 0 - SELECT_BY_MSGTYPE = 1 - - _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value'] - - def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME): - """ - @type selected_topics: list of tuples. - @param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...] - @type select_topic_type: int - @param select_topic_type: Can specify either the name of topics or by - the type of topic, to filter the topics to - show. If 'select_topic_type' argument is - None, this arg shouldn't be meaningful. - """ - super(TopicWidget, self).__init__() - - self._select_topic_type = select_topic_type - - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'TopicWidget.ui') - loadUi(ui_file, self) - self._plugin = plugin - self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder) - header = self.topics_tree_widget.header() - header.setResizeMode(QHeaderView.ResizeToContents) - header.customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested) - header.setContextMenuPolicy(Qt.CustomContextMenu) - - # Whether to get all topics or only the topics that are set in advance. - # Can be also set by the setter method "set_selected_topics". - self._selected_topics = selected_topics - - self._current_topic_list = [] - self._topics = {} - self._tree_items = {} - self._column_index = {} - for column_name in self._column_names: - self._column_index[column_name] = len(self._column_index) - - # self.refresh_topics() - - # init and start update timer - self._timer_refresh_topics = QTimer(self) - self._timer_refresh_topics.timeout.connect(self.refresh_topics) - - def set_topic_specifier(self, specifier): - self._select_topic_type = specifier - - def start(self): - """ - This method needs to be called to start updating topic pane. - """ - self._timer_refresh_topics.start(1000) - - @Slot() - def refresh_topics(self): - """ - refresh tree view items - """ - - if self._selected_topics is None: - topic_list = rospy.get_published_topics() - if topic_list is None: - rospy.logerr('Not even a single published topic found. Check network configuration') - return - else: # Topics to show are specified. - topic_list = self._selected_topics - topic_specifiers_server_all = None - topic_specifiers_required = None - - rospy.logdebug('refresh_topics) self._selected_topics=%s' % (topic_list,)) - - if self._select_topic_type == self.SELECT_BY_NAME: - topic_specifiers_server_all = [name for name, type in rospy.get_published_topics()] - topic_specifiers_required = [name for name, type in topic_list] - elif self._select_topic_type == self.SELECT_BY_MSGTYPE: - # The topics that are required (by whoever uses this class). - topic_specifiers_required = [type for name, type in topic_list] - - # The required topics that match with published topics. - topics_match = [(name, type) for name, type in rospy.get_published_topics() if type in topic_specifiers_required] - topic_list = topics_match - rospy.logdebug('selected & published topic types=%s' % (topic_list,)) - - rospy.logdebug('server_all=%s\nrequired=%s\ntlist=%s' % (topic_specifiers_server_all, topic_specifiers_required, topic_list)) - if len(topic_list) == 0: - rospy.logerr('None of the following required topics are found.\n(NAME, TYPE): %s' % (self._selected_topics,)) - return - - if self._current_topic_list != topic_list: - self._current_topic_list = topic_list - - # start new topic dict - new_topics = {} - - for topic_name, topic_type in topic_list: - # if topic is new or has changed its type - if topic_name not in self._topics or \ - self._topics[topic_name]['type'] != topic_type: - # create new TopicInfo - topic_info = TopicInfo(topic_name, topic_type) - message_instance = None - if topic_info.message_class is not None: - message_instance = topic_info.message_class() - # add it to the dict and tree view - topic_item = self._recursive_create_widget_items(self.topics_tree_widget, topic_name, topic_type, message_instance) - new_topics[topic_name] = { - 'item': topic_item, - 'info': topic_info, - 'type': topic_type, - } - else: - # if topic has been seen before, copy it to new dict and - # remove it from the old one - new_topics[topic_name] = self._topics[topic_name] - del self._topics[topic_name] - - # clean up old topics - for topic_name in self._topics.keys(): - self._topics[topic_name]['info'].stop_monitoring() - index = self.topics_tree_widget.indexOfTopLevelItem( - self._topics[topic_name]['item']) - self.topics_tree_widget.takeTopLevelItem(index) - del self._topics[topic_name] - - # switch to new topic dict - self._topics = new_topics - - self._update_topics_data() - - def _update_topics_data(self): - for topic in self._topics.values(): - topic_info = topic['info'] - if topic_info.monitoring: - # update rate - rate, _, _, _ = topic_info.get_hz() - rate_text = '%1.2f' % rate if rate != None else 'unknown' - - # update bandwidth - bytes_per_s, _, _, _ = topic_info.get_bw() - if bytes_per_s is None: - bandwidth_text = 'unknown' - elif bytes_per_s < 1000: - bandwidth_text = '%.2fB/s' % bytes_per_s - elif bytes_per_s < 1000000: - bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.) - else: - bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.) - - # update values - value_text = '' - self.update_value(topic_info._topic_name, topic_info.last_message) - - else: - rate_text = '' - bandwidth_text = '' - value_text = 'not monitored' if topic_info.error is None else topic_info.error - - self._tree_items[topic_info._topic_name].setText(self._column_index['rate'], rate_text) - self._tree_items[topic_info._topic_name].setText(self._column_index['bandwidth'], bandwidth_text) - self._tree_items[topic_info._topic_name].setText(self._column_index['value'], value_text) - - def update_value(self, topic_name, message): - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name in message.__slots__: - self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name)) - - elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'): - - for index, slot in enumerate(message): - if topic_name + '[%d]' % index in self._tree_items: - self.update_value(topic_name + '[%d]' % index, slot) - else: - base_type_str, _ = self._extract_array_info(self._tree_items[topic_name].text(self._column_index['type'])) - self._recursive_create_widget_items(self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot) - # remove obsolete children - if len(message) < self._tree_items[topic_name].childCount(): - for i in range(len(message), self._tree_items[topic_name].childCount()): - item_topic_name = topic_name + '[%d]' % i - self._recursive_delete_widget_items(self._tree_items[item_topic_name]) - else: - if topic_name in self._tree_items: - self._tree_items[topic_name].setText(self._column_index['value'], repr(message)) - - def _extract_array_info(self, type_str): - array_size = None - if '[' in type_str and type_str[-1] == ']': - type_str, array_size_str = type_str.split('[', 1) - array_size_str = array_size_str[:-1] - if len(array_size_str) > 0: - array_size = int(array_size_str) - else: - array_size = 0 - - return type_str, array_size - - def _recursive_create_widget_items(self, parent, topic_name, type_name, message): - if parent is self.topics_tree_widget: - # show full topic name with preceding namespace on toplevel item - topic_text = topic_name - item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent) - else: - topic_text = topic_name.split('/')[-1] - if '[' in topic_text: - topic_text = topic_text[topic_text.index('['):] - item = QTreeWidgetItem(parent) - item.setText(self._column_index['topic'], topic_text) - item.setText(self._column_index['type'], type_name) - item.setData(0, Qt.UserRole, topic_name) - self._tree_items[topic_name] = item - if hasattr(message, '__slots__') and hasattr(message, '_slot_types'): - for slot_name, type_name in zip(message.__slots__, message._slot_types): - self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name)) - - else: - base_type_str, array_size = self._extract_array_info(type_name) - try: - base_instance = roslib.message.get_message_class(base_type_str)() - except (ValueError, TypeError): - base_instance = None - if array_size is not None and hasattr(base_instance, '__slots__'): - for index in range(array_size): - self._recursive_create_widget_items(item, topic_name + '[%d]' % index, base_type_str, base_instance) - return item - - def _toggle_monitoring(self, topic_name): - item = self._tree_items[topic_name] - if item.checkState(0): - self._topics[topic_name]['info'].start_monitoring() - else: - self._topics[topic_name]['info'].stop_monitoring() - - def _recursive_delete_widget_items(self, item): - def _recursive_remove_items_from_tree(item): - for index in reversed(range(item.childCount())): - _recursive_remove_items_from_tree(item.child(index)) - topic_name = item.data(0, Qt.UserRole) - del self._tree_items[topic_name] - _recursive_remove_items_from_tree(item) - item.parent().removeChild(item) - - @Slot('QPoint') - def handle_header_view_customContextMenuRequested(self, pos): - header = self.topics_tree_widget.header() - - # show context menu - menu = QMenu(self) - action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize') - action = menu.exec_(header.mapToGlobal(pos)) - - # evaluate user action - if action is action_toggle_auto_resize: - if header.resizeMode(0) == QHeaderView.ResizeToContents: - header.setResizeMode(QHeaderView.Interactive) - else: - header.setResizeMode(QHeaderView.ResizeToContents) - - @Slot('QPoint') - def on_topics_tree_widget_customContextMenuRequested(self, pos): - item = self.topics_tree_widget.itemAt(pos) - if item is None: - return - - # show context menu - menu = QMenu(self) - action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children') - action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children') - action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos)) - - # evaluate user action - if action in (action_item_expand, action_item_collapse): - expanded = (action is action_item_expand) - - def recursive_set_expanded(item): - item.setExpanded(expanded) - for index in range(item.childCount()): - recursive_set_expanded(item.child(index)) - recursive_set_expanded(item) - - def shutdown_plugin(self): - for topic in self._topics.values(): - topic['info'].stop_monitoring() - self._timer_refresh_topics.stop() - - def set_selected_topics(self, selected_topics): - """ - @param selected_topics: list of tuple. [(topic_name, topic_type)] - @type selected_topics: [] - """ - rospy.logdebug('set_selected_topics topics={}'.format( - len(selected_topics))) - self._selected_topics = selected_topics - - # TODO(Enhancement) Save/Restore tree expansion state - def save_settings(self, plugin_settings, instance_settings): - header_state = self.topics_tree_widget.header().saveState() - instance_settings.set_value('tree_widget_header_state', header_state) - - def restore_settings(self, pluggin_settings, instance_settings): - if instance_settings.contains('tree_widget_header_state'): - header_state = instance_settings.value('tree_widget_header_state') - if not self.topics_tree_widget.header().restoreState(header_state): - rospy.logwarn("rqt_topic: Failed to restore header state.") - -class TreeWidgetItem(QTreeWidgetItem): - - def __init__(self, check_state_changed_callback, topic_name, parent=None): - super(TreeWidgetItem, self).__init__(parent) - self._check_state_changed_callback = check_state_changed_callback - self._topic_name = topic_name - self.setCheckState(0, Qt.Unchecked) - - def setData(self, column, role, value): - if role == Qt.CheckStateRole: - state = self.checkState(column) - super(TreeWidgetItem, self).setData(column, role, value) - if role == Qt.CheckStateRole and state != self.checkState(column): - self._check_state_changed_callback(self._topic_name) diff --git a/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst b/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst deleted file mode 100644 index f0af5027..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/CHANGELOG.rst +++ /dev/null @@ -1,104 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package rqt_web -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -0.3.13 (2016-03-08) -------------------- - -0.3.12 (2015-07-24) -------------------- - -0.3.11 (2015-04-30) -------------------- - -0.3.10 (2014-10-01) -------------------- -* update plugin scripts to use full name to avoid future naming collisions - -0.3.9 (2014-08-18) ------------------- - -0.3.8 (2014-07-15) ------------------- - -0.3.7 (2014-07-11) ------------------- -* export architecture_independent flag in package.xml (`#254 `_) - -0.3.6 (2014-06-02) ------------------- - -0.3.5 (2014-05-07) ------------------- - -0.3.4 (2014-01-28) ------------------- - -0.3.3 (2014-01-08) ------------------- -* add groups for rqt plugins, renamed some plugins (`#167 `_) - -0.3.2 (2013-10-14) ------------------- - -0.3.1 (2013-10-09) ------------------- - -0.3.0 (2013-08-28) ------------------- -* fix rendering of icons on OS X (`ros-visualization/rqt#83 `_) - -0.2.17 (2013-07-04) -------------------- - -0.2.16 (2013-04-09 13:33) -------------------------- - -0.2.15 (2013-04-09 00:02) -------------------------- - -0.2.14 (2013-03-14) -------------------- - -0.2.13 (2013-03-11 22:14) -------------------------- - -0.2.12 (2013-03-11 13:56) -------------------------- - -0.2.11 (2013-03-08) -------------------- - -0.2.10 (2013-01-22) -------------------- - -0.2.9 (2013-01-17) ------------------- - -0.2.8 (2013-01-11) ------------------- - -0.2.7 (2012-12-24) ------------------- - -0.2.6 (2012-12-23) ------------------- - -0.2.5 (2012-12-21 19:11) ------------------------- - -0.2.4 (2012-12-21 01:13) ------------------------- - -0.2.3 (2012-12-21 00:24) ------------------------- - -0.2.2 (2012-12-20 18:29) ------------------------- - -0.2.1 (2012-12-20 17:47) ------------------------- - -0.2.0 (2012-12-20 17:39) ------------------------- -* first release of this package into groovy diff --git a/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt b/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt deleted file mode 100644 index 424592cb..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/CMakeLists.txt +++ /dev/null @@ -1,18 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(rqt_web) -# Load catkin and all dependencies required for this package -find_package(catkin REQUIRED COMPONENTS) -catkin_package() -catkin_python_setup() - -install(FILES plugin.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(DIRECTORY resource - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -install(PROGRAMS scripts/rqt_web - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) diff --git a/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox b/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox deleted file mode 100644 index 51e27395..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/mainpage.dox +++ /dev/null @@ -1,7 +0,0 @@ -/** -\mainpage -\htmlinclude manifest.html - -\b rqt_web provides a GUI plugin for displaying and interacting with web tools - -*/ diff --git a/workspace/src/rqt_common_plugins/rqt_web/package.xml b/workspace/src/rqt_common_plugins/rqt_web/package.xml deleted file mode 100644 index ac87cba9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - rqt_web - 0.3.13 - rqt_web is a simple web content viewer for rqt. Users can show web content in Qt-based window by specifying its URL. - Aaron Blasdel - - BSD - - http://ros.org/wiki/rqt_web - https://github.com/ros-visualization/rqt_common_plugins - https://github.com/ros-visualization/rqt_common_plugins/issues - - Aaron Blasdel - - catkin - - python-rospkg - qt_gui - rospy - rqt_gui - rqt_gui_py - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/plugin.xml b/workspace/src/rqt_common_plugins/rqt_web/plugin.xml deleted file mode 100644 index 33b57d9f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/plugin.xml +++ /dev/null @@ -1,17 +0,0 @@ - - - - A Python GUI plugin displaying and interacting with web tools. - - - - - folder - Plugins related to miscellaneous tools. - - - applications-internet - A Python GUI plugin for displaying and interacting with web tools. - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui b/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui deleted file mode 100644 index 5181c11f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/resource/web_widget.ui +++ /dev/null @@ -1,59 +0,0 @@ - - - Web - - - - 0 - 0 - 637 - 557 - - - - Web - - - - 0 - - - 1 - - - - - - - - - - - 27 - 16777215 - - - - - - - - 16 - 16 - - - - true - - - true - - - - - - - - - - diff --git a/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web b/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web deleted file mode 100755 index 5b0bccd9..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/scripts/rqt_web +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import sys - -from rqt_gui.main import Main - -main = Main() -sys.exit(main.main(sys.argv, standalone='rqt_web.web.Web')) diff --git a/workspace/src/rqt_common_plugins/rqt_web/setup.py b/workspace/src/rqt_common_plugins/rqt_web/setup.py deleted file mode 100644 index f08e4d01..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup( - packages=['rqt_web'], - package_dir={'': 'src'} -) - -setup(**d) diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/__init__.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/__init__.py deleted file mode 100644 index e69de29b..00000000 diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py deleted file mode 100644 index f478a892..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web.py +++ /dev/null @@ -1,72 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -from qt_gui.plugin import Plugin - -from web_widget import WebWidget - - -class Web(Plugin): - """ - Plugin to interface with webtools via ros_gui - """ - def __init__(self, context): - """ - :param context: plugin context hook to enable adding widgets as a ROS_GUI pane, ''PluginContext'' - """ - super(Web, self).__init__(context) - self.setObjectName('Web') - - # This method is used to allow user to type a url into the url bar - self._web = WebWidget() - if context.serial_number() > 1: - self._web.setWindowTitle(self._web.windowTitle() + (' (%d)' % context.serial_number())) - context.add_widget(self._web) - - # This method is used to specify a static url - # NOTE: this method will hide the url bar - #self._web = WebWidget('http://ros.org') - #context.add_widget(self._web) - - # To change the url at a later time use this function - # self._web.set_url('http://willowgarage.com') - - def shutdown_plugin(self): - pass - - def save_settings(self, plugin_settings, instance_settings): - # NOTE: This line is required to save the url bar autocompleter data between sessions - self._web.save_settings(instance_settings) - - def restore_settings(self, plugin_settings, instance_settings): - # NOTE: This line is required to restore the url bar autocompleter data between sessions - self._web.restore_settings(instance_settings) diff --git a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py b/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py deleted file mode 100644 index 73e78b1f..00000000 --- a/workspace/src/rqt_common_plugins/rqt_web/src/rqt_web/web_widget.py +++ /dev/null @@ -1,174 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -import os -import rospkg - -from python_qt_binding import loadUi -from python_qt_binding.QtCore import Qt, QUrl -from python_qt_binding.QtGui import QCompleter, QIcon, QWidget -from python_qt_binding.QtWebKit import QWebPage, QWebView - - -class WebWidget(QWidget): - def __init__(self, url=None): - """ - Class to load a webpage in a widget. - - :param url: If url is empty then a navigation bar is shown otherwise the url is loaded and the navigation bar is hidden, ''str'' - """ - super(WebWidget, self).__init__() - rp = rospkg.RosPack() - ui_file = os.path.join(rp.get_path('rqt_web'), 'resource', 'web_widget.ui') - loadUi(ui_file, self) - self.setObjectName('WebWidget') - - self._loading = False - self._stop_icon = QIcon.fromTheme('process-stop') - self._reload_icon = QIcon.fromTheme('view-refresh') - self._working_icon = QIcon.fromTheme('process-working') - - self._completer_word_list = [''] - self._view = QWebView() - self.verticalLayout.addWidget(self._view) - if url is None: - self.set_url("http://ros.org", True) - else: - self.set_url(url, False) - - self.url_lineedit.returnPressed.connect(self._handle_url_change) - self._view.loadFinished[bool].connect(self._handle_load_finished) - self.reload_button.clicked.connect(self._handle_reload_clicked) - self._view.linkClicked.connect(self._handle_link_clicked) - self._view.urlChanged[QUrl].connect(self._handle_url_changed) - - def set_url(self, url, showinput=False): - """ - Sets the url and begins loading that page - :param url: url to load in the webview, ''str or QUrl'' - :param showinput: if true the input bar will be shown, else hidden, ''bool'' - """ - if url is not None: - self._url = QUrl(url) - self.set_show_url_input(showinput) - self._view.setUrl(self._url) - - def set_show_url_input(self, showinput): - """ - Sets the value of the show_url_input flag and hides/shows the widgets as required - :param showinput: true - show inputbar false - hide , ''bool'' - """ - self._show_url_input = showinput - self.url_lineedit.setVisible(self._show_url_input) - self.reload_button.setVisible(self._show_url_input) - if self._show_url_input: - self._view.page().setLinkDelegationPolicy(QWebPage.DelegateAllLinks) - else: - self._view.page().setLinkDelegationPolicy(QWebPage.DontDelegateLinks) - - def save_settings(self, settings): - settings.set_value('url_completion', self._pack(self._completer_word_list)) - settings.set_value('url_current', self._url.toString()) - - def restore_settings(self, settings): - self._completer_word_list += self._unpack(settings.value('url_completion')) - self._completer_word_list = list(set(self._completer_word_list)) - url = settings.value('url_current') - if url: - self.set_url(url, self._show_url_input) - - def _handle_url_change(self): - self.set_url(self.url_lineedit.text(), True) - - def _handle_link_clicked(self, url): - self.set_url(url, True) - - def _handle_reload_clicked(self): - if self._loading: - self._view.stop() - self._loading = False - self.reload_button.setIcon(self._reload_icon) - else: - self._view.reload() - self._loading = True - self.reload_button.setIcon(self._stop_icon) - - def _handle_url_changed(self, url): - # set text to the current loading item - self.url_lineedit.setText(url.toString()) - self.reload_button.setIcon(self._stop_icon) - self._loading = True - - def _handle_load_finished(self, ok): - self._loading = False - self.reload_button.setIcon(self._reload_icon) - if ok: - self._add_completer_list_item(self._url.toString()) - else: - # need to disconnect or we will resend the signal once the error page loads - self._view.loadFinished[bool].disconnect(self._handle_load_finished) - self._view.page().currentFrame().setHtml('

The url you entered seems to be faulty.

') - self._view.loadFinished[bool].connect(self._handle_load_finished) - - def _add_completer_list_item(self, url): - self._completer_word_list.append(self.url_lineedit.text()) - self._completer_word_list = list(set(self._completer_word_list)) - self._completer = QCompleter(self._completer_word_list) - self._completer.setCaseSensitivity(Qt.CaseInsensitive) - self._completer.setCompletionMode(QCompleter.PopupCompletion) - self.url_lineedit.setCompleter(self._completer) - - @staticmethod - def _pack(data): - """ - Packs 'data' into a form that can be easily and readably written to an ini file - :param data: A list of strings to be flattened into a string ''list'' - :return: A string suitable for output to ini files ''str'' - """ - if len(data) == 0: - return '' - if len(data) == 1: - return data[0] - return data - - @staticmethod - def _unpack(data): - """ - Unpacks the values read from an ini file - :param data: An entry taken from an ini file ''list or string'' - :return: A list of strings ''list'' - """ - if data is None or data == '': - data = [] - elif isinstance(data, basestring): - data = [data] - return data