// // void step() // Last modified: 27Dec2010 // // Executes the appropriate active behavior. // // Returns: <none> // Parameters: <none> // void Robot::step() { GLfloat collDist = collisionRadius(); vector<Vector> rels = getObjectRelationships(collDist); if (rels.size() > 0) { GLfloat minDist = HUGE; GLint minIndex = 0; for (GLint i = 0, n = rels.size(); i < n; ++i) { GLfloat currDist = rels[i].magnitude(); if (currDist < minDist) { minDist = currDist; minIndex = i; } } //avoid(rels[minIndex], collDist); } if (behavior.isActive()) { translateRelative(getTransVel()); rotateRelative(getAngVel()); } } // step()
void Robot::step() { if (behavior.isActive()) { translateRelative(getTransVel()); rotateRelative(getAngVel()); } } // step()
void trackPointCB(const geometry_msgs::Point::ConstPtr& msg) { // make sure that the action hasn't been canceled if (!as_.isActive()) { twist_.angular.z = 0; twist_.linear.x = 0; twist_pub_.publish(twist_); return; } if( !has_goal_) { twist_.angular.z = 0; twist_.linear.x = 0; twist_pub_.publish(twist_); return; } if( obstacle_detected_ && msg->y < SVTIUS) { result_.success = false; ROS_INFO("%s: Obsticle Detected", action_name_.c_str()); // set the action state to succeeded as_.setSucceeded(result_); } feedback_.success = false; twist_.angular.z = 0; twist_.linear.x = 0; if ( msg->y >= TOP_T && msg->x <= RIGHT_T && msg->x >= LEFT_T ) { twist_pub_.publish(twist_); ROS_INFO("%s: Succeeded", action_name_.c_str()); // set the action state to succeeded result_.success = true; as_.setSucceeded(result_); } if(drop_flag) { twist_pub_.publish(twist_); ROS_INFO("%s: Succeeded", action_name_.c_str()); // set the action state to succeeded result_.success = true; drop_flag = false; as_.setSucceeded(result_); } if (msg->y < TOP_T) { ROS_INFO("MOVE FORWARD"); twist_.linear.x = getLinVel(msg->y, TOP_T); } if (msg->x < LEFT_T) { ROS_INFO("TURN RIGHT"); twist_.angular.z = getAngVel(msg->x, LEFT_T); } else if (msg->x > RIGHT_T) { ROS_INFO("TURN LEFT"); twist_.angular.z = -1.0 * getAngVel(msg->x, RIGHT_T); } twist_pub_.publish(twist_); as_.publishFeedback(feedback_); }