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