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);
}
