void CrowdToolState::setMoveTarget(const float* p, bool adjust)
{
if (!m_sample) return;
// Find nearest point on navmesh and set move request to that location.
dtNavMeshQuery* navquery = m_sample->getNavMeshQuery();
dtCrowd* crowd = m_sample->getCrowd();
const dtQueryFilter* filter = crowd->getFilter();
const float* ext = crowd->getQueryExtents();
if (adjust)
{
float vel[3];
// Request velocity
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag && ag->active)
{
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(m_agentDebug.idx, vel);
}
}
else
{
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(i, vel);
}
}
}
else
{
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
if (m_agentDebug.idx != -1)
{
const dtCrowdAgent* ag = crowd->getAgent(m_agentDebug.idx);
if (ag && ag->active)
crowd->requestMoveTarget(m_agentDebug.idx, m_targetRef, m_targetPos);
}
else
{
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
}
}
}
}
void OgreDetourCrowd::setMoveTarget(Ogre::Vector3 position, bool adjust)
{
// Find nearest point on navmesh and set move request to that location.
dtNavMeshQuery* navquery = m_recast->m_navQuery;
dtCrowd* crowd = m_crowd;
const dtQueryFilter* filter = crowd->getFilter();
const float* ext = crowd->getQueryExtents();
float p[3];
OgreRecast::OgreVect3ToFloatA(position, p);
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
// Adjust target using tiny local search. (instead of recalculating full path)
if (adjust)
{
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
float vel[3];
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(i, vel);
}
}
else
{
// Move target using path finder (recalculate a full new path)
for (int i = 0; i < crowd->getAgentCount(); ++i)
{
const dtCrowdAgent* ag = crowd->getAgent(i);
if (!ag->active) continue;
crowd->requestMoveTarget(i, m_targetRef, m_targetPos);
}
}
}
Ogre::Vector3 OgreDetourCrowd::calcVel(Ogre::Vector3 position, Ogre::Vector3 target, Ogre::Real speed)
{
float pos[3];
OgreRecast::OgreVect3ToFloatA(position, pos);
float tgt[3];
OgreRecast::OgreVect3ToFloatA(target, tgt);
float res[3];
calcVel(res, pos, tgt, speed);
Ogre::Vector3 result;
OgreRecast::FloatAToOgreVect3(res, result);
return result;
}
void OgreDetourCrowd::setMoveTarget(int agentId, Ogre::Vector3 position, bool adjust)
{
// TODO extract common method
// Find nearest point on navmesh and set move request to that location.
dtNavMeshQuery* navquery = m_recast->m_navQuery;
dtCrowd* crowd = m_crowd;
const dtQueryFilter* filter = crowd->getFilter();
const float* ext = crowd->getQueryExtents();
float p[3];
OgreRecast::OgreVect3ToFloatA(position, p);
navquery->findNearestPoly(p, ext, filter, &m_targetRef, m_targetPos);
// ----
if (adjust) {
const dtCrowdAgent *ag = getAgent(agentId);
float vel[3];
calcVel(vel, ag->npos, p, ag->params.maxSpeed);
crowd->requestMoveVelocity(agentId, vel);
} else {
m_crowd->requestMoveTarget(agentId, m_targetRef, m_targetPos);
}
}
void update(monitor_odometry_target_data &data, monitor_odometry_target_config config)
{
/* protected region user update on begin */
//Abstand des neue KS vom Roboter (bei Nullgeschwindigkeit).
// config.kindOf_primary_filter = 1;
// config.kindOf_secondary_filter = 5;
// config.kindOf_target_alignment = 0;
// config.noise_threshold = 0.02;
// config.mean_time_threshold = 2;
// config.vel_time_threshold = 2;
// config.vel_threshold = 0.05;
// config.kindOf_stagnancy_override = 0;
// config.mean_vel_threshold = 0.01;
// config.inner_radius = 1.0;
// config.outer_radius = 2.0;
// config.max_vel = 0.30;
// config.min_vel = 0.1;
double output_z = 0;
double radius_of_robot = 1;
ROS_INFO("********************* stage I ***********************");
/*
* stage: (I)
* input odometry data:
* velocity in x;
* velocity in y;
* header:
* timestamp stamp;
* frame_id;
*
*/
x_ = data.in_odometry.twist.twist.linear.x;
y_ = data.in_odometry.twist.twist.linear.y;
header.frame_id = config.base_frame_id;
header.stamp = data.in_odometry.header.stamp;
/*
* info:
*/
//TODO: delete this:
ROS_INFO("input:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
//end delete this;
// std::fstream f;
// f.open("testlog_x.dat", std::ios::out | std::ios::app);
// f << x_ << std::endl;
// f.close();
//
// f.open("testlog_y.dat", std::ios::out | std::ios::app);
// f << y_ << std::endl;
// f.close();
//TODO:
ROS_INFO("not good for performace but do anyway!");
velocity_ = calcAbsVec(x_, y_);
//TODO: just for now:
if (velocity_ > config.max_vel) {
ROS_INFO("ERROR vel > max_vel");
}
// velocity_ = calcVel(x_, y_, config.max_vel);
velList = updateVelList(velList, velocity_, header.stamp.toSec(), config.vel_time_threshold);
double meanVel = calcMeanVel(velList);
ROS_INFO("********************* stage II **********************");
/*
* stage: (II)
* primary filters:
*/
if (config.kindOf_primary_filter == 0) {
ROS_INFO("no primary filter active ...");
// velocity_ = calcVel(x_, y_, config.max_vel);
}
else if (config.kindOf_primary_filter == 1) {
ROS_INFO("noise suppression filter active ...");
x_ = noiseSuppression(x_ , config.noise_threshold);
y_ = noiseSuppression(y_ , config.noise_threshold);
// velocity_ = calcVel(x_,y_, config.max_vel);
}
else if (config.kindOf_primary_filter == 2) {
ROS_INFO("velocity based suppression filter active ...");
// velocity_ = calcVel(x_,y_, config.max_vel);
if (velocity_ < config.vel_threshold) {
x_ = 0.0;
y_ = 0.0;
velocity_ = 0.0;
}
}
else if (config.kindOf_primary_filter == 3) {
ROS_INFO("combined suppression filter active ...");
x_ = noiseSuppression(x_ , config.noise_threshold);
y_ = noiseSuppression(y_ , config.noise_threshold);
velocity_ = calcVel(x_,y_, config.max_vel);
if (velocity_ < config.vel_threshold) {
x_ = 0.0;
y_ = 0.0;
velocity_ = 0.0;
//TODO: Ist das nicht gefährlich??? Der Roboter könnte sich bewegen aber die Durchschnittsgeschw.
// über 2-3 Sekunden trotzdem immer null sein???
}
}
else
ROS_WARN("Error - 0001 - primary filter parameter out of bounds!");
ROS_INFO("after primary filters:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
//save velocity data
// std::fstream f;
// f.open("testlog_velocity.dat", std::ios::out | std::ios::app);
// f << velocity_ << std::endl;
// f.close();
ROS_INFO("********************* stage III *********************");
/*
* stage: (III)
* target alignment:
*/
//TODO:
if (config.kindOf_target_alignment == 0) {
ROS_INFO("no target alignment active ...");
z_ = (double) hight;
}
else {
//for all cases:
int overrideZ = checkForOverrideZ(config.min_vel, config.max_vel, velocity_);
if (config.kindOf_target_alignment == 1) {
ROS_INFO("vertical target alignment active ...");
z_ = (velocity_ / config.max_vel) * (double) hight;
}
else if (config.kindOf_target_alignment == 2) {
ROS_INFO("linear target alignment active ...");
z_ = (velocity_ / config.max_vel) * (double) hight;
d_ = z_ * ((config.outer_radius - config.inner_radius) / (double) hight);
}
else if (config.kindOf_target_alignment == 3) {
ROS_INFO("elliptical target aligment active ...");
z_ = -1 * sqrt( 1 - (((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((velocity_ / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
d_ = (velocity_ / config.max_vel) * (config.outer_radius - config.inner_radius);
}
else if (config.kindOf_target_alignment == 4) {
ROS_INFO("elliptical target alignment with mean velocity active...");
z_ = -1 * sqrt( 1 - (((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)) * ((meanVel / config.max_vel) / (config.outer_radius - config.inner_radius)))) * (double) hight + hight;
d_ = (meanVel / config.max_vel) * (config.outer_radius - config.inner_radius);
}
else {
ROS_WARN("Error - 0002 - target alignment parameter out of bounds!");
}
ROS_INFO("OVERRIDE BEFORE IF_ = %u",overrideZ);
if (overrideZ == 0) {
z_ = 0.0;
ROS_INFO("OVI 0");
}
else if (overrideZ == 1) {
z_ = (double) hight;
ROS_INFO("OVI 1=hight");
}
else if (overrideZ == 3) {
ROS_ERROR("Error - 0003 - override parameter out of bounds!");
}
else {
ROS_INFO("OVI wahrscheinlich 2 also nix tun!");
}
}
/*
* stage: (IV)
* secondary filters:
*/
ROS_INFO("********************* stage IV **********************");
if (config.kindOf_secondary_filter == 0) {
ROS_INFO("no secondary filter active ...");
}
else if (config.kindOf_secondary_filter == 1) {
ROS_INFO("mean filter active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanXY = calcMeanXY(list_of_values_xystamp_);
x_ = meanXY.at(0);
y_ = meanXY.at(1);
// ROS_INFO("Anzahl Elemente: %lu", list_of_values_xystamp_.size());
// ROS_INFO("meanX %f und meanY %f", meanXY.at(0), meanXY.at(1));
}
else if (config.kindOf_secondary_filter == 2) {
ROS_INFO("median filter active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> medianXY = calcMedianXY(list_of_values_xystamp_);
x_ = medianXY.at(0);
y_ = medianXY.at(1);
}
else if (config.kindOf_secondary_filter == 3) {
ROS_INFO("quantified mean filter slope active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanSlope = calcQuantifiedMeanSlope(list_of_values_xystamp_);
x_ = meanSlope.at(0);
y_ = meanSlope.at(1);
}
else if (config.kindOf_secondary_filter == 4) {
ROS_INFO("quantified mean filter stairs active ...");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanStairs = calcQuantifiedMeanStairs(list_of_values_xystamp_);
x_ = meanStairs.at(0);
y_ = meanStairs.at(1);
}
else if (config.kindOf_secondary_filter == 5) {
ROS_INFO("quantified mean filter square active");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> meanSquare = calcQuantifiedMeanSquare(list_of_values_xystamp_);
x_ = meanSquare.at(0);
y_ = meanSquare.at(1);
}
else if (config.kindOf_secondary_filter == 6) {
ROS_INFO("quantified median filter slope active");
list_of_values_xystamp_ = updateList(list_of_values_xystamp_, x_, y_, header.stamp.toSec(), config.mean_time_threshold);
std::vector<double> medianSlope = calcQuantifiedMedianSlope(list_of_values_xystamp_);
x_ = medianSlope.at(0);
y_ = medianSlope.at(1);
}
else
ROS_WARN("Error - 0003 - secondary filter parameter out of bounds!");
ROS_INFO("after secondary filters:");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
/*
* stage: (V)
* output production:
*/
// yaw_ = noZeroArcTan(y_,x_);
//for stag test:
//listOfValuesForMeanYaw_ = updateListOfDouble(listOfValuesForMeanYaw_, yaw_, 100);
//end of stag test
// yaw_ = noZeroArcTan(y_,x_);
/*
* stage: (V)
* stagnancy override:
*/
ROS_INFO("********************* stage V ***********************");
if (config.kindOf_stagnancy_override == 0) {
ROS_INFO("no stagnancyoverride active ...");
}
else if (config.kindOf_stagnancy_override == 1) {
ROS_INFO("stagnancy override active ...");
// velList = updateVelList(velList, velocity_, header.stamp.toSec());
// double meanVel = calcMeanVel(velList);
if (meanVel < config.mean_vel_threshold) {
stagnancy_override_counter++;
z_ = (double) hight;
d_ = 0.0;
//wieder zurück drehen???
}
}
else if (config.kindOf_stagnancy_override == 2) {
ROS_INFO("stagnancy override: twist back torso");
if (meanVel < config.mean_vel_threshold) {
// yaw_ = calcMean(listOfValuesForMeanYaw_);
// ROS_INFO("Mean yaw_ %f",yaw_);
}
}
else
ROS_ERROR("Error - 0004 - stagnancy override parameter out of bounds!");
/*
* stage: (VI)
* output calculator:
*/
ROS_INFO("********************* stage VI **********************");
yaw_ = newArcTan(x_,y_);
// ROS_INFO("d_ = %f",d_);
x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
// x_ = calcX(yaw_, (config.outer_radius - config.inner_radius) + d_);
// y_ = calcY(yaw_, (config.outer_radius - config.inner_radius) + d_);
ROS_INFO("Output: ---------------------");
ROS_INFO("x_ = %f",x_);
ROS_INFO("y_ = %f",y_);
ROS_INFO("yaw_ = %f",yaw_);
ROS_INFO("END ....................");
ROS_INFO("Schwellwert: %f", config.noise_threshold);
if (config.testbool) {
ROS_INFO("testbool is true");
}
else {
ROS_INFO("testbool is false");
}
/*
* stage: (VII)
* tf
*/
ROS_INFO("********************* stage VII *********************");
frame_transform_output(tf::Vector3(x_,y_,z_));
stamped_transform_output.stamp_ = header.stamp;
stamped_transform_output.frame_id_ = config.base_frame_id;
stamped_transform_output.child_frame_id_ = config.lookat_frame_id;
tf::Quaternion q;
q.setRPY(0, 0, yaw_);
stamped_transform_output.setRotation(q);
stamped_transform_output.setOrigin(tf::Vector3(x_,y_,z_));
static tf::TransformBroadcaster br;
br.sendTransform(stamped_transform_output);
// /*
// * evaluations:
// */
// if (active_) {
// tf::Vector3 newOrigin;
// newOrigin.setX(x_);
// newOrigin.setY(y_);
// newOrigin.setZ(z_);
// double distance = calcEuklidianDistance(newOrigin,oldOrigin);
// quickAndDirty++;
// //Problem: Sprung von Null auf den Startwert wieder löschen!
// if (quickAndDirty > 4 ) {
// distances_list.push_back(distance);
// }
//
// double max_value = calcMax(distances_list);
// double sum_value = calcSum(distances_list);
// ROS_INFO("+++++++++++++++++++++++++++++++++++++++");
// ROS_INFO("Auswertung: ");/
// ROS_INFO("Distance = %f",distance);
// ROS_INFO("MAX = %f ", max_value);
// ROS_INFO("SUM = %f", sum_value);
// // showList(distances_list);
// oldOrigin = newOrigin;
//
// //TODO:
// //eval: yaw:
// double yawDistance = calcYawDistance(yaw_, oldYaw);
// if (quickAndDirty > 4) {
// yaw_distances_list.push_back(yawDistance);
// }
// double yaw_max_value = calcMax(yaw_distances_list);
// double yaw_sum_value = calcSum(yaw_distances_list);
// ROS_INFO("++++++++++++++++++++++++++++++++++++++++");
// ROS_INFO("YawDistance = %f", yawDistance);
// ROS_INFO("MAX YAW = %f", yaw_max_value);
// ROS_INFO("SUM YAW = %f ", yaw_sum_value);
// oldYaw = yaw_;
//
// ROS_INFO("stagnacy_override_counter = %u", stagnancy_override_counter);
// }
/* protected region user update end */
}
/**
*@brief 更新
* @param st 刻み幅
*/
void RobotArm::update(double st)
{
if(pauseFalg || stopFalg || !serbo)
return;
//std::cout << targetPoint.end_time << "\t" << targetPoint.time << std::endl;
if(targetPoint.end_time < targetPoint.time)
{
if(targetPoint.type == Point)
{
Vector3d pos = calcKinematics(theta);
double dPx = Kp*(targetPoint.target_pos(0)-pos(0));
double dPy = Kp*(targetPoint.target_pos(1)-pos(1));
double dPz = Kp*(targetPoint.target_pos(2)-pos(2));
Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));
updatePos(dthe(0), dthe(1), dthe(2), 0);
theta[3] = targetPoint.target_theta;
}
else
{
for(int i=0;i < 4;i++)
{
theta[i] = targetPoint.target_joint_pos[i];
}
if (!judgeSoftLimitJoint(theta))stop();
}
setTargetPos();
return;
}
else
{
if(targetPoint.type == Point)
{
double td = calcVel(targetPoint.target_theta, targetPoint.start_theta, targetPoint.end_time, targetPoint.time, theta[3]);
dt = st;
MinTime = dt;
double dx = targetPoint.target_pos(0)-targetPoint.start_pos(0);
double dy = targetPoint.target_pos(1)-targetPoint.start_pos(1);
double dz = targetPoint.target_pos(2)-targetPoint.start_pos(2);
double ST = sqrt(dx*dx+dy*dy+dz*dz);
if(ST < 0.001)
{
updatePos(0, 0, 0, td);
targetPoint.time += dt;
return;
}
double A = 2*M_PI*ST/(targetPoint.end_time*targetPoint.end_time);
double s = A*targetPoint.end_time/(2*M_PI)*(targetPoint.time - targetPoint.end_time/(2*M_PI)*sin(2*M_PI/targetPoint.end_time*targetPoint.time));
double Px = s*dx/ST + targetPoint.start_pos(0);
double Py = s*dy/ST + targetPoint.start_pos(1);
double Pz = s*dz/ST + targetPoint.start_pos(2);
double ds = A*targetPoint.end_time/(2*M_PI)*(1 - cos(2*M_PI/targetPoint.end_time*targetPoint.time));
Vector3d pos = calcKinematics(theta);
double dPx = ds*dx/ST + Kp*(Px-pos(0));
double dPy = ds*dy/ST + Kp*(Py-pos(1));
double dPz = ds*dz/ST + Kp*(Pz-pos(2));
//ofs << ds << "\t" << s << std::endl;
//std::cout << pos << std::endl;
Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz));
updatePos(dthe(0), dthe(1), dthe(2), td);
}
else
{
double dthe[4];
for(int i=0;i < 4;i++)
{
dthe[i] = calcVel(targetPoint.target_joint_pos[i], targetPoint.start_joint_pos[i], targetPoint.end_time, targetPoint.time, theta[i]);
}
updatePos(dthe[0], dthe[1], dthe[2], dthe[3]);
}
targetPoint.time += dt;
}
}
