void BaseMotionController::run() { while (ros::ok()) { ros::Rate(10).sleep(); ros::spinOnce(); // wait for start trigger and new Twist message if (!start_relative_movement_ || !new_command_sent_) { continue; } odom_received_ = false; if (start_relative_movement_) { // rotate first then translate if (rotate_first_) { if (z_rot_ != 0.0) { rotateRelative(z_rot_); } if (x_trans_ != 0.0 || y_trans_ != 0.0) { translateRelative(x_trans_, y_trans_); } } else { // translate first then rotate if (x_trans_ != 0.0 || y_trans_ != 0.0) { translateRelative(x_trans_, y_trans_); } if (z_rot_ != 0.0) { rotateRelative(z_rot_); } } } if (new_command_sent_) { new_command_sent_ = false; x_trans_ = 0.0; y_trans_ = 0.0; z_rot_ = 0.0; } done_moving_ = true; std_msgs::String move_done_str; move_done_str.data = "e_done"; move_done_pub_.publish(move_done_str); } }
// // 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()