Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

remove dead pick and place code #3025

Merged
merged 2 commits into from
Oct 11, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -302,10 +302,6 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
/** \brief Set the starting state for planning to be that reported by the robot's joint state publication */
void setStartStateToCurrentState();

/** \brief For pick/place operations, the name of the support surface is used to specify the fact that attached
* objects are allowed to touch the support surface */
void setSupportSurfaceName(const std::string& name);

/**
* \name Setting a joint state target (goal)
*
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -585,11 +585,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
pose_reference_frame_ = pose_reference_frame;
}

void setSupportSurfaceName(const std::string& support_surface)
{
support_surface_ = support_surface;
}

const std::string& getPoseReferenceFrame() const
{
return pose_reference_frame_;
Expand Down Expand Up @@ -1083,61 +1078,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
constructMotionPlanRequest(goal.request);
}

// moveit_msgs::action::Pickup::Goal constructPickupGoal(const std::string& object,
// std::vector<moveit_msgs::msg::Grasp>&& grasps,
// bool plan_only = false) const
// {
// moveit_msgs::action::Pickup::Goal goal;
// goal.target_name = object;
// goal.group_name = opt_.group_name;
// goal.end_effector = getEndEffector();
// goal.support_surface_name = support_surface_;
// goal.possible_grasps = std::move(grasps);
// if (!support_surface_.empty())
// goal.allow_gripper_support_collision = true;
//
// if (path_constraints_)
// goal.path_constraints = *path_constraints_;
//
// goal.planner_id = planner_id_;
// goal.allowed_planning_time = allowed_planning_time_;
//
// goal.planning_options.plan_only = plan_only;
// goal.planning_options.look_around = can_look_;
// goal.planning_options.replan = can_replan_;
// goal.planning_options.replan_delay = replan_delay_;
// goal.planning_options.planning_scene_diff.is_diff = true;
// goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
// return goal;
// }

// moveit_msgs::action::Place::Goal constructPlaceGoal(const std::string& object,
// std::vector<moveit_msgs::msg::PlaceLocation>&& locations,
// bool plan_only = false) const
// {
// moveit_msgs::action::Place::Goal goal;
// goal.group_name = opt_.group_name;
// goal.attached_object_name = object;
// goal.support_surface_name = support_surface_;
// goal.place_locations = std::move(locations);
// if (!support_surface_.empty())
// goal.allow_gripper_support_collision = true;
//
// if (path_constraints_)
// goal.path_constraints = *path_constraints_;
//
// goal.planner_id = planner_id_;
// goal.allowed_planning_time = allowed_planning_time_;
//
// goal.planning_options.plan_only = plan_only;
// goal.planning_options.look_around = can_look_;
// goal.planning_options.replan = can_replan_;
// goal.planning_options.replan_delay = replan_delay_;
// goal.planning_options.planning_scene_diff.is_diff = true;
// goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
// return goal;
// }

void setPathConstraints(const moveit_msgs::msg::Constraints& constraint)
{
path_constraints_ = std::make_unique<moveit_msgs::msg::Constraints>(constraint);
Expand Down Expand Up @@ -1308,7 +1248,6 @@ class MoveGroupInterface::MoveGroupInterfaceImpl
std::unique_ptr<moveit_msgs::msg::TrajectoryConstraints> trajectory_constraints_;
std::string end_effector_link_;
std::string pose_reference_frame_;
std::string support_surface_;

// ROS communication
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr trajectory_event_publisher_;
Expand Down Expand Up @@ -1511,46 +1450,6 @@ moveit::core::MoveItErrorCode MoveGroupInterface::plan(Plan& plan)
return impl_->plan(plan);
}

// moveit_msgs::action::Pickup::Goal MoveGroupInterface::constructPickupGoal(const std::string& object,
// std::vector<moveit_msgs::msg::Grasp> grasps,
// bool plan_only = false) const
//{
// return impl_->constructPickupGoal(object, std::move(grasps), plan_only);
//}
//
// moveit_msgs::action::Place::Goal MoveGroupInterface::constructPlaceGoal(
// const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations, bool plan_only = false) const
//{
// return impl_->constructPlaceGoal(object, std::move(locations), plan_only);
//}
//
// std::vector<moveit_msgs::msg::PlaceLocation>
// MoveGroupInterface::posesToPlaceLocations(const std::vector<geometry_msgs::msg::PoseStamped>& poses) const
//{
// return impl_->posesToPlaceLocations(poses);
//}
//
// moveit::core::MoveItErrorCode MoveGroupInterface::pick(const moveit_msgs::action::Pickup::Goal& goal)
//{
// return impl_->pick(goal);
//}
//
// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const std::string& object, bool plan_only)
//{
// return impl_->planGraspsAndPick(object, plan_only);
//}
//
// moveit::core::MoveItErrorCode MoveGroupInterface::planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object,
// bool plan_only)
//{
// return impl_->planGraspsAndPick(object, plan_only);
//}
//
// moveit::core::MoveItErrorCode MoveGroupInterface::place(const moveit_msgs::action::Place::Goal& goal)
//{
// return impl_->place(goal);
//}

double MoveGroupInterface::computeCartesianPath(const std::vector<geometry_msgs::msg::Pose>& waypoints, double eef_step,
moveit_msgs::msg::RobotTrajectory& trajectory, bool avoid_collisions,
moveit_msgs::msg::MoveItErrorCodes* error_code)
Expand Down Expand Up @@ -2310,11 +2209,6 @@ double MoveGroupInterface::getPlanningTime() const
return impl_->getPlanningTime();
}

void MoveGroupInterface::setSupportSurfaceName(const std::string& name)
{
impl_->setSupportSurfaceName(name);
}

const rclcpp::Node::SharedPtr& MoveGroupInterface::getNode() const
{
return impl_->node_;
Expand Down
Loading