Skip to content

Commit

Permalink
Fix Jafar's change requests
Browse files Browse the repository at this point in the history
  • Loading branch information
bostoncleek committed Jun 17, 2022
1 parent 5145e0c commit 7c47532
Show file tree
Hide file tree
Showing 3 changed files with 6 additions and 13 deletions.
13 changes: 3 additions & 10 deletions core/src/stages/action_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -44,8 +44,8 @@ constexpr char LOGNAME[] = "action_base";

ActionBase::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<grasping_msgs::GraspPlanningAction>(nh_, action_name_, spin_thread));
clientPtr_ = std::make_unique<actionlib::SimpleActionClient<grasping_msgs::GraspPlanningAction>>(nh_, action_name_,
spin_thread);

// Negative timeouts are set to zero
server_timeout_ = server_timeout_ < std::numeric_limits<double>::epsilon() ? 0.0 : server_timeout_;
Expand All @@ -57,14 +57,7 @@ ActionBase::ActionBase(const std::string& action_name, bool spin_thread, double
}

ActionBase::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<grasping_msgs::GraspPlanningAction>(nh_, action_name_, spin_thread));

ROS_DEBUG_STREAM_NAMED(LOGNAME, "Waiting for connection to grasp generation action server...");
clientPtr_->waitForServer(ros::Duration(server_timeout_));
ROS_DEBUG_STREAM_NAMED(LOGNAME, "Connected to server");
}
: ActionBase::ActionBase(action_name, spin_thread, 0.0, 0.0) {}
} // namespace stages
} // namespace task_constructor
} // namespace moveit
2 changes: 1 addition & 1 deletion core/src/stages/grasp_provider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ void GraspProvider::compute() {
composeGoal();

// monitor feedback/results
// blocking function untill timeout reached or results received
// blocking function until timeout reached or results received
if (monitorGoal()) {
// Protect grasp candidate incase feedback is being recieved asynchronously
const std::lock_guard<std::mutex> lock(grasp_mutex_);
Expand Down
4 changes: 2 additions & 2 deletions msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,6 @@ generate_messages(DEPENDENCIES ${MSG_DEPS})

catkin_package(
CATKIN_DEPENDS
message_runtime
${MSG_DEPS}
message_runtime
${MSG_DEPS}
)

0 comments on commit 7c47532

Please sign in to comment.