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 actualizeGoal(ros::NodeHandle& nh,
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction>& client,
arm_navigation_msgs::MoveArmGoal goal) {
if (nh.ok())
{
bool finished_within_time = false;
client.sendGoal(goal);
finished_within_time = client.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
client.cancelGoal();
ROS_INFO("Timed out achieving goal");
}
else
{
actionlib::SimpleClientGoalState state = client.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
}
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");
}
void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
{
ROS_INFO("[pick and place] Picking. Also placing.");
simple_arm_server::MoveArmGoal goal;
simple_arm_server::ArmAction action;
simple_arm_server::ArmAction grip;
/* open gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.move_time.sec = 1.0;
grip.command = gripper_open;
goal.motions.push_back(grip);
/* arm straight up */
tf::Quaternion temp;
temp.setRPY(0,1.57,0);
action.goal.orientation.x = temp.getX();
action.goal.orientation.y = temp.getY();
action.goal.orientation.z = temp.getZ();
action.goal.orientation.w = temp.getW();
/* hover over */
action.goal.position.x = start_pose.position.x;
action.goal.position.y = start_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 0.25;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = start_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* close gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.command = gripper_closed;
grip.move_time.sec = 1.0;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* hover over */
action.goal.position.x = end_pose.position.x;
action.goal.position.y = end_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = end_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* open gripper */
grip.command = gripper_open;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
goal.header.frame_id = arm_link;
client_.sendGoal(goal);
ROS_INFO("[pick and place] Sent goal. Waiting.");
client_.waitForResult(ros::Duration(30.0));
ROS_INFO("[pick and place] Received result.");
if (client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
as_.setSucceeded(result_);
else
{
ROS_INFO("Actual state: %s", client_.getState().toString().c_str());
as_.setAborted(result_);
}
}
bool executeGrasps(const std::vector<manipulation_msgs::Grasp>& possible_grasps,
const geometry_msgs::Pose& block_pose)
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Creating Pickup Goal");
// ---------------------------------------------------------------------------------------------
// Create PlanningOptions
moveit_msgs::PlanningOptions options;
// The diff to consider for the planning scene (optional)
//PlanningScene planning_scene_diff
// If this flag is set to true, the action
// returns an executable plan in the response but does not attempt execution
options.plan_only = false;
// If this flag is set to true, the action of planning &
// executing is allowed to look around (move sensors) if
// it seems that not enough information is available about
// the environment
options.look_around = false;
// If this value is positive, the action of planning & executing
// is allowed to look around for a maximum number of attempts;
// If the value is left as 0, the default value is used, as set
// with dynamic_reconfigure
//int32 look_around_attempts
// If set and if look_around is true, this value is used as
// the maximum cost allowed for a path to be considered executable.
// If the cost of a path is higher than this value, more sensing or
// a new plan needed. If left as 0.0 but look_around is true, then
// the default value set via dynamic_reconfigure is used
//float64 max_safe_execution_cost
// If the plan becomes invalidated during execution, it is possible to have
// that plan recomputed and execution restarted. This flag enables this
// functionality
options.replan = false;
// The maximum number of replanning attempts
//int32 replan_attempts
// ---------------------------------------------------------------------------------------------
// Create and populate the goal
moveit_msgs::PickupGoal goal;
// An action for picking up an object
// The name of the object to pick up (as known in the planning scene)
goal.target_name = chosen_block_object_.id;
// which group should be used to plan for pickup
goal.group_name = PLANNING_GROUP_NAME;
// which end-effector to be used for pickup (ideally descpending from the group above)
goal.end_effector = EE_NAME;
// a list of possible grasps to be used. At least one grasp must be filled in
goal.possible_grasps.resize(possible_grasps.size());
goal.possible_grasps = possible_grasps;
// the name that the support surface (e.g. table) has in the collision map
// can be left empty if no name is available
//string collision_support_surface_name
// whether collisions between the gripper and the support surface should be acceptable
// during move from pre-grasp to grasp and during lift. Collisions when moving to the
// pre-grasp location are still not allowed even if this is set to true.
goal.allow_gripper_support_collision = true;
// The names of the links the object to be attached is allowed to touch;
// If this is left empty, it defaults to the links in the used end-effector
//string[] attached_object_touch_links
// Optional constraints to be imposed on every point in the motion plan
//Constraints path_constraints
// an optional list of obstacles that we have semantic information about
// and that can be touched/pushed/moved in the course of grasping;
// CAREFUL: If the object name 'all' is used, collisions with all objects are disabled during the approach & lift.
//string[] allowed_touch_objects
// The maximum amount of time the motion planner is allowed to plan for
goal.allowed_planning_time = 10.0; // seconds?
// Planning options
goal.planning_options = options;
//ROS_INFO_STREAM_NAMED("pick_place_moveit","Pause");
//ros::Duration(5.0).sleep();
// ---------------------------------------------------------------------------------------------
// Send the grasp to move_group/Pickup
ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending pick action to move_group/Pickup");
movegroup_action_.sendGoal(goal);
if(!movegroup_action_.waitForResult(ros::Duration(20.0)))
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Returned early?");
return false;
}
if (movegroup_action_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Plan successful!");
}
else
{
ROS_ERROR_STREAM_NAMED("pick_place_moveit","FAILED: " << movegroup_action_.getState().toString() << ": " << movegroup_action_.getState().getText());
return false;
}
return true;
}
// 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();
}
