void LCMFrontEnd::publishHead() { RBIS state; RBIM cov; state_estimator->getHeadState(state, cov); publishState(state, cov); }
// Imu callback function void EkfNode::imuSubCB(const sensor_msgs::Imu& msg){ double dtSys = dtSystem(msg.header.stamp); double z = msg.angular_velocity.z; // If first Run, only initialize lastSystTime_ if (!firstRunSys_) { // Update system and IMU measurement models for ekf_map_ ekf_map_.systemUpdate(dtSys); ekf_map_.measurementUpdateIMU(z); // Update system and IMU measurement models for ekf_odom_ ekf_odom_.systemUpdate(dtSys); ekf_odom_.measurementUpdateIMU(z); // and publish odom message and update transform tree publishState(msg.header.stamp); } firstRunSys_ = false; }
void LCMFrontEnd::smooth(double dt) { if (state_estimator == NULL) { fprintf(stderr, "Error: can't smooth with LCMFrontEnd before setStateEstimator\n"); exit(1); } fprintf(stderr, "performing smoothing forward pass\n"); int64_t override_val = numeric_limits<int64_t>::max(); fprintf(stderr, "overriding utime history span from %jd to %jd", state_estimator->utime_history_span, override_val); state_estimator->utime_history_span = override_val; fprintf(stderr, "turning off pose and filter state publishing going forwards\n"); bool set_publish_filter_state = publish_filter_state; publish_filter_state = false; bool set_publish_pose = publish_pose; publish_pose = false; while (true) { int ret = lcm_recv->handle(); if (ret != 0) { printf("log is done\n"); break; } } publish_filter_state = set_publish_filter_state; publish_pose = set_publish_pose; fprintf(stderr, "performing smoothing backwards pass\n"); state_estimator->EKFSmoothBackwardsPass(dt); updateHistory::historyMapIterator current_it; fprintf(stderr, "republishing smoothed pose messages\n"); for (current_it = state_estimator->history.updateMap.begin(); current_it != state_estimator->history.updateMap.end(); current_it++) { RBISUpdateInterface * current_update = current_it->second; if (current_update->sensor_id == RBISUpdateInterface::ins) { RBIS state = current_update->posterior_state; RBIM cov = current_update->posterior_covariance; publishState(state, cov); } } }
// DecaWave callback function void EkfNode::dwSubCB(const snowmower_msgs::DecaWaveMsg& msg){ double dtSys = dtSystem(msg.header.stamp); ROS_INFO_STREAM("dtSys = " << dtSys); Vector4d z; z << msg.dist[0], msg.dist[1], msg.dist[2],msg.dist[3]; ROS_INFO_STREAM("dw = \n" << z); // If first Run, only initialize lastSystTime_ if (!firstRunSys_) { // Update system and decawave measurement models for ekf_map_ ekf_map_.systemUpdate(dtSys); ekf_map_.measurementUpdateDecaWave(z); // Update only the system model for ekf_odom_ (decawave is not used in odom) ekf_odom_.systemUpdate(dtSys); // and publish odom message and update transform tree publishState(msg.header.stamp); } firstRunSys_ = false; }
// --------------------------------------------------------------------------- // CSsmStatePolicyBase::PrepareCommandList // --------------------------------------------------------------------------- // EXPORT_C void CSsmStatePolicyBase::PrepareCommandList( TSsmState aState, #ifdef INFO_TRACE TInt aReason, #else TInt /*aReason*/, #endif TRequestStatus& aStatus ) { FUNC_LOG; INFO_3( "State %04x.%04x, reason %04x", aState.MainState(), aState.SubState(), aReason ); TSsmState publishState( aState.MainState(), TargetSubState( aState.SubState() ) ); // Build the commandlist from resource. // Substate id is used as a command list id. iCommandListResourceReader->PrepareCommandList( publishState.SubState(), publishState, aStatus ); }
int main(int argc, char* argv[]) { int ret = libusb_init(0); if (ret) { ROS_ERROR_STREAM("Cannot initialize libusb, error: " << ret); return 1; } ros::init(argc, argv, "kinect_aux_node"); ros::NodeHandle n; int deviceIndex; n.param<int>("device_index", deviceIndex, 0); openAuxDevice(deviceIndex); if (!dev) { ROS_ERROR_STREAM("No valid aux device found"); libusb_exit(0); return 2; } pub_imu = n.advertise<sensor_msgs::Imu>("imu", 15); pub_tilt_angle = n.advertise<std_msgs::Float64>("cur_tilt_angle", 15); pub_tilt_status = n.advertise<std_msgs::UInt8>("cur_tilt_status", 15); sub_tilt_angle = n.subscribe("tilt_angle", 1, setTiltAngle); sub_led_option = n.subscribe("led_option", 1, setLedOption); while (ros::ok()) { ros::spinOnce(); publishState(); } libusb_exit(0); return 0; }
// Wheel Encoder callback function void EkfNode::encSubCB(const snowmower_msgs::EncMsg& msg){ double dtSys = dtSystem(msg.header.stamp); double dtEnc = dtEncoder(msg.header.stamp); ROS_INFO_STREAM("dtSys = " << dtSys); ROS_INFO_STREAM("dtEnc = " << dtEnc); Vector2i z; z << msg.right, msg.left; // If first Run, only initialize zPre_, lastSysTime_, and lastEncTime_. if (!firstRunEnc_) { // Update system and encoder measurement models for ekf_map_ ekf_map_.systemUpdate(dtSys); ekf_map_.measurementUpdateEncoders(z, zPre_, dtEnc); // Update system and encoder measurement models for ekf_odom_ ekf_odom_.systemUpdate(dtSys); ekf_odom_.measurementUpdateEncoders(z, zPre_, dtEnc); // and publish odom message and update transform tree publishState(msg.header.stamp); } // Store current measurement as zPre_ zPre_ = z; firstRunEnc_ = false; firstRunSys_ = false; }
void Footcoords::filter(const geometry_msgs::WrenchStamped::ConstPtr& lfoot, const geometry_msgs::WrenchStamped::ConstPtr& rfoot) { bool success_to_update = false; // lowpass filter double lfoot_force = applyLowPassFilter(lfoot->wrench.force.z, prev_lforce_); double rfoot_force = applyLowPassFilter(rfoot->wrench.force.z, prev_rforce_); lforce_list_.push_back(ValueStamped::Ptr(new ValueStamped(lfoot->header, lfoot_force))); rforce_list_.push_back(ValueStamped::Ptr(new ValueStamped(rfoot->header, rfoot_force))); // ROS_INFO("lforce_list: %lu", lforce_list_.size()); // ROS_INFO("rforce_list: %lu", rforce_list_.size()); // ROS_INFO("%f -> %f", lfoot->wrench.force.z, lfoot_force); // ROS_INFO("%f -> %f", rfoot->wrench.force.z, rfoot_force); // update prev value prev_lforce_ = lfoot_force; prev_rforce_ = rfoot_force; if (allValueLargerThan(lforce_list_, force_thr_) && allValueLargerThan(rforce_list_, force_thr_)) { // on ground support_status_ = BOTH_GROUND; publishState("ground"); success_to_update = computeMidCoords(lfoot->header.stamp); updateGroundTF(); } else if (allValueSmallerThan(lforce_list_, force_thr_) && allValueSmallerThan(rforce_list_, force_thr_)) { before_on_the_air_ = true; support_status_ = AIR; publishState("air"); success_to_update = computeMidCoords(lfoot->header.stamp); // do not update odom_on_ground } else if (allValueLargerThan(lforce_list_, force_thr_)) { // only left support_status_ = LLEG_GROUND; publishState("lfoot"); success_to_update = computeMidCoordsFromSingleLeg(lfoot->header.stamp, true); updateGroundTF(); } else if (allValueLargerThan(rforce_list_, force_thr_)) { // only right support_status_ = RLEG_GROUND; publishState("rfoot"); success_to_update = computeMidCoordsFromSingleLeg(lfoot->header.stamp, false); updateGroundTF(); } else { // unstable support_status_ = UNSTABLE; publishState("unstable"); } if (success_to_update) { publishTF(lfoot->header.stamp); diagnostic_updater_->update(); publishContactState(lfoot->header.stamp); } lforce_list_.removeBefore(lfoot->header.stamp - ros::Duration(sampling_time_)); rforce_list_.removeBefore(rfoot->header.stamp - ros::Duration(sampling_time_)); }
/*! * \brief Executes the callback from the command_vel topic. * * Set the current velocity target. * \param msg JointVelocities */ void topicCallback_CommandVel(const brics_actuator::JointVelocities::ConstPtr& msg) { ROS_DEBUG("Received new velocity command"); if (!initialized_) { ROS_WARN("Skipping command: powercubes not initialized"); publishState(false); return; } if (pc_ctrl_->getPC_Status() != PowerCubeCtrl::PC_CTRL_OK) { publishState(false); return; } PowerCubeCtrl::PC_CTRL_STATUS status; std::vector<std::string> errorMessages; pc_ctrl_->getStatus(status, errorMessages); /// ToDo: don't rely on position of joint names, but merge them (check between msg.joint_uri and member variable JointStates) unsigned int DOF = pc_params_->GetDOF(); std::vector<std::string> jointNames = pc_params_->GetJointNames(); std::vector<double> cmd_vel(DOF); std::string unit = "rad"; /// check dimensions if (msg->velocities.size() != DOF) { ROS_ERROR("Skipping command: Commanded velocities and DOF are not same dimension."); return; } /// parse velocities for (unsigned int i = 0; i < DOF; i++) { /// check joint name if (msg->velocities[i].joint_uri != jointNames[i]) { ROS_ERROR("Skipping command: Received joint name %s doesn't match expected joint name %s for joint %d.",msg->velocities[i].joint_uri.c_str(),jointNames[i].c_str(),i); return; } /// check unit if (msg->velocities[i].unit != unit) { ROS_ERROR("Skipping command: Received unit %s doesn't match expected unit %s.",msg->velocities[i].unit.c_str(),unit.c_str()); return; } /// if all checks are successful, parse the velocity value for this joint ROS_DEBUG("Parsing velocity %f for joint %s",msg->velocities[i].value,jointNames[i].c_str()); cmd_vel[i] = msg->velocities[i].value; } /// command velocities to powercubes if (!pc_ctrl_->MoveVel(cmd_vel)) { error_ = true; error_msg_ = pc_ctrl_->getErrorMessage(); ROS_ERROR("Skipping command: %s",pc_ctrl_->getErrorMessage().c_str()); return; } ROS_DEBUG("Executed velocity command"); publishState(false); }
// Initialization process void EkfNode::init() { /************************************************************************** * Initialize firstRun, time, and frame names **************************************************************************/ // Set first run to true for encoders. Once a message is received, this will // be set to false. firstRunEnc_ = true; firstRunSys_ = true; // Set Times ros::Time currTime = ros::Time::now(); lastEncTime_ = currTime; lastSysTime_ = currTime; // Use the ROS parameter server to initilize parameters if(!private_nh_.getParam("base_frame", base_frame_)) base_frame_ = "base_link"; if(!private_nh_.getParam("odom_frame", base_frame_)) odom_frame_ = "odom"; if(!private_nh_.getParam("map_frame", map_frame_)) map_frame_ = "map"; /************************************************************************** * Initialize state for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize state double state_temp[] = {0, 0, 0, 0, 0, 0}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> state (state_temp, state_temp + sizeof(state_temp) / sizeof(double)); // Check the parameter server and initialize state if(!private_nh_.getParam("state", state)) { ROS_WARN_STREAM("No state found. Using default."); } // Check to see if the size is not equal to 6 if (state.size() != 6) { ROS_WARN_STREAM("state isn't 6 elements long!"); } // And initialize the Matrix typedef Matrix<double, 6, 1> Vector6d; Vector6d stateMat(state.data()); std::cout << "state_map = " << std::endl; std::cout << stateMat << std::endl; ekf_map_.initState(stateMat); // Odom is always initialized at all zeros. stateMat << 0, 0, 0, 0, 0, 0; ekf_odom_.initState(stateMat); std::cout << "state_odom = " << std::endl; std::cout << stateMat << std::endl; /************************************************************************** * Initialize covariance for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize covariance double cov_temp[] = {0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> cov (cov_temp, cov_temp + sizeof(cov_temp) / sizeof(double)); // Check the parameter server and initialize cov if(!private_nh_.getParam("covariance", cov)) { ROS_WARN_STREAM("No covariance found. Using default."); } // Check to see if the size is not equal to 36 if (cov.size() != 36) { ROS_WARN_STREAM("cov isn't 36 elements long!"); } // And initialize the Matrix typedef Matrix<double, 6, 6, RowMajor> Matrix66; Matrix66 covMat(cov.data()); std::cout << "covariance = " << std::endl; std::cout << covMat << std::endl; ekf_map_.initCov(covMat); // Initialize odom covariance the same as the map covariance (this isn't // correct. But, since it is all an estimate anyway, it should be fine. ekf_odom_.initCov(covMat); /************************************************************************** * Initialize Q for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize Q double Q_temp[] = {0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> Q (Q_temp, Q_temp + sizeof(Q_temp) / sizeof(double)); // Check the parameter server and initialize Q if(!private_nh_.getParam("Q", Q)) { ROS_WARN_STREAM("No Q found. Using default."); } // Check to see if the size is not equal to 36 if (Q.size() != 36) { ROS_WARN_STREAM("Q isn't 36 elements long!"); } // And initialize the Matrix Matrix66 QMat(Q.data()); std::cout << "Q = " << std::endl; std::cout << QMat << std::endl; ekf_map_.initSystem(QMat); ekf_odom_.initSystem(QMat); /************************************************************************** * Initialize Decawave for ekf_map_ (not used in ekf_odom_) **************************************************************************/ // Create temp array to initialize R for DecaWave double RDW_temp[] = {0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> RDW (RDW_temp, RDW_temp + sizeof(RDW_temp) / sizeof(double)); // Check the parameter server and initialize RDW if(!private_nh_.getParam("DW_R", RDW)) { ROS_WARN_STREAM("No DW_R found. Using default."); } // Check to see if the size is not equal to 16 if (RDW.size() != 16) { ROS_WARN_STREAM("DW_R isn't 16 elements long!"); } // And initialize the Matrix typedef Matrix<double, 4, 4, RowMajor> Matrix44; Matrix44 RDWMat(RDW.data()); std::cout << "RDecaWave = " << std::endl; std::cout << RDWMat << std::endl; // Create temp array to initialize beacon locations for DecaWave double DWBL_temp[] = {0, 0, 5, 0, 5, 15, 0, 15}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> DWBL (DWBL_temp, DWBL_temp + sizeof(DWBL_temp) / sizeof(double)); // Check the parameter server and initialize DWBL if(!private_nh_.getParam("DW_Beacon_Loc", DWBL)) { ROS_WARN_STREAM("No DW_Beacon_Loc found. Using default."); } // Check to see if the size is not equal to 8 if (DWBL.size() != 8) { ROS_WARN_STREAM("DW_Beacon_Loc isn't 8 elements long!"); } // And initialize the Matrix typedef Matrix<double, 4, 2, RowMajor> Matrix42; Matrix42 DWBLMat(DWBL.data()); std::cout << "DW_Beacon_Loc = " << std::endl; std::cout << DWBLMat << std::endl; MatrixXd DecaWaveBeaconLoc(4,2); MatrixXd DecaWaveOffset(2,1); double DW_offset_x; double DW_offset_y; if(!private_nh_.getParam("DW_offset_x", DW_offset_x)) DW_offset_x = 0.0; if(!private_nh_.getParam("DW_offset_y", DW_offset_y)) DW_offset_y = 0.0; DecaWaveOffset << DW_offset_x, DW_offset_y; std::cout << "DecaWaveOffset = " << std::endl; std::cout << DecaWaveOffset << std::endl; ekf_map_.initDecaWave(RDWMat, DWBLMat, DecaWaveOffset); // Decawave is not used in odom, so no need to initialize /************************************************************************** * Initialize Encoders for ekf_odom_ and ekf_map_ **************************************************************************/ // Create temp array to initialize R for DecaWave double REnc_temp[] = {0.01, 0, 0, 0.01}; // And a std::vector, which will be used to initialize an Eigen Matrix std::vector<double> REnc (REnc_temp, REnc_temp + sizeof(REnc_temp) / sizeof(double)); // Check the parameter server and initialize RDW if(!private_nh_.getParam("Enc_R", REnc)) { ROS_WARN_STREAM("No Enc_R found. Using default."); } // Check to see if the size is not equal to 4 if (REnc.size() != 4) { ROS_WARN_STREAM("Enc_R isn't 4 elements long!"); } // And initialize the Matrix typedef Matrix<double, 2, 2, RowMajor> Matrix22; Matrix22 REncMat(REnc.data()); std::cout << "R_Enc = " << std::endl; std::cout << REncMat << std::endl; double b, tpmRight, tpmLeft; if(!private_nh_.getParam("track_width", b)) b = 1; if(!private_nh_.getParam("tpm_right", tpmRight)) tpmRight = 1; if(!private_nh_.getParam("tpm_left", tpmLeft)) tpmLeft = 1; ekf_map_.initEnc(REncMat, b, tpmRight, tpmLeft); ekf_odom_.initEnc(REncMat, b, tpmRight, tpmLeft); std::cout << "track_width = " << b << std::endl; std::cout << "tpm_right = " << tpmRight << std::endl; std::cout << "tpm_left = " << tpmLeft << std::endl; /************************************************************************** * Initialize IMU for ekf_odom_ and ekf_map_ **************************************************************************/ double RIMU; if(!private_nh_.getParam("R_IMU", RIMU)) RIMU = 0.01; ekf_map_.initIMU(RIMU); ekf_odom_.initIMU(RIMU); std::cout << "R_IMU = " << RIMU << std::endl; // Publish the initialized state and exit initialization. publishState(currTime); ROS_INFO_STREAM("Finished with initialization."); }
/** * Updates path logic. */ void PathHandler::updatePath() { geometry_msgs::Twist command; publishState(); // no path? nothing to handle if (getPathSize() == 0) return; // update our position if possible. if (updateCurrentPosition() == false) { ROS_ERROR("Failed to update robot position."); return; } if (mCurrentPathIndex < getPathSize() - 1) // we are moving along a line segment in the path { geometry_msgs::Point robotPos; robotPos.x = mRobotPosition.getOrigin().x(); robotPos.y = mRobotPosition.getOrigin().y(); geometry_msgs::Point closestOnPath = closestPointOnLine(robotPos, mPath[mCurrentPathIndex].position, mPath[mCurrentPathIndex + 1].position); //ROS_INFO("robotPos[%lf, %lf], closestOnPath[%lf, %lf]", robotPos.x, robotPos.y, closestOnPath.x, closestOnPath.y); if (distanceBetweenPoints(closestOnPath, robotPos) > mResetDistanceTolerance) { ROS_INFO("Drove too far away from path, re-sending goal."); mFollowState = FOLLOW_STATE_IDLE; resendGoal(); clearPath(); return; } geometry_msgs::Point pointOnPath = getPointOnPathWithDist(closestOnPath, mLookForwardDistance); //ROS_INFO("closestOnPath[%lf, %lf], pointOnPath[%lf, %lf]", closestOnPath.x, closestOnPath.y, pointOnPath.x, pointOnPath.y); double angle = angleTo(pointOnPath); /*double robotYaw = tf::getYaw(mRobotPosition.getRotation()); // only used for printing ROS_INFO("Follow state: %d Robot pos: (%lf, %lf, %lf). Target pos: (%lf, %lf, %lf). RobotYaw: %lf. FocusYaw: %lf", mFollowState, mRobotPosition.getOrigin().getX(), mRobotPosition.getOrigin().getY(), mRobotPosition.getOrigin().getZ(), pointOnPath.x, pointOnPath.y, pointOnPath.z, robotYaw, angle);*/ //ROS_INFO("Angle to path: %f", angle); command.linear.x = getScaledLinearSpeed(angle); command.angular.z = getScaledAngularSpeed(angle); if (distanceBetweenPoints(closestOnPath, mPath[mCurrentPathIndex + 1].position) < mDistanceTolerance) { //ROS_INFO("Moving to next line segment on path."); mCurrentPathIndex++; } publishLocalPath(command.linear.x * 3, angle); } else // only rotate for final yaw { double yaw = tf::getYaw(mPath[getPathSize() - 1].orientation); btVector3 orientation = btVector3(cos(yaw), sin(yaw), 0.0).rotate(btVector3(0,0,1), -tf::getYaw(mRobotPosition.getRotation())); double angle = atan2(orientation.getY(), orientation.getX()); //ROS_INFO("Angle to final orientation: %f", angle); if (fabs(angle) > mFinalYawTolerance) command.angular.z = getScaledAngularSpeed(angle, true); else { // path is done! mFollowState = FOLLOW_STATE_FINISHED; clearPath(); } publishLocalPath(1.0, angle); } mCommandPub.publish(command); }