Skip to content

Commit

Permalink
Fix move_group_interface_improved.h file.
Browse files Browse the repository at this point in the history
  • Loading branch information
@MikelBueno
MikelBueno committed Mar 4, 2023
1 parent 87f2c13 commit a24fa82
Showing 1 changed file with 47 additions and 58 deletions.
105 changes: 47 additions & 58 deletions include/move_group_interface_improved.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@

#include <moveit/macros/class_forward.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/utils/moveit_error_code.h>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
#include <moveit_msgs/msg/planner_interface_description.hpp>
Expand Down Expand Up @@ -67,34 +68,7 @@ namespace moveit
/** \brief Simple interface to MoveIt components */
namespace planning_interface
{
class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes
{
public:
MoveItErrorCode()
{
val = 0;
}
MoveItErrorCode(int code)
{
val = code;
}
MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code)
{
val = code.val;
}
explicit operator bool() const
{
return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
}
bool operator==(const int c) const
{
return val == c;
}
bool operator!=(const int c) const
{
return val != c;
}
};
using MoveItErrorCode [[deprecated("Use moveit::core::MoveItErrorCode")]] = moveit::core::MoveItErrorCode;

MOVEIT_CLASS_FORWARD(MoveGroupInterface); // Defines MoveGroupInterfacePtr, ConstPtr, WeakPtr... etc

Expand All @@ -112,8 +86,10 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
/** \brief Specification of options to use when constructing the MoveGroupInterface class */
struct Options
{
Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION)
: group_name_(std::move(group_name)), robot_description_(std::move(desc))
Options(std::string group_name, std::string desc = ROBOT_DESCRIPTION, std::string move_group_namespace = "")
: group_name_(std::move(group_name))
, robot_description_(std::move(desc))
, move_group_namespace_(std::move(move_group_namespace))
{
}

Expand All @@ -125,6 +101,9 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface

/// Optionally, an instance of the RobotModel to use can be also specified
moveit::core::RobotModelConstPtr robot_model_;

/// The namespace for the move group node
std::string move_group_namespace_;
};

MOVEIT_STRUCT_FORWARD(Plan);
Expand All @@ -149,23 +128,24 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
it has to be of type ros::CallbackQueue
(which is the default type of callback queues used in ROS)
\param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified,
one will be constructed internally along with an internal TF2_ROS TransformListener
one will be constructed internally
\param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting.
*/
MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt,
const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
const rclcpp::Duration& wait_for_servers = rclcpp::Duration(-1));
const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1));

/**
\brief Construct a client for the MoveGroup action for a particular \e group.
\param tf_buffer. Specify a TF2_ROS Buffer instance to use. If not specified,
one will be constructed internally along with an internal TF2_ROS TransformListener
one will be constructed internally
\param wait_for_servers. Timeout for connecting to action servers. -1 time means unlimited waiting.
*/
MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group,
const std::shared_ptr<tf2_ros::Buffer>& tf_buffer = std::shared_ptr<tf2_ros::Buffer>(),
const rclcpp::Duration& wait_for_servers = rclcpp::Duration(-1));
const rclcpp::Duration& wait_for_servers = rclcpp::Duration::from_seconds(-1));

inline MoveGroupInterface(){}
~MoveGroupInterface();

Expand Down Expand Up @@ -721,7 +701,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
/** \brief Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified
target.
This call is not blocking (does not wait for the execution of the trajectory to complete). */
MoveItErrorCode asyncMove();
moveit::core::MoveItErrorCode asyncMove();

/** \brief Get the move_group action client used by the \e MoveGroupInterface.
The client can be used for querying the execution state of the trajectory and abort trajectory execution
Expand All @@ -733,24 +713,24 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
target.
This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous
spinner to be started.*/
MoveItErrorCode move();
moveit::core::MoveItErrorCode move();

/** \brief Compute a motion plan that takes the group declared in the constructor from the current state to the
specified
target. No execution is performed. The resulting plan is stored in \e plan*/
MoveItErrorCode plan(Plan& plan);
moveit::core::MoveItErrorCode plan(Plan& plan);

/** \brief Given a \e plan, execute it without waiting for completion. */
MoveItErrorCode asyncExecute(const Plan& plan);
moveit::core::MoveItErrorCode asyncExecute(const Plan& plan);

/** \brief Given a \e robot trajectory, execute it without waiting for completion. */
MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory);
moveit::core::MoveItErrorCode asyncExecute(const moveit_msgs::msg::RobotTrajectory& trajectory);

/** \brief Given a \e plan, execute it while waiting for completion. */
MoveItErrorCode execute(const Plan& plan);
moveit::core::MoveItErrorCode execute(const Plan& plan);

/** \brief Given a \e robot trajectory, execute it while waiting for completion. */
MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory);
moveit::core::MoveItErrorCode execute(const moveit_msgs::msg::RobotTrajectory& trajectory);

/** \brief Compute a Cartesian path that follows specified waypoints with a step size of at most \e eef_step meters
between end effector configurations of consecutive points in the result \e trajectory. The reference frame for the
Expand Down Expand Up @@ -785,13 +765,22 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
/** \brief Stop any trajectory execution, if one is active */
void stop();

/** \brief Specify whether the robot is allowed to look around before moving if it determines it should (default is
* true) */
void allowLooking(bool flag);

/** \brief Specify whether the robot is allowed to replan if it detects changes in the environment */
void allowReplanning(bool flag);

/** \brief Maximum number of replanning attempts */
void setReplanAttempts(int32_t attempts);

/** \brief Sleep this duration between replanning attempts (in walltime seconds) */
void setReplanDelay(double delay);

/** \brief Specify whether the robot is allowed to look around
before moving if it determines it should (default is false) */
void allowLooking(bool flag);

/** \brief How often is the system allowed to move the camera to update environment model when looking */
void setLookAroundAttempts(int32_t attempts);

/** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it
in \e request */
void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);
Expand Down Expand Up @@ -821,20 +810,20 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
/** \brief Pick up an object
This applies a number of hard-coded default grasps */
// MoveItErrorCode pick(const std::string& object, bool plan_only = false)
// moveit::core::MoveItErrorCode pick(const std::string& object, bool plan_only = false)
// {
// return pick(constructPickupGoal(object, std::vector<moveit_msgs::msg::Grasp>(), plan_only));
// }
//
/** \brief Pick up an object given a grasp pose */
// MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false)
// moveit::core::MoveItErrorCode pick(const std::string& object, const moveit_msgs::msg::Grasp& grasp, bool plan_only = false)
// {
// return pick(constructPickupGoal(object, { grasp }, plan_only));
// }
//
/** \brief Pick up an object given possible grasp poses */
// MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::msg::Grasp> grasps, bool plan_only =
// false)
// moveit::core::MoveItErrorCode pick(const std::string& object, std::vector<moveit_msgs::msg::Grasp> grasps, bool
// plan_only = false)
// {
// return pick(constructPickupGoal(object, std::move(grasps), plan_only));
// }
Expand All @@ -844,40 +833,40 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
Use as follows: first create the goal with constructPickupGoal(), then set \e possible_grasps and any other
desired variable in the goal, and finally pass it on to this function */

// MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal);
// moveit::core::MoveItErrorCode pick(const moveit_msgs::action::Pickup::Goal& goal);

/** \brief Pick up an object
calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */
// MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);
// moveit::core::MoveItErrorCode planGraspsAndPick(const std::string& object = "", bool plan_only = false);

/** \brief Pick up an object
calls the external moveit_msgs::srv::GraspPlanning service "plan_grasps" to compute possible grasps */
// MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false);
// moveit::core::MoveItErrorCode planGraspsAndPick(const moveit_msgs::msg::CollisionObject& object, bool plan_only = false);

/** \brief Place an object somewhere safe in the world (a safe location will be detected) */
// MoveItErrorCode place(const std::string& object, bool plan_only = false)
// moveit::core::MoveItErrorCode place(const std::string& object, bool plan_only = false)
// {
// return place(constructPlaceGoal(object, std::vector<moveit_msgs::msg::PlaceLocation>(), plan_only));
// }

/** \brief Place an object at one of the specified possible locations */
// MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations,
// moveit::core::MoveItErrorCode place(const std::string& object, std::vector<moveit_msgs::msg::PlaceLocation> locations,
// bool plan_only = false)
// {
// return place(constructPlaceGoal(object, std::move(locations), plan_only));
// }

/** \brief Place an object at one of the specified possible locations */
// MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::msg::PoseStamped>& poses,
// moveit::core::MoveItErrorCode place(const std::string& object, const std::vector<geometry_msgs::msg::PoseStamped>& poses,
// bool plan_only = false)
// {
// return place(constructPlaceGoal(object, posesToPlaceLocations(poses), plan_only));
// }

/** \brief Place an object at one of the specified possible location */
// MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool plan_only =
// false)
// moveit::core::MoveItErrorCode place(const std::string& object, const geometry_msgs::msg::PoseStamped& pose, bool
// plan_only = false)
// {
// return place(constructPlaceGoal(object, posesToPlaceLocations({ pose }), plan_only));
// }
Expand All @@ -887,7 +876,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
Use as follows: first create the goal with constructPlaceGoal(), then set \e place_locations and any other
desired variable in the goal, and finally pass it on to this function */

// MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal);
// moveit::core::MoveItErrorCode place(const moveit_msgs::action::Place::Goal& goal);

/** \brief Given the name of an object in the planning scene, make
the object attached to a link of the robot. If no link name is
Expand Down

0 comments on commit a24fa82

Please sign in to comment.