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()
// // void translateRelative(dx, dy) // Last modified: 03Sep2006 // // Translates the robot relative to itself based // on the parameterized x-/y-coordinate translations. // // Returns: <none> // Parameters: // dx in the x-coordinate translation // dy in the y-coordinate translation // void Robot::translateRelative(const GLfloat dx, const GLfloat dy) { translateRelative(Vector(dx, dy)); } // rotateRelative(const GLfloat, const GLfloat)