void timer_callback(const ros::TimerEvent &) { if (!c3trajectory) return; ros::Time now = ros::Time::now(); if (actionserver.isPreemptRequested()) { current_waypoint = c3trajectory->getCurrentPoint(); current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities current_waypoint_t = now; // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits)); c3trajectory_t = now; } if (actionserver.isNewGoalAvailable()) { boost::shared_ptr<const uf_common::MoveToGoal> goal = actionserver.acceptNewGoal(); current_waypoint = subjugator::C3Trajectory::Waypoint( Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed, !goal->uncoordinated); current_waypoint_t = now; // goal->header.stamp; this->linear_tolerance = goal->linear_tolerance; this->angular_tolerance = goal->angular_tolerance; c3trajectory_t = now; } while ((c3trajectory_t + traj_dt < now) and ros::ok()) { // ROS_INFO("Acting"); c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec()); c3trajectory_t += traj_dt; } PoseTwistStamped msg; msg.header.stamp = c3trajectory_t; msg.header.frame_id = fixed_frame; msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint()); trajectory_pub.publish(msg); PoseStamped posemsg; posemsg.header.stamp = c3trajectory_t; posemsg.header.frame_id = fixed_frame; posemsg.pose = Pose_from_Waypoint(current_waypoint); waypoint_pose_pub.publish(posemsg); if (actionserver.isActive() && c3trajectory->getCurrentPoint().is_approximately( current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) && current_waypoint.r.qdot == subjugator::Vector6d::Zero()) { actionserver.setSucceeded(); } }
void timer_callback(const ros::TimerEvent &) { mil_msgs::MoveToResult actionresult; // Handle disabled, killed, or no odom before attempting to produce trajectory std::string err = ""; if (disabled) err = "c3 disabled"; else if (kill_listener.isRaised()) err = "killed"; else if (!c3trajectory) err = "no odom"; if (!err.empty()) { if (c3trajectory) c3trajectory.reset(); // On revive/enable, wait for odom before station keeping // Cancel all goals while killed/disabled/no odom if (actionserver.isNewGoalAvailable()) actionserver.acceptNewGoal(); if (actionserver.isActive()) { actionresult.error = err; actionresult.success = false; actionserver.setAborted(actionresult); } return; } ros::Time now = ros::Time::now(); auto old_waypoint = current_waypoint; if (actionserver.isNewGoalAvailable()) { boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal(); current_waypoint = subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist), goal->speed, !goal->uncoordinated, !goal->blind); current_waypoint_t = now; this->linear_tolerance = goal->linear_tolerance; this->angular_tolerance = goal->angular_tolerance; waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN); // Check if waypoint is valid std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid( Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation); actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second); actionresult.success = checkWPResult.first; if (checkWPResult.first == false && waypoint_check_) // got a point that we should not move to { waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED); if (checkWPResult.second == WAYPOINT_ERROR_TYPE::UNKNOWN) // if unknown, check if there's a huge displacement with the new waypoint { auto a_point = Pose_from_Waypoint(current_waypoint); auto b_point = Pose_from_Waypoint(old_waypoint); // If moved more than .5m, then don't allow if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5) { ROS_ERROR("can't move there! - need to rotate"); current_waypoint = old_waypoint; } } // if point is occupied, reject move if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED) { ROS_ERROR("can't move there! - waypoint is occupied"); current_waypoint = old_waypoint; } // if point is above water, reject move if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER) { ROS_ERROR("can't move there! - waypoint is above water"); current_waypoint = old_waypoint; } if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID) { ROS_ERROR("WaypointValidity - Did not recieve any ogrid"); } } } if (actionserver.isPreemptRequested()) { current_waypoint = c3trajectory->getCurrentPoint(); current_waypoint.do_waypoint_validation = false; current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities current_waypoint_t = now; // don't try to make output c3 continuous when cancelled - instead stop as quickly as possible c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits)); c3trajectory_t = now; } // Remember the previous trajectory auto old_trajectory = c3trajectory->getCurrentPoint(); while (c3trajectory_t + traj_dt < now) { c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec()); c3trajectory_t += traj_dt; } // Check if we will hit something while in trajectory the new trajectory geometry_msgs::Pose traj_point; // Convert messages to correct type auto p = c3trajectory->getCurrentPoint(); traj_point.position = vec2xyz<Point>(p.q.head(3)); quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation); std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation); if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_) { // New trajectory will hit an occupied goal, so reject ROS_ERROR("can't move there! - bad trajectory"); current_waypoint = old_trajectory; current_waypoint.do_waypoint_validation = false; current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities current_waypoint_t = now; c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits)); c3trajectory_t = now; actionresult.success = false; actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY); } PoseTwistStamped msg; msg.header.stamp = c3trajectory_t; msg.header.frame_id = fixed_frame; msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint()); trajectory_pub.publish(msg); waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200); PoseStamped msgVis; msgVis.header = msg.header; msgVis.pose = msg.posetwist.pose; trajectory_vis_pub.publish(msgVis); PoseStamped posemsg; posemsg.header.stamp = c3trajectory_t; posemsg.header.frame_id = fixed_frame; posemsg.pose = Pose_from_Waypoint(current_waypoint); waypoint_pose_pub.publish(posemsg); if (actionserver.isActive() && c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance), max(1e-3, angular_tolerance)) && current_waypoint.r.qdot == subjugator::Vector6d::Zero()) { actionresult.error = ""; actionresult.success = true; actionserver.setSucceeded(actionresult); } }