void executeCB(const tilting_servo::servoGoalConstPtr &goal)
{
if(start == true)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
start = false;
}
if(as_.isNewGoalAvailable())
as_.acceptNewGoal();
if(goal->angle != prev_goal || goal->speed != prev_speed)
{
servo_move(Comport, servo_id, goal->angle, goal->speed);
prev_goal = goal->angle;
prev_speed = goal->speed;
}
while((as_.isNewGoalAvailable() == false) && ros::ok())
{
feedback_.angle = read_angle(Comport, servo_id);
if(feedback_.angle >= min_angle - 10 && feedback_.angle <= max_angle + 10)
as_.publishFeedback(feedback_);
}
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
// set the action state to preempted
as_.setPreempted();
}
}
void Explore::goalCB(){
// explore_ = true;
// accept the new goal
//goal_ =
explore_state_ = FULL_ROTATE;
as_.acceptNewGoal();
}
// Action server sends goals here
void goalCB()
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Received goal -----------------------------------------------");
pick_place_goal_ = action_server_.acceptNewGoal();
base_link_ = pick_place_goal_->frame;
processGoal(pick_place_goal_->pickup_pose, pick_place_goal_->place_pose);
}
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 goalCB()
{
ROS_INFO("[pick and place] Received goal!");
goal_ = as_.acceptNewGoal();
arm_link = goal_->frame;
gripper_open = goal_->gripper_open;
gripper_closed = goal_->gripper_closed;
z_up = goal_->z_up;
if (goal_->topic.length() < 1)
pickAndPlace(goal_->pickup_pose, goal_->place_pose);
else
pick_and_place_sub_ = nh_.subscribe(goal_->topic, 1, &PickAndPlaceServer::sendGoalFromTopic, this);
}
void GoalCB() {
ROS_INFO("%s: received new goal.", action_name_.c_str());
blobTracker.clear();
comps.clear();
hierarchy.clear();
contours.clear();
numCC.clear();
// accept the new goal
goal_ = *server_.acceptNewGoal();
if (goal_.numSearchGoals <= 0)
return;
num_feature_goals_ = goal_.numSearchGoals;
feature_mean_goals_.clear();
feature_var_goals_.clear();
feature_mean_goals_.resize(num_feature_goals_);
feature_var_goals_.resize(num_feature_goals_);
numCC.resize(num_feature_goals_);
comps = vector<vector<ConnectedComp> >();
contours = vector<vector<vector<Point> > >();
hierarchy = vector<vector<Vec4i> >();
comps.resize(num_feature_goals_);
contours.resize(num_feature_goals_);
hierarchy.resize(num_feature_goals_);
blobTracker.resize(num_feature_goals_);
for(int i=0; i<num_feature_goals_; i++)
{
comps[i].resize(maxNumComponents);
blobTracker[i] = FeatureBlobTracker(maxNumComponents, width, height);
blobTracker[i].initialize();
feature_mean_goals_[i] = goal_.signal[i].mean[0];
feature_var_goals_[i] = goal_.signal[i].variance[0];
}
got_goals = true;
ROS_INFO("%d features for color feature search received.", num_feature_goals_);
}
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
void goalCB()
{
// accept the new goal
feedback_.forward_distance = 0.0;
feedback_.turn_distance = 0.0;
result_.forward_distance = 0.0;
result_.turn_distance = 0.0;
goal_ = as_.acceptNewGoal();
if (!turnOdom(goal_->turn_distance))
{
as_.setAborted(result_);
return;
}
if (driveForwardOdom(goal_->forward_distance))
as_.setSucceeded(result_);
else
as_.setAborted(result_);
}
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);
}
}
void goalCB()
{
// accept the new goal
has_goal_ = as_.acceptNewGoal()->approach;
}
void goalCB()
{
// ---------------------------------------------------------------------------------------------
// Accept the new goal
goal_ = action_server_.acceptNewGoal();
ROS_INFO("[block logic] Received goal! %f, %s", goal_->block_size, goal_->frame.c_str());
block_size = goal_->block_size;
arm_link = goal_->frame;
if (initialized_)
{
addBlocks(msg_);
}
// --------------------------------------------------------------------------------------------
// Start pose - choose one that is preferrably not in the goal region
geometry_msgs::Pose start_pose;
bool found_pose = false;
// Check if there is only 1 block detected
if( !msg_->poses.size() )
{
// no blocks, what to do?
ROS_WARN("[block logic] No blocks found");
}
else if( msg_->poses.size() == 1 )
{
start_pose = msg_->poses[0];
found_pose = true;
ROS_INFO("[block logic] Only 1 block, using it");
}
else
{
// Search for block that meets our criteria
for(int i = 0; i < msg_->poses.size(); ++i)
{
if( msg_->poses[i].position.y > -0.12 && msg_->poses[i].position.y < 0.2 ) // start of goal region
{
start_pose = msg_->poses[i];
found_pose = true;
ROS_INFO_STREAM("[block logic] Chose this block:\n" << start_pose);
break;
}
}
if( !found_pose )
{
ROS_INFO("[block logic] Did not find a good block, default to first");
start_pose = msg_->poses[0];
found_pose = true;
}
}
// --------------------------------------------------------------------------------------------
// End pose is just chosen place on board
geometry_msgs::Pose end_pose;
end_pose.orientation = msg_->poses[0].orientation; // keep the same orientation
end_pose.position.x = 0.225;
end_pose.position.y = 0.18;
end_pose.position.z = msg_->poses[0].position.z;
// --------------------------------------------------------------------------------------------
// Move that block
if( found_pose )
{
moveBlock(start_pose, end_pose);
}
}
void Explore::goalCB(){
explore_state_ = FULL_ROTATE;
as_.acceptNewGoal();
}
