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()); }