void SpecificWorker::setSpeedBase(const float advx, const float advz, const float rotv)
{
computeOdometry(true);
QMutexLocker locker(mutex);
const QVec v = QVec::vec3(advz, advx, rotv);
const QVec wheels = M_vels_2_wheels * v;
setWheels(wheels);
}
int main()
{
//Load and initialized learned odometry model
std::string filepathX = "../Data/Odometry/model_walk_delta_x.bin";
std::string filepathY = "../Data/Odometry/model_walk_delta_y.bin";
std::string filepathTheta = "../Data/Odometry/model_walk_delta_theta.bin";
LWPR_Object* modelX = new LWPR_Object(filepathX.c_str());
LWPR_Object* modelY = new LWPR_Object(filepathY.c_str());
LWPR_Object* modelTheta = new LWPR_Object(filepathTheta.c_str());
//Walk orders
double walkStepX = 0.02;
double walkStepY = 0.01;
double walkStepTheta = 0.1;
//Computed head displacement delta
double deltaX = 0.0;
double deltaY = 0.0;
double deltaTheta = 0.0;
//Integrated robot pose
double poseX = 0.0;
double poseY = 0.0;
double poseTheta = 0.0;
bool isLeftSupportFoot = true;
for (size_t k=0;k<10;k++) {
//Compute corrected displacement delta
//with given last two walk orders and
//current support foot
computeOdometry(
walkStepX, walkStepY, walkStepTheta,
walkStepX, walkStepY, walkStepTheta,
isLeftSupportFoot,
modelX, modelY, modelTheta,
deltaX, deltaY, deltaTheta);
//Integrate delta displacements
odometryIntegration(
deltaX, deltaY, deltaTheta,
poseX, poseY, poseTheta);
std::cout << k << " "
<< deltaX << " " << deltaY << " " << deltaTheta
<< poseX << " " << poseY << " " << poseTheta
<< std::endl;
//Swap support foot
isLeftSupportFoot = !isLeftSupportFoot;
}
//Free LWPR model
delete modelX;
delete modelY;
delete modelTheta;
return 0;
}
void SpecificWorker::compute()
{
setWheels(wheelVels);
computeOdometry(false);
}
void Wifibot::update()
{
roswifibot::Status topicStatus;
// Send speeds only if needed
if (_updated)
_pDriver->setSpeeds(_speedLeft, _speedRight);
_updated = false;
// get data from driver
wifibot::driverData st = _pDriver->readData();
_timeCurrent = ros::Time::now();
// Fill status topic
topicStatus.battery_level = st.voltage;
topicStatus.current = st.current; // see libwifibot to adapt this value
topicStatus.ADC1 = st.adc[0];
topicStatus.ADC2 = st.adc[1];
topicStatus.ADC3 = st.adc[2];
topicStatus.ADC4 = st.adc[3];
topicStatus.speed_front_left = st.speedFrontLeft;
topicStatus.speed_front_right = st.speedFrontRight;
topicStatus.odometry_left = st.odometryLeft;
topicStatus.odometry_right = st.odometryRight;
topicStatus.version = st.version;
bool r1, r2, r3;
_pDriver->getRelays(r1, r2, r3);
topicStatus.relay1 = r1;
topicStatus.relay2 = r2;
topicStatus.relay3 = r3;
// publish status
if (_pubStatus.getNumSubscribers())
_pubStatus.publish(topicStatus);
// compute position
computeOdometry(st.odometryLeft, st.odometryRight);
//TRANSFORM we'll publish the transform over tf
_odomTf.header.stamp = _timeCurrent;
_odomTf.header.frame_id = _frameOdom;
_odomTf.child_frame_id = _frameBase;
_odomTf.transform.translation.x = _position.x;
_odomTf.transform.translation.y = _position.y;
_odomTf.transform.translation.z = 0.0;
_odomTf.transform.rotation =
tf::createQuaternionMsgFromYaw(_position.th);
//send the transform
_odomBroadcast.sendTransform(_odomTf);
//TOPIC, we'll publish the odometry message over ROS
nav_msgs::Odometry odometryTopic;
odometryTopic.header.stamp = _timeCurrent;
odometryTopic.header.frame_id = _frameOdom;
//set the position
odometryTopic.pose.pose.position.x = _position.x;
odometryTopic.pose.pose.position.y = _position.y;
odometryTopic.pose.pose.position.z = 0.0;
odometryTopic.pose.pose.orientation =
tf::createQuaternionMsgFromYaw(_position.th);
//set the velocity
odometryTopic.child_frame_id = _frameBase;
odometryTopic.twist.twist.linear.x = getSpeedLinear(
st.speedFrontLeft, st.speedFrontRight);
odometryTopic.twist.twist.linear.y = 0.0;
odometryTopic.twist.twist.angular.z = getSpeedAngular(
st.speedFrontLeft, st.speedFrontRight);
/*
ROS_INFO("lin:%0.3f ang:%0.3f",
odometryTopic.twist.twist.linear.x,
odometryTopic.twist.twist.angular.z);
*/
//publish the message
if (_pubOdometry.getNumSubscribers())
_pubOdometry.publish(odometryTopic);
// BATTERIES
std_msgs::Float32 batt_voltage;
batt_voltage.data = st.voltage;
if (_batteryMinVoltage > st.voltage) {
_nh.setParam("robot_battery_min_voltage", st.voltage);
_nh.setParam("computer_battery_min_voltage", st.voltage);
_batteryMinVoltage = st.voltage;
}
/* when plugged, battery level ~= 17.5, otherwise 12.5 -> 10
else if (_batteryMaxVoltage < st.voltage) {
_nh.setParam("robot_battery_max_voltage", st.voltage);
_nh.setParam("computer_battery_max_voltage", st.voltage);
_batteryMaxVoltage = st.voltage;
}
* */
if (_pubRobotBatteryVoltage.getNumSubscribers())
_pubRobotBatteryVoltage.publish(batt_voltage);
if (_pubComputerBatteryVoltage.getNumSubscribers())
_pubComputerBatteryVoltage.publish(batt_voltage);
//std_msgs::Bool is_charging_msg;
//is_charging_msg.data = (st.voltage > 15);
//if (_pubIsCharging.getNumSubscribers())
// _pubIsCharging.publish(is_charging_msg);
// Update last time
_timeLast = _timeCurrent;
}
