void ExcavaROBOdometry::update()
{
if (!isInputValid()) {
if (verbose_)
debug_publisher_->msg_.input_valid = false;
ROS_DEBUG("Odometry: Input velocities are invalid");
return;
}
else {
if (verbose_)
debug_publisher_->msg_.input_valid = true;
}
current_time_ = base_kin_.robot_state_->getTime();
ros::Time update_start = ros::Time::now();
updateOdometry();
// double update_time = (ros::Time::now()-update_start).toSec();
ros::Time publish_start = ros::Time::now();
if (publish_odom_)
publishOdometry();
if (publish_odometer_)
publishOdometer();
if (publish_state_odometry_)
publishStateOdometry();
if (publish_tf_)
publishTransform();
last_r_wheel_rotation_ = current_r_wheel_rotation_;
last_l_wheel_rotation_ = current_l_wheel_rotation_;
last_time_ = current_time_;
sequence_ ++;
}
void MotionCore::processMotionFrame() {
unsigned int &frame_id = frame_info_->frame_id;
if (alreadyProcessedFrame()) {
//std::cout << "processMotionFrame, skipping frame " << frame_info_->frame_id << std::endl;
return;
}
if (frame_id > last_frame_processed_ + 1) {
std::cout << "Skipped a frame: went from " << last_frame_processed_ << " to " << frame_id << std::endl;
}
// frame rate
double time_passed = frame_info_->seconds_since_start - fps_time_;
fps_frames_processed_++;
if (time_passed >= 10.0) {
std::cout << "MOTION FRAME RATE: " << fps_frames_processed_ / time_passed << std::endl;
fps_frames_processed_ = 0;
fps_time_ = frame_info_->seconds_since_start;
}
// actually do stuff
processSensorUpdate();
// check if the get up wants us to stand
if (getup_->needsStand()) {
walk_request_->new_command_ = true;
walk_request_->slow_stand_ = true;
}
// Determine commands
if (walk_ != NULL)
walk_->handleStepIntoKick();
// kicks need to be before walk, so that they can change the walk request
if (use_com_kick_)
kick_->processFrame();
else
motion_->processFrame();
if (walk_ != NULL)
walk_->processFrame();
updateOdometry();
// override commands with getup if necessary
// cout<<(walk_request_->motion_==WalkRequestBlock::FALLING)<<"("<< walk_request_->fallen_counter_<<std::endl;
// cout<<(walk_request_->motion_==WalkRequestBlock::FALLING)<<"("<<processed_sensors->values_[angleY]<<","<<processed_sensors->values_[angleX]<<")"<<std::endl;
if (getup_->isGettingUp() || walk_request_->motion_==WalkRequestBlock::FALLING){
// possibly init get up
if (!getup_->isGettingUp())
getup_->initGetup();
getup_->processFrame();
}
//std::cout << "left " << body_model_->rel_parts_no_rotations_[BodyPart::left_hand].translation << " | right " << body_model_->rel_parts_no_rotations_[BodyPart::right_hand].translation << std::endl;
last_frame_processed_ = frame_id;
}
void RosieOdometry::update()
{
current_time_ = base_kin_.robot_state_->getTime();
updateOdometry();
publish();
last_time_ = current_time_;
}
void Irobot::sensorCB(const turtlebot_node::TurtlebotSensorStateConstPtr& packet){
connected = true;
if(!saveInProgress)
{
currentSensorPacket = packet;
updateOdometry();
}
}
void LaserMapping::laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
_timeLaserOdometry = laserOdometry->header.stamp;
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
updateOdometry(-pitch, -yaw, roll,
laserOdometry->pose.pose.position.x,
laserOdometry->pose.pose.position.y,
laserOdometry->pose.pose.position.z);
_newLaserOdometry = true;
}
// Primary Interrupt vector
void interrupt isr()
{
if (SSPIF == 1) // interrupt is I2C related
{
F.I2C = 1; // set i2c flag bit
//Update OdometryCounts
updateOdometry();
//Go to the i2cisrhandler
i2cIsrHandler(); // interrupt flag was cleared in this function
} else if (T0IF == 1) // overflow of timer 0
{ // TMR0 overflows every 10 ms
TMR0OverflowCounter++;
if (TMR0OverflowCounter == 5){ // 5 times of TMR0 overflow = 50 ms or 0.05 second
F.T0 = 1; // set T0 flag bit
TMR0OverflowCounter = 0;
}
T0IF = 0;
} else if (TMR1IF == 1) // overflow of counts; probably never happens
{ // Only 2 bytes of data are being sent through I2C so therefore, the
// overflown is not taken into account right now
TMR1IF = 0;
} else if (RBIF == 1) // PORTB change
{
if (RB5 != DirectionRead) // verify that it was a direction change
{
F.DIR = 1; // set direction flag bit
} else // Error, probably H-Bridge related
{
; // Not sure if we need to do anything right now
}
RBIF = 0;
} else // Any other interrupt error
{
; // Not sure if we need to do anything right now
}
} // end interrupt function
/**
* @brief RobotThread::RobotThread
*/
RobotThread::RobotThread()
{
// necessary to send this type over qued connections
qRegisterMetaType< QVector<double> >("QVector<double>");
qRegisterMetaType< QList<Obstacle> >("QList<Obstacle>");
qRegisterMetaType< QList< QPair<double,double> > >("QList< QPair<double,double> >"); // fuer path planning -> path realizer
qRegisterMetaType< PathPlotData >("PathPlotData");
qRegisterMetaType< FilterParams >("FilterParams");
qRegisterMetaType< CameraParams >("CameraParams");
qRegisterMetaType< PIDParams >("PIDParams");
qRegisterMetaType< PIDPlotData >("PIDPlotData");
qRegisterMetaType< cv::Mat >("cv::Mat");
qRegisterMetaType< QString >("QString"); //Log
qRegisterMetaType< LaserPlotData > ("LaserPlotData");
qRegisterMetaType< Position > ("Position");
qRegisterMetaType< TeamColor >("TeamColor");
qRegisterMetaType< CamColor >("CamColor");
qDebug() << "INIT 'RobotThread'";
//initialize Objects for Actors and Sensory
actorLowLevel = new ActorLowLevel();
actorHighLevel = new ActorHighLevel();
sensorHighLevel = new SensorHighLevel();
sensorLowLevel = new SensorLowLevel();
pathPlanner = new PathPlanning();
gameEngine = new GameEngine();
cam = new Cam();
// moving objects to different threads
sensorLowLevel->moveToThread(&threadRobotLowLevel);
sensorHighLevel->moveToThread(&threadSensorHighLevel);
actorLowLevel->moveToThread(&threadRobotLowLevel);
actorHighLevel->moveToThread(&threadActorHighLevel);
pathPlanner->moveToThread(&threadPathPlanner);
gameEngine->moveToThread(&threadGameEngine);
cam->moveToThread(&threadCam);
// Name the threads (helpful for debugging)
threadRobotLowLevel.setObjectName("Robot Low Level Thread");
threadSensorHighLevel.setObjectName("High Level Sensor Thread");
threadPathRealizer.setObjectName("Path Realizer Thread");
threadPathPlanner.setObjectName("Path Planner Thread");
threadGameEngine.setObjectName("Game Engine Thread");
threadCam.setObjectName("Cam Thread");
// ***********************************************************************
// DELETE LATER
connect(&threadRobotLowLevel, SIGNAL(finished()),
sensorLowLevel, SLOT(deleteLater()));
connect(&threadSensorHighLevel, SIGNAL(finished()),
sensorHighLevel, SLOT(deleteLater()));
connect(&threadRobotLowLevel, SIGNAL(finished()),
actorLowLevel, SLOT(deleteLater()));
connect(&threadActorHighLevel, SIGNAL(finished()),
actorHighLevel, SLOT(deleteLater()));
connect(&threadPathPlanner, SIGNAL(finished()),
pathPlanner, SLOT(deleteLater()));
connect(&threadCam, SIGNAL(finished()),
cam, SLOT(deleteLater()));
connect(&threadGameEngine, SIGNAL(finished()),
gameEngine, SLOT(deleteLater()));
// ***********************************************************************
// SIGNALS FROM GUI
// start Orientation
connect(mainWindow,SIGNAL(signalStartOrientation(bool)),
sensorHighLevel, SLOT(slotStartDetection(bool)));
// remote controle over GUI
connect(mainWindow,SIGNAL(robotRemoteControllUpdate(double,double)),
actorLowLevel,SLOT(setRobotRemoteControllParams(double,double)));
// remote Odometry update
connect(mainWindow,SIGNAL(updateRemoteOdometry(Position)),
actorLowLevel, SLOT(setOdometry(Position)));
// Cam Display
// Camera Parameters in GUI have changed, send to Cam class
connect(mainWindow, SIGNAL(signalChangeCamParams(CameraParams)),
cam, SLOT(slotSetCameraParams(CameraParams)));
// Set Median Filter
connect(mainWindow, SIGNAL(signalChangeFilterParams(FilterParams)),
sensorHighLevel, SLOT(slotSetFilterParams(FilterParams)));
// PID Tab
connect(mainWindow, SIGNAL(signalChangePIDParams(PIDParams)),
actorHighLevel, SLOT(slotChangePIDParams(PIDParams)));
// ***********************************************************************
// SIGNALS TO GUI
// Sensor Display
// Display the sensor data in GUI
connect(sensorLowLevel, SIGNAL(signalLaserPlotRaw(LaserPlotData)), // raw data
mainWindow, SLOT(slotLaserDisplay(LaserPlotData)));
connect(sensorHighLevel, SIGNAL(signalSendLaserData(LaserPlotData)), // filtered data
mainWindow, SLOT(slotLaserDisplay(LaserPlotData)));
// Path Display
// Display potential map and raw waypoints
connect(pathPlanner, SIGNAL(pathDisplay(PathPlotData)),
mainWindow, SLOT(updatePathDisplay(PathPlotData)));
// Display calculated spline
connect(actorHighLevel, SIGNAL(signalSplinePlot(PathPlotData)),
mainWindow, SLOT(updatePathDisplay(PathPlotData)));
// Display PID Plot
connect(actorHighLevel, SIGNAL(signalPIDPlot(PIDPlotData)),
mainWindow, SLOT(slotPIDPlot(PIDPlotData)));
// Cam grabbed a frame, display in GUI
connect(cam, SIGNAL(signalDisplayFrame(cv::Mat)),
mainWindow, SLOT(slotDisplayFrame(cv::Mat)));
// ***********************************************************************
// emergency detected/received
connect(sensorHighLevel,SIGNAL(signalEmergencyStopEnabled(bool)), // emergency stop durch kollisionsvermeidung
actorLowLevel,SLOT(slotEmergencyStopEnabled(bool)));
connect(gameEngine, SIGNAL(signalEmergencyStopEnabled(bool)), // stopMovement() von Referee
actorLowLevel, SLOT(slotEmergencyStopEnabled(bool)));
//send turn command from high sensor to low level actor
connect(sensorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)),
actorLowLevel,SLOT(setRobotControllParams(double,double)));
// controle command from High Level Aktor
connect(actorHighLevel,SIGNAL(signalSendRobotControlParams(double,double)),
actorLowLevel,SLOT(setRobotControllParams(double,double)));
// Get waypoints from path planning into the path realizer
connect(pathPlanner, SIGNAL(sendUpdatedWaypoints(QList< QPair<double,double> >)),
actorHighLevel, SLOT(slotUpdateWaypoints(QList< QPair<double,double> >)));
// SENSOR DATA TO HIGH LEVEL SENSOR
connect(sensorLowLevel,SIGNAL(laserDataReady(QVector<double>)),
sensorHighLevel, SLOT(getLaserData(QVector<double>)));
connect(sensorLowLevel,SIGNAL(sonarDataReady(QVector<double>)),
sensorHighLevel, SLOT(getSonarData(QVector<double>)));
// start color detection from sensor
connect(sensorHighLevel, SIGNAL(signalStartColorDetection()),
cam, SLOT(slotStartColorDetection()));
// color succesfully detected
connect(cam, SIGNAL(signalColorDetected(CamColor)),
sensorHighLevel, SLOT(slotColorDetected(CamColor)));
// ***********************************************************************
// update Odometry
connect(sensorLowLevel,SIGNAL(updateOdometry()),
actorLowLevel, SLOT(getOdometry()));
connect(sensorHighLevel, SIGNAL(sendOdometryData(Position)),
actorLowLevel, SLOT(setOdometry(Position)));
// ***********************************************************************
// Signals from GameEngine
connect(gameEngine,SIGNAL(signalStartDetection(bool)),
sensorHighLevel, SLOT(slotStartDetection(bool)));
// Signals to GameEngine
connect(sensorHighLevel, SIGNAL(signalSendTeamColor(CamColor)),
gameEngine, SLOT(slotDetectionFinished(CamColor)));
// starting the eventloop of all threads
threadRobotLowLevel.start();
threadSensorHighLevel.start();
threadPathRealizer.start();
threadPathPlanner.start();
threadCam.start();
threadGameEngine.start();
// Starte Game Engine State Machine
gameEngine->start();
// Start the PathPlanning "loop"
/// \todo: Das ist erstmal nur zum Debuggen drinnen.
// Spaeter darf die Pfadplanung nur bei beginn der Spielphase aus der GameEngine heraus angestossen werden
QMetaObject::invokeMethod(pathPlanner, "planPath");
}
//-----------------------------------------------------------------------------
// MAIN STARTS HERE -----------------------------------------------------------
int main()
{
// BEGIN
Initialise(); // Function to do initial setup
while(1) // Infinite loop
{
if (F.I2C == 1) // I2C message was received
{
// Perform some I2C operation
// If read, reset Data and PID information
// Perform operation for Target
Target = i2cTarget; // Receive velocity from CPU
setDirection(MOTOR_DIRECTION); // Set direction of the motor to the direction
// sent from CPU
OdometryCounts = 0; // Reset the variables for PID and odometry
if (i2cTarget == 0) // This to ensure a sharp stop
{
setPWM(0);
TargetZeroFlag = 1;
}
// Clear Flag
F.I2C = 0;
}
if (F.DIR == 1)
{
// Update counts before updating direction
updateOdometry();
// Update direction
DirectionRead = PORTBbits.RB5;
// Clear Flag
F.DIR = 0;
}
// PID Loop occurs at regular time intervals
if (F.T0 == 1)
{
// Update to most recent encoder counts
updateEncoder();
// If we recieve a stop command then just skip the first PID loop
// This gives the motor enough time to react to the stop command
// Without it, the motor will jitter a little bit after a stop command
// is sent.
if (TargetZeroFlag == 1)
{
AccumulatedError = 0;
EncoderCounts = 0;
TargetZeroFlag = 0;
}
/*************** Calculate stuff for PID **********************************************/
Error = Target - EncoderCounts;
AccumulatedError = AccumulatedError + Error;
DeltaError = Error - PreviousError;
PreviousError = Error;
// Set bounds for the accumulated error ///
if (AccumulatedError > 10000)
AccumulatedError = 10000;
else if (AccumulatedError < -10000)
AccumulatedError = -10000;
// Performing the actual PID
PID = (Error * KP) + (AccumulatedError * KI) + (DeltaError * KD ); // Performing PID calculation
//Note: Even through the variable types are
// different, the result should be the correct
// value of with the type of int
if (Error != 0) // If no error
{
CurrentPwm = PID + PWM_OFFSET;
if (CurrentPwm >= 255) // PWM should be between 0 - 255
CurrentPwm = 255;
else if (CurrentPwm < 0)
CurrentPwm = 0;
setPWM(CurrentPwm); // set new PWM
}
F.T0 = 0; // reset TMR0 flag
} // end PID Loop
} // end while(1)
return 1; // standard ending for an "int main"
} // END MAIN ---------------------------------------------------------------
