MovePlatformAction() :
as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
ac_planner_("/plan_action", true), // Planner action CLIENT
ac_control_("/control_action", true) // Controller action CLIENT
{
n_.param("/move_platform_server/debug", debug_, false);
std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
name = name + ".debug";
logger_name_ = "debug";
//logger is ros.carlos_motion_action_server.debug
if (debug_)
{
// if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
// so for debug we'll use a named logger
if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
ros::console::notifyLoggerLevelsChanged();
}
else // if not DEBUG we want INFO
{
if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
ros::console::notifyLoggerLevelsChanged();
}
ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");
as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));
//start the move server
as_.start();
ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");
// now wait for the other servers (planner + controller) to start
ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
ac_planner_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected());
ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
ac_control_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected());
n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);
planning_ = false;
controlling_ = false;
//set_terminal_state_;
ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);
}
BlockManipulationAction() :
block_detection_action_("block_detection", true),
interactive_manipulation_action_("interactive_manipulation", true),
pick_and_place_action_("pick_and_place", true),
reset_arm_action_("reset_arm", true)
{
// Load parameters
nh_.param<std::string>("/block_manipulation_action_demo/arm_link", arm_link, "/base_link");
nh_.param<double>("/block_manipulation_action_demo/gripper_open", gripper_open, 0.042);
nh_.param<double>("/block_manipulation_action_demo/gripper_closed", gripper_closed, 0.024);
nh_.param<double>("/block_manipulation_action_demo/z_up", z_up, 0.12);
nh_.param<double>("/block_manipulation_action_demo/table_height", z_down, 0.01);
nh_.param<double>("/block_manipulation_action_demo/block_size", block_size, 0.03);
nh_.param<bool>("once", once, false);
ROS_INFO("Block size %f", block_size);
ROS_INFO("Table height %f", z_down);
// Initialize goals
block_detection_goal_.frame = arm_link;
block_detection_goal_.table_height = z_down;
block_detection_goal_.block_size = block_size;
pick_and_place_goal_.frame = arm_link;
pick_and_place_goal_.z_up = z_up;
pick_and_place_goal_.gripper_open = gripper_open;
pick_and_place_goal_.gripper_closed = gripper_closed;
pick_and_place_goal_.topic = pick_and_place_topic;
interactive_manipulation_goal_.block_size = block_size;
interactive_manipulation_goal_.frame = arm_link;
ROS_INFO("Finished initializing, waiting for servers...");
block_detection_action_.waitForServer();
ROS_INFO("Found block detection server.");
interactive_manipulation_action_.waitForServer();
ROS_INFO("Found interactive manipulation.");
pick_and_place_action_.waitForServer();
ROS_INFO("Found pick and place server.");
ROS_INFO("Found servers.");
reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal());
ROS_INFO("Reseted arm action.");
detectBlocks();
}
PickAndPlaceServer(const std::string name) :
nh_("~"), as_(name, false), action_name_(name), client_("/move_right_arm", true)
{
//register the goal and feeback callbacks
as_.registerGoalCallback(boost::bind(&PickAndPlaceServer::goalCB, this));
as_.registerPreemptCallback(boost::bind(&PickAndPlaceServer::preemptCB, this));
as_.start();
client_.waitForServer();
ROS_INFO("Connected to server");
gripper = nh_.advertise<std_msgs::Float64>("/parallel_gripper_controller/command", 1, false);
}
void turn() {
turtlebot_actions::TurtlebotMoveGoal goal;
goal.forward_distance = 0;
goal.turn_distance = M_PI;
action_client.waitForServer();
action_client.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = action_client.waitForResult(
ros::Duration(30.0));
if (finished_before_timeout) {
actionlib::SimpleClientGoalState state = action_client.getState();
ROS_INFO("Action finished: %s", state.toString().c_str());
} else
ROS_INFO("Action did not finish before the time out.");
}
void move_straight() {
float current_angle = 0;
float desired_angle = 0;
/*
if (map->frames.size() > 0) {
Sophus::SE3f position =
map->frames[map->frames.size() - 1]->get_pos();
Eigen::Vector3f current_heading = position.unit_quaternion()
* Eigen::Vector3f::UnitZ();
current_angle = std::atan2(-current_heading(1), current_heading(0));
desired_angle = std::asin(position.translation()(1)/10.0);
}*/
turtlebot_actions::TurtlebotMoveGoal goal;
goal.forward_distance = 25.0;
goal.turn_distance = current_angle - desired_angle;
action_client.waitForServer();
action_client.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = action_client.waitForResult(
ros::Duration(500.0));
if (finished_before_timeout) {
actionlib::SimpleClientGoalState state = action_client.getState();
ROS_INFO("Action finished: %s", state.toString().c_str());
} else
ROS_INFO("Action did not finish before the time out.");
map->save("corridor_map");
}
// Constructor
PickPlaceMoveItServer(const std::string name):
nh_("~"),
movegroup_action_("pickup", true),
clam_arm_client_("clam_arm", true),
ee_marker_is_loaded_(false),
block_published_(false),
action_server_(name, false)
{
base_link_ = "/base_link";
// -----------------------------------------------------------------------------------------------
// Adding collision objects
collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1);
// -----------------------------------------------------------------------------------------------
// Connect to move_group/Pickup action server
while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server");
}
// ---------------------------------------------------------------------------------------------
// Connect to ClamArm action server
while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server");
}
// ---------------------------------------------------------------------------------------------
// Create planning scene monitor
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));
if (planning_scene_monitor_->getPlanningScene())
{
//planning_scene_monitor_->startWorldGeometryMonitor();
//planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
//planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
}
else
{
ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured");
}
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_));
// ---------------------------------------------------------------------------------------------
// Register the goal and preempt callbacks
action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this));
action_server_.start();
// Announce state
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready.");
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command...");
// ---------------------------------------------------------------------------------------------
// Send home
ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home");
clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET;
clam_arm_client_.sendGoal(clam_arm_goal_);
while(!clam_arm_client_.getState().isDone() && ros::ok())
ros::Duration(0.1).sleep();
// ---------------------------------------------------------------------------------------------
// Send fake command
fake_goalCB();
}
void executeCb(const frontier_exploration::ExploreTaskGoalConstPtr &goal)
{
success_ = false;
moving_ = false;
explore_costmap_ros_->resetLayers();
//create costmap services
ros::ServiceClient updateBoundaryPolygon = private_nh_.serviceClient<frontier_exploration::UpdateBoundaryPolygon>("explore_costmap/explore_boundary/update_boundary_polygon");
ros::ServiceClient getNextFrontier = private_nh_.serviceClient<frontier_exploration::GetNextFrontier>("explore_costmap/explore_boundary/get_next_frontier");
//wait for move_base and costmap services
if(!move_client_.waitForServer() || !updateBoundaryPolygon.waitForExistence() || !getNextFrontier.waitForExistence()){
as_.setAborted();
return;
}
//set region boundary on costmap
if(ros::ok() && as_.isActive()){
frontier_exploration::UpdateBoundaryPolygon srv;
srv.request.explore_boundary = goal->explore_boundary;
if(updateBoundaryPolygon.call(srv)){
ROS_INFO("Region boundary set");
}else{
ROS_ERROR("Failed to set region boundary");
as_.setAborted();
return;
}
}
//loop until all frontiers are explored
ros::Rate rate(frequency_);
while(ros::ok() && as_.isActive()){
frontier_exploration::GetNextFrontier srv;
//placeholder for next goal to be sent to move base
geometry_msgs::PoseStamped goal_pose;
//get current robot pose in frame of exploration boundary
tf::Stamped<tf::Pose> robot_pose;
explore_costmap_ros_->getRobotPose(robot_pose);
//provide current robot pose to the frontier search service request
tf::poseStampedTFToMsg(robot_pose,srv.request.start_pose);
//evaluate if robot is within exploration boundary using robot_pose in boundary frame
geometry_msgs::PoseStamped eval_pose = srv.request.start_pose;
if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){
tf_listener_.transformPose(goal->explore_boundary.header.frame_id, srv.request.start_pose, eval_pose);
}
//check if robot is not within exploration boundary and needs to return to center of search area
if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){
//check if robot has explored at least one frontier, and promote debug message to warning
if(success_){
ROS_WARN("Robot left exploration boundary, returning to center");
}else{
ROS_DEBUG("Robot not initially in exploration boundary, traveling to center");
}
//get current robot position in frame of exploration center
geometry_msgs::PointStamped eval_point;
eval_point.header = eval_pose.header;
eval_point.point = eval_pose.pose.position;
if(eval_point.header.frame_id != goal->explore_center.header.frame_id){
geometry_msgs::PointStamped temp = eval_point;
tf_listener_.transformPoint(goal->explore_center.header.frame_id, temp, eval_point);
}
//set goal pose to exploration center
goal_pose.header = goal->explore_center.header;
goal_pose.pose.position = goal->explore_center.point;
goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(eval_point.point, goal->explore_center.point) );
}else if(getNextFrontier.call(srv)){ //if in boundary, try to find next frontier to search
ROS_DEBUG("Found frontier to explore");
success_ = true;
goal_pose = feedback_.next_frontier = srv.response.next_frontier;
retry_ = 5;
}else{ //if no frontier found, check if search is successful
ROS_DEBUG("Couldn't find a frontier");
//search is succesful
if(retry_ == 0 && success_){
ROS_WARN("Finished exploring room");
as_.setSucceeded();
boost::unique_lock<boost::mutex> lock(move_client_lock_);
move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
return;
}else if(retry_ == 0 || !ros::ok()){ //search is not successful
ROS_ERROR("Failed exploration");
as_.setAborted();
return;
}
ROS_DEBUG("Retrying...");
retry_--;
//try to find frontier again, without moving robot
continue;
}
//if above conditional does not escape this loop step, search has a valid goal_pose
//check if new goal is close to old goal, hence no need to resend
if(!moving_ || !pointsNearby(move_client_goal_.target_pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)){
ROS_DEBUG("New exploration goal");
move_client_goal_.target_pose = goal_pose;
boost::unique_lock<boost::mutex> lock(move_client_lock_);
if(as_.isActive()){
move_client_.sendGoal(move_client_goal_, boost::bind(&FrontierExplorationServer::doneMovingCb, this, _1, _2),0,boost::bind(&FrontierExplorationServer::feedbackMovingCb, this, _1));
moving_ = true;
}
lock.unlock();
}
//check if continuous goal updating is enabled
if(frequency_ > 0){
//sleep for specified frequency and then continue searching
rate.sleep();
}else{
//wait for movement to finish before continuing
while(ros::ok() && as_.isActive() && moving_){
ros::WallDuration(0.1).sleep();
}
}
}
//goal should never be active at this point
ROS_ASSERT(!as_.isActive());
}
