Skip to content

Commit

Permalink
Remove template paramete to action base class
Browse files Browse the repository at this point in the history 
The action base class can only be used with the action msg
GraspPlanning.action from grasping_msgs.
  • Loading branch information
@bostoncleek @v4hn
bostoncleek authored and v4hn committed Jun 25, 2021
1 parent 19fb72c commit ae376b6
Show file tree
Hide file tree
Showing 10 changed files with 301 additions and 261 deletions.
3 changes: 3 additions & 0 deletions core/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
actionlib
eigen_conversions
geometry_msgs
grasping_msgs
moveit_core
moveit_ros_planning
moveit_ros_planning_interface
Expand All @@ -23,7 +24,9 @@ catkin_package(
INCLUDE_DIRS
include
CATKIN_DEPENDS
actionlib
geometry_msgs
grasping_msgs
moveit_core
moveit_task_constructor_msgs
rviz_marker_tools
Expand Down
37 changes: 6 additions & 31 deletions core/include/moveit/task_constructor/stages/action_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,23 +41,15 @@
#include <limits>

#include <actionlib/client/simple_action_client.h>
#include <grasping_msgs/GraspPlanningAction.h>

namespace moveit {
namespace task_constructor {
namespace stages {

/**
* @brief Interface allowing stages to use a simple action client
* @param ActionSpec - action message (action message name + "ACTION")
* @details Some stages may require an action client. This class wraps the
* simple client action interface and exposes event based execution callbacks.
*/
template <class ActionSpec>
/** @brief Interface allowing stages to use a simple action client */
class ActionBase
{
protected:
ACTION_DEFINITION(ActionSpec);

public:
/**
* @brief Constructor
Expand Down Expand Up @@ -87,39 +79,22 @@ class ActionBase
* @brief Called every time feedback is received for the goal
* @param feedback - pointer to the feedback message
*/
virtual void feedbackCallback(const FeedbackConstPtr& feedback) = 0;
virtual void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) = 0;

/**
* @brief Called once when the goal completes
* @param state - state info for goal
* @param result - pointer to result message
*/
virtual void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) = 0;
virtual void doneCallback(const actionlib::SimpleClientGoalState& state,
const grasping_msgs::GraspPlanningResultConstPtr& result) = 0;

protected:
ros::NodeHandle nh_;
std::string action_name_; // action name space
std::unique_ptr<actionlib::SimpleActionClient<ActionSpec>> clientPtr_; // action client
std::unique_ptr<actionlib::SimpleActionClient<grasping_msgs::GraspPlanningAction>> clientPtr_; // action client
double server_timeout_, goal_timeout_; // connection and goal completed time out
};

template <class ActionSpec>
ActionBase<ActionSpec>::ActionBase(const std::string& action_name, bool spin_thread, double goal_timeout,
double server_timeout)
: action_name_(action_name), server_timeout_(server_timeout), goal_timeout_(goal_timeout) {
clientPtr_.reset(new actionlib::SimpleActionClient<ActionSpec>(nh_, action_name_, spin_thread));

// Negative timeouts are set to zero
server_timeout_ = server_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : server_timeout_;
goal_timeout_ = goal_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : goal_timeout_;
}

template <class ActionSpec>
ActionBase<ActionSpec>::ActionBase(const std::string& action_name, bool spin_thread)
: action_name_(action_name), server_timeout_(0.0), goal_timeout_(0.0) {
clientPtr_.reset(new actionlib::SimpleActionClient<ActionSpec>(nh_, action_name_, spin_thread));
}

} // namespace stages
} // namespace task_constructor
} // namespace moveit
209 changes: 10 additions & 199 deletions core/include/moveit/task_constructor/stages/grasp_provider.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#pragma once

#include <functional>
#include <mutex>

#include <moveit/task_constructor/stages/generate_pose.h>
#include <moveit/task_constructor/stages/action_base.h>
#include <moveit/task_constructor/storage.h>
Expand All @@ -48,20 +50,9 @@ namespace moveit {
namespace task_constructor {
namespace stages {

constexpr char LOGNAME[] = "grasp_provider";

/**
* @brief Generate grasp candidates using deep learning approaches
* @param ActionSpec - action message (action message name + "ACTION")
* @details Interfaces with a deep learning based grasp library using a action client
*/
template <class ActionSpec>
class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
/** @brief Generate grasp candidates using deep learning approaches */
class GraspProvider : public GeneratePose, ActionBase
{
private:
typedef ActionBase<ActionSpec> ActionBaseT;
ACTION_DEFINITION(ActionSpec);

public:
/**
* @brief Constructor
Expand All @@ -71,7 +62,7 @@ class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
* @param server_timeout - connection to server time out (0 is considered infinite timeout)
* @details Initialize the client and connect to server
*/
GraspProvider(const std::string& action_name, const std::string& stage_name = "generate grasp pose",
GraspProvider(const std::string& action_name, const std::string& stage_name = "grasp provider",
double goal_timeout = 0.0, double server_timeout = 0.0);

/**
Expand All @@ -88,8 +79,9 @@ class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
bool monitorGoal();

void activeCallback() override;
void feedbackCallback(const FeedbackConstPtr& feedback) override;
void doneCallback(const actionlib::SimpleClientGoalState& state, const ResultConstPtr& result) override;
void feedbackCallback(const grasping_msgs::GraspPlanningFeedbackConstPtr& feedback) override;
void doneCallback(const actionlib::SimpleClientGoalState& state,
const grasping_msgs::GraspPlanningResultConstPtr& result) override;

void init(const core::RobotModelConstPtr& robot_model) override;
void compute() override;
Expand All @@ -106,191 +98,10 @@ class GraspProvider : public GeneratePose, ActionBase<ActionSpec>
void onNewSolution(const SolutionBase& s) override;

private:
std::mutex grasp_mutex_;
bool found_candidates_;
std::vector<geometry_msgs::PoseStamped> grasp_candidates_;
std::vector<double> costs_;
std::vector<moveit_msgs::Grasp> grasp_candidates_;
};

template <class ActionSpec>
GraspProvider<ActionSpec>::GraspProvider(const std::string& action_name, const std::string& stage_name,
double goal_timeout, double server_timeout)
: GeneratePose(stage_name), ActionBaseT(action_name, false, goal_timeout, server_timeout), found_candidates_(false) {
auto& p = properties();
p.declare<std::string>("eef", "name of end-effector");
p.declare<std::string>("object");
p.declare<boost::any>("pregrasp", "pregrasp posture");
p.declare<boost::any>("grasp", "grasp posture");

ROS_INFO_NAMED(LOGNAME, "Waiting for connection to grasp generation action server...");
ActionBaseT::clientPtr_->waitForServer(ros::Duration(ActionBaseT::server_timeout_));
ROS_INFO_NAMED(LOGNAME, "Connected to server");
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::composeGoal() {
Goal goal;
goal.action_name = ActionBaseT::action_name_;
ActionBaseT::clientPtr_->sendGoal(
goal, std::bind(&GraspProvider<ActionSpec>::doneCallback, this, std::placeholders::_1, std::placeholders::_2),
std::bind(&GraspProvider<ActionSpec>::activeCallback, this),
std::bind(&GraspProvider<ActionSpec>::feedbackCallback, this, std::placeholders::_1));

ROS_INFO_NAMED(LOGNAME, "Goal sent to server: %s", ActionBaseT::action_name_.c_str());
}

template <class ActionSpec>
bool GraspProvider<ActionSpec>::monitorGoal() {
// monitor timeout
const bool monitor_timeout = ActionBaseT::goal_timeout_ > std::numeric_limits<double>::epsilon() ? true : false;
const double timeout_time = ros::Time::now().toSec() + ActionBaseT::goal_timeout_;

while (ActionBaseT::nh_.ok()) {
ros::spinOnce();

// timeout reached
if (ros::Time::now().toSec() > timeout_time && monitor_timeout) {
ActionBaseT::clientPtr_->cancelGoal();
ROS_ERROR_NAMED(LOGNAME, "Grasp pose generator time out reached");
return false;
} else if (found_candidates_) {
// timeout not reached (or not active) and grasps are found
// only way return true
break;
}
}

return true;
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::activeCallback() {
ROS_INFO_NAMED(LOGNAME, "Generate grasp goal now active");
found_candidates_ = false;
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::feedbackCallback(const FeedbackConstPtr& feedback) {
// each candidate should have a cost
if (feedback->grasp_candidates.size() != feedback->costs.size()) {
ROS_ERROR_NAMED(LOGNAME, "Invalid input: each grasp candidate needs an associated cost");
} else {
ROS_INFO_NAMED(LOGNAME, "Grasp generated feedback received %lu candidates: ", feedback->grasp_candidates.size());

grasp_candidates_.resize(feedback->grasp_candidates.size());
costs_.resize(feedback->costs.size());

grasp_candidates_ = feedback->grasp_candidates;
costs_ = feedback->costs;

found_candidates_ = true;
}
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::doneCallback(const actionlib::SimpleClientGoalState& state,
const ResultConstPtr& result) {
if (state == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO_NAMED(LOGNAME, "Found grasp candidates (result): %s", result->grasp_state.c_str());
} else {
ROS_ERROR_NAMED(LOGNAME, "No grasp candidates found (state): %s",
ActionBaseT::clientPtr_->getState().toString().c_str());
}
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::init(const core::RobotModelConstPtr& robot_model) {
InitStageException errors;
try {
GeneratePose::init(robot_model);
} catch (InitStageException& e) {
errors.append(e);
}

const auto& props = properties();

// check availability of object
props.get<std::string>("object");
// check availability of eef
const std::string& eef = props.get<std::string>("eef");
if (!robot_model->hasEndEffector(eef)) {
errors.push_back(*this, "unknown end effector: " + eef);
} else {
// check availability of eef pose
const moveit::core::JointModelGroup* jmg = robot_model->getEndEffector(eef);
const std::string& name = props.get<std::string>("pregrasp");
std::map<std::string, double> m;
if (!jmg->getVariableDefaultPositions(name, m)) {
errors.push_back(*this, "unknown end effector pose: " + name);
}
}

if (errors) {
throw errors;
}
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::compute() {
if (upstream_solutions_.empty()) {
return;
}
planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();

// set end effector pose
const auto& props = properties();
const std::string& eef = props.get<std::string>("eef");
const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getEndEffector(eef);

robot_state::RobotState& robot_state = scene->getCurrentStateNonConst();
robot_state.setToDefaultValues(jmg, props.get<std::string>("pregrasp"));

// compose/send goal
composeGoal();

// monitor feedback/results
// blocking function untill timeout reached or results received
if (monitorGoal()) {
// ROS_WARN_NAMED(LOGNAME, "number %lu: ",grasp_candidates_.size());
for (unsigned int i = 0; i < grasp_candidates_.size(); i++) {
InterfaceState state(scene);
state.properties().set("target_pose", grasp_candidates_.at(i));
props.exposeTo(state.properties(), { "pregrasp", "grasp" });

SubTrajectory trajectory;
trajectory.setCost(costs_.at(i));
trajectory.setComment(std::to_string(i));

// add frame at target pose
rviz_marker_tools::appendFrame(trajectory.markers(), grasp_candidates_.at(i), 0.1, "grasp frame");

spawn(std::move(state), std::move(trajectory));
}
}
}

template <class ActionSpec>
void GraspProvider<ActionSpec>::onNewSolution(const SolutionBase& s) {
planning_scene::PlanningSceneConstPtr scene = s.end()->scene();

const auto& props = properties();
const std::string& object = props.get<std::string>("object");
if (!scene->knowsFrameTransform(object)) {
const std::string msg = "object '" + object + "' not in scene";
if (storeFailures()) {
InterfaceState state(scene);
SubTrajectory solution;
solution.markAsFailure();
solution.setComment(msg);
spawn(std::move(state), std::move(solution));
} else {
ROS_WARN_STREAM_NAMED(LOGNAME, msg);
}
return;
}

upstream_solutions_.push(&s);
}

} // namespace stages
} // namespace task_constructor
} // namespace moveit
1 change: 1 addition & 0 deletions core/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,7 @@

<depend>actionlib</depend>
<depend>eigen_conversions</depend>
<depend>grasping_msgs</depend>
<depend>geometry_msgs</depend>
<depend>moveit_core</depend>
<depend>moveit_ros_planning</depend>
Expand Down
5 changes: 4 additions & 1 deletion core/src/stages/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,12 @@ add_library(${PROJECT_NAME}_stages

${PROJECT_INCLUDE}/stages/action_base.h
${PROJECT_INCLUDE}/stages/current_state.h
${PROJECT_INCLUDE}/stages/grasp_provider.h
${PROJECT_INCLUDE}/stages/fixed_state.h
${PROJECT_INCLUDE}/stages/fixed_cartesian_poses.h
${PROJECT_INCLUDE}/stages/generate_pose.h
${PROJECT_INCLUDE}/stages/generate_grasp_pose.h
${PROJECT_INCLUDE}/stages/generate_place_pose.h
${PROJECT_INCLUDE}/stages/grasp_provider.h
${PROJECT_INCLUDE}/stages/compute_ik.h
${PROJECT_INCLUDE}/stages/passthrough.h
${PROJECT_INCLUDE}/stages/predicate_filter.h
Expand All @@ -24,12 +24,14 @@ add_library(${PROJECT_NAME}_stages
modify_planning_scene.cpp
fix_collision_objects.cpp

action_base.cpp
current_state.cpp
fixed_state.cpp
fixed_cartesian_poses.cpp
generate_pose.cpp
generate_grasp_pose.cpp
generate_place_pose.cpp
grasp_provider.cpp
compute_ik.cpp
passthrough.cpp
predicate_filter.cpp
Expand All @@ -41,6 +43,7 @@ add_library(${PROJECT_NAME}_stages
simple_grasp.cpp
pick.cpp
)

target_link_libraries(${PROJECT_NAME}_stages ${PROJECT_NAME} ${catkin_LIBRARIES})

add_library(${PROJECT_NAME}_stage_plugins
Expand Down
Loading

0 comments on commit ae376b6

Please sign in to comment.