shared_ptr<action::Action> FixedAngleTrace::control(const string& taskName)
{
float simTime = WM.getSimTime();
LOG_PRINTF("task","current time: %f", simTime);
//cout<<"TaskName: "<<taskName<<endl;
if( taskName!=mTaskName && true==mTask->changeable )
{
mEntryTaskName=taskName;
setCurrentTask(taskName);
LOG_PRINT("task"," start new task "+mTaskName);
LOG_PRINT("task","current pos is "+getCurrentPose());
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 1
}
float remainTime = mTaskBeginTime + mTask->time - simTime;
if ( remainTime > 0.005f )
{
LOG_PRINT("task"," continue task "+mTaskName);
LOG_PRINT("task","current pos is "+getCurrentPose());
LOG_FLUSH;
shared_ptr<action::Action> act
//= Timing::control(WM.predictedPerception(), mDesiredPose, remainTime);
= Timing::control(WM.lastPerception(), mDesiredPose, remainTime,taskName); //TT 2
int adjustFoot = mTask->adjustFoot;
if ( mFootExchanged ) adjustFoot = -adjustFoot;
// FOOT_ADJUSTER.adjust( adjustFoot, shared_static_cast<action::JointAction>(act) );
mIsFinished = false;
return act;
}
// the task should be finished
//// if no next task
if ( "null" == mTask->next )
{
//cout<<" stop at task "<<mTaskName<<' '<<mTask->next<<'\n';
mIsFinished = true;
mTaskName = "null";
//mEntryTaskName = "null";
LOG_PRINTF("task","do null");
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(),mDesiredPose,(20.0f * serversetting::sim_step));
return Timing::control(WM.lastPerception(),mDesiredPose,(20.0f * serversetting::sim_step),taskName); //TT 3
}
//// start next task
setCurrentTask( mTask->next );
mIsStartNextTask = true;
LOG_PRINT("task"," start next task "+mTaskName);
LOG_FLUSH;
//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 4
}
void GoToSelectedBall::publishPose(float dist_from_ball){
ROS_INFO("enter publishPose, distance = %f", dist_from_ball);
if(isBallPoseSet== false){
ROS_INFO("publishPose, wait for ball position, return");
return;
}
float ball_odom_x = getCurrentPose().x;
float ball_odom_y = getCurrentPose().y;
float robot_odom_x, robot_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
float dx = ball_odom_x - robot_odom_x;
float dy = ball_odom_y - robot_odom_y;
float dist = sqrt(pow(ball_odom_x - robot_odom_x, 2) + pow(ball_odom_y-robot_odom_y, 2) );
ROS_INFO("dist = %f",dist);
float goal_odom_x = robot_odom_x + dx * (dist - dist_from_ball) / dist;
float goal_odom_y = robot_odom_y + dy * (dist - dist_from_ball) / dist;
float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y);
tf::Quaternion q;
float goal_map_x, goal_map_y;
transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, goal_map_x, goal_map_y, q);
geometry_msgs::Quaternion qMsg;
tf::quaternionTFToMsg(q, qMsg);
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = goal_map_x;
goal.target_pose.pose.position.y = goal_map_y;
goal.target_pose.pose.position.z = 0;
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/map";
ROS_INFO("Sending goal");
ac.sendGoal(goal);
firstGoalSent = true;
}
float GoToSelectedBall::getAngleDiff(){
float robot_odom_x, robot_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
float goalAngle = getAngle(robot_odom_x, robot_odom_y, getCurrentPose().x, getCurrentPose().y);
float robotAngle = getRobotAngleInOdom();
return fabs(goalAngle-robotAngle);
}
void BoxGrid::renderAnchor()
{
glColor4f(1.0,0,0,1.0);
glBegin(GL_QUADS);
for(int i=0;i<anchors.size();i++)
{
int idBox = anchors[i];
RowVector3 p[8];
p[0] = getCurrentPose(getBoxNodes(idBox,7));
p[1] = getCurrentPose(getBoxNodes(idBox,6));
p[2] = getCurrentPose(getBoxNodes(idBox,5));
p[3] = getCurrentPose(getBoxNodes(idBox,4));
p[4] = getCurrentPose(getBoxNodes(idBox,3));
p[5] = getCurrentPose(getBoxNodes(idBox,2));
p[6] = getCurrentPose(getBoxNodes(idBox,1));
p[7] = getCurrentPose(getBoxNodes(idBox,0));
glNormal((p[3]-p[7]).cross(p[6]-p[7]));//bottom
glVertex(p[7]);
glVertex(p[3]);
glVertex(p[2]);
glVertex(p[6]);
glNormal((p[4]-p[5]).cross(p[1]-p[5]));//top
glVertex(p[5]);
glVertex(p[4]);
glVertex(p[0]);
glVertex(p[1]);
glNormal((p[5]-p[7]).cross(p[3]-p[7]));//far
glVertex(p[7]);
glVertex(p[5]);
glVertex(p[1]);
glVertex(p[3]);
glNormal((p[1]-p[3]).cross(p[2]-p[3]));//right
glVertex(p[3]);
glVertex(p[1]);
glVertex(p[0]);
glVertex(p[2]);
glNormal((p[2]-p[6]).cross(p[4]-p[6]));//front
glVertex(p[6]);
glVertex(p[2]);
glVertex(p[0]);
glVertex(p[4]);
glNormal((p[6]-p[7]).cross(p[5]-p[7]));//left
glVertex(p[7]);
glVertex(p[6]);
glVertex(p[4]);
glVertex(p[5]);
}
glEnd();
RowVector4 color(1.0,0.0,0.0,1.0);
for(int i=0;i<anchorNodes.size();i++)
{
RowVector3 p = getCurrentPose(anchorNodes[i]);
paintPoint(p, 0.007,color);
}
}
void GoToSelectedBall::publishAngle(){
ROS_INFO("enter publishAngle");
if(isBallPoseSet== false){
ROS_INFO("publishAngle, wait for ball position, return");
return;
}
float ball_odom_x = getCurrentPose().x;
float ball_odom_y = getCurrentPose().y;
float robot_odom_x, robot_odom_y;
getRobotPositionInOdom(robot_odom_x, robot_odom_y);
float goal_odom_x = robot_odom_x;// + dx * (dist - dist_from_ball) / dist;
float goal_odom_y = robot_odom_y;// + dy * (dist - dist_from_ball) / dist;
float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y);
//tf::Quaternion q;
//float goal_map_x, goal_map_y;
//transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, goal_map_x, goal_map_y, q);
geometry_msgs::Quaternion qMsg;
// tf::quaternionTFToMsg(q, qMsg);
setAngle(angle, qMsg);
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = goal_odom_x;
goal.target_pose.pose.position.y = goal_odom_y;
goal.target_pose.pose.position.z = 0;
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/odom";
ROS_INFO("Sending goal");
ac.sendGoal(goal);
firstGoalSent = true;
// isBallPoseSet = false;
ROS_INFO("return publishAngle");
}
void BoxGrid::computeBoxPositions() {
m_boxPositions.setZero(getNumBoxes(),3);
#pragma omp parallel for
for(int i = 0 ; i < getNumBoxes() ; i++) {
for(int k = 0 ; k < 8 ; ++k) {
m_boxPositions.row(i) += getCurrentPose(m_boxNodes(i,k));
}
}
m_boxPositions *= 0.125;
}
//zhiping luo
void BoxGrid::computeTransformation(NodeHandles & m_nodehandles)
{
vector<int> nodes;
for(int idBox = 0; idBox <nnzBoxes; idBox++)
{
if(isHull(idBox))
{
for(int i = 0; i< 8; i++){
int idNode = getBoxNodes(idBox, i);
if(!inTheList(nodes, idNode)){
nodes.push_back(idNode);
RowVector3 P = getCurrentPose(idNode);
RowVector3 P_rest = getRestPose(idNode);
int m_numEdges = 0;
Quaternion quaternions[6];
for(int j = 0; j<6;j++){
int idNeighborNode = getNodeNodes(idNode, j);
if(idNeighborNode!=-1){
float tmp[4];
RowVector3 P1 = getRestPose(idNeighborNode);
RowVector3 P2 = getCurrentPose(idNeighborNode);
RowVector3 P3 = (P_rest - P1).normalized();
RowVector3 P4 = (P - P2).normalized();
extract_rotation_pseudo_edge(tmp, P3[0],P3[1],P3[2],P4[0],P4[1],P4[2]);
Quaternion r(tmp[3],tmp[0],tmp[1],tmp[2]);
r.normalize();
quaternions[m_numEdges] = r;
m_numEdges++;
}
}
Quaternion r = AccurateAverageQuaternion(quaternions,m_numEdges);
m_nodehandles.setR(idNode, r);
m_nodehandles.setT(idNode, P);
}
}
}
}
nodes.clear();
}
void GoToSelectedBall::goToBall(){
move_base_msgs::MoveBaseGoal goal;
goal.target_pose.pose.position.x = getCurrentPose().x;
goal.target_pose.pose.position.y = getCurrentPose().y;
geometry_msgs::Quaternion qMsg;
setAngle(getRobotAngleInMap(), qMsg);
goal.target_pose.pose.orientation = qMsg;
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.header.frame_id ="/odom";
ROS_INFO("Sending goal...");
ac.sendGoalAndWait(goal,ros::Duration(120),ros::Duration(0.1));
firstGoalSent = true;
}
const RowVector3 & BoxGrid::V(int idV) const {
return getCurrentPose(idV);
}
const RowVector3 & BoxGrid::V(int idF, int idV) const {
return getCurrentPose(faces(idF,idV));
}
void MoveToSimpleServer::goalCallback(const MoveToSimpleGoalConstPtr& goal)
{
ROS_INFO("Got a new goal to work on... Hmmm...");
int driving_direction = 1;
if(goal->driving_direction == MoveToSimpleGoal::REVERSE)
driving_direction = -1;
geometry_msgs::PoseStamped start_pose = getCurrentPose();
geometry_msgs::PoseStamped current_pose;
current_pose.header = start_pose.header;
current_pose.pose = start_pose.pose;
geometry_msgs::Point final_point;
float angle;
// Simple Transformation.
// X = x.cos(theta) - y.sin(theta) + trans-x;
// Y = x.sin(theta) + y.cos(theta) + trans-y;
final_point.x = goal->goal_pose.pose.position.x * cos(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) -
goal->goal_pose.pose.position.y * sin(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
start_pose.pose.position.x;
final_point.y = goal->goal_pose.pose.position.x * sin(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
goal->goal_pose.pose.position.y * cos(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
start_pose.pose.position.y;
ros::Rate publish_rate(4);
float xy_tolerance = goal->xy_tolerance > 0.075 ? goal->xy_tolerance : 0.075;
while(!equals(final_point, current_pose.pose.position, xy_tolerance))
{
////////////////////////////
///// Publish velocity /////
////////////////////////////
if(driving_direction == 1)
angle = atan2(final_point.y - current_pose.pose.position.y, final_point.x - current_pose.pose.position.x);
else
angle = atan2(current_pose.pose.position.y - final_point.y, current_pose.pose.position.x - final_point.x);
float error_fi = angle - (2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w));
geometry_msgs::Twist resultant_velocity;
if( sqrt( (final_point.x - current_pose.pose.position.x) * (final_point.x - current_pose.pose.position.x) +
(final_point.y - current_pose.pose.position.y) * (final_point.y - current_pose.pose.position.y)) < 0.75)
resultant_velocity.linear.x = 0.15 * driving_direction;
else
resultant_velocity.linear.x = 0.3 * driving_direction;
resultant_velocity.angular.z = (atan2( sin(error_fi), cos(error_fi)) * 0.66);
if(resultant_velocity.angular.z > 1.5)
resultant_velocity.angular.z = 1.5;
else if(resultant_velocity.angular.z < -1.5)
resultant_velocity.angular.z = -1.5;
_cmd_vel_pub.publish(resultant_velocity);
//ROS_INFO("Velocities: x, theta = (%f, %f)", resultant_velocity.linear.x, resultant_velocity.angular.z);
//ROS_INFO("ANGLE: (%f) and [%f]", angle, (2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)));
////////////////////////////////////
///// Recalculate our position /////
////////////////////////////////////
current_pose = getCurrentPose();
publish_rate.sleep();
}
// Execute the rotation behaviour.
float goal_fi_relative = 2*atan2(goal->goal_pose.pose.orientation.z, goal->goal_pose.pose.orientation.w);
float goal_fi = 2*atan2(start_pose.pose.orientation.z, start_pose.pose.orientation.w) + goal_fi_relative;
goal_fi = atan2(sin(goal_fi), cos(goal_fi));
float our_fi = 2*atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w);
float yaw_tolerance = goal->yaw_tolerance > 0.15 ? goal->yaw_tolerance : 0.15;;
while(ros::ok())
{
current_pose = getCurrentPose();
our_fi = 2*atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w);
float del_fi = atan2(sin(goal_fi - our_fi), cos((goal_fi - our_fi)));
geometry_msgs::Twist resultant_velocity;
resultant_velocity.angular.z = (del_fi * 0.66);
if(resultant_velocity.angular.z > 1.5)
resultant_velocity.angular.z = 1.5;
else if(resultant_velocity.angular.z < -1.5)
resultant_velocity.angular.z = -1.5;
_cmd_vel_pub.publish(resultant_velocity);
//ROS_INFO("ANGLE: goal_relative: %f", goal_fi_relative);
//ROS_INFO("del_fi: (%f)", del_fi);
publish_rate.sleep();
if(fabs(del_fi) < yaw_tolerance )
break;
}
ROS_INFO("Action succeeded!");
_server.setSucceeded();
}
