mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
generate grasp poses: add time management
This commit is contained in:
parent
5ff374a551
commit
34bd5f8614
@ -39,7 +39,8 @@ protected:
|
||||
|
||||
double timeout_;
|
||||
|
||||
/* temp values */
|
||||
/* temp values */
|
||||
|
||||
double current_angle_;
|
||||
|
||||
double remaining_time_;
|
||||
|
||||
@ -11,9 +11,11 @@
|
||||
|
||||
#include <Eigen/Geometry>
|
||||
|
||||
#include <chrono>
|
||||
|
||||
moveit::task_constructor::subtasks::GenerateGraspPose::GenerateGraspPose(std::string name)
|
||||
: moveit::task_constructor::SubTask::SubTask(name),
|
||||
timeout_(0.2),
|
||||
timeout_(0.1),
|
||||
current_angle_(0.0),
|
||||
remaining_time_(timeout_)
|
||||
{
|
||||
@ -73,7 +75,9 @@ moveit::task_constructor::subtasks::GenerateGraspPose::compute(){
|
||||
pose.orientation= tf::createQuaternionMsgFromRollPitchYaw(M_PI, 0.0, current_angle_);
|
||||
std::cout << "trying " << current_angle_ << std::endl;
|
||||
|
||||
auto now= std::chrono::steady_clock::now();
|
||||
bool succeeded= grasp_state.setFromIK(jmg, pose, eef_, remaining_time_/*,validState*/);
|
||||
remaining_time_-= std::chrono::duration<double>(std::chrono::steady_clock::now()- now).count();
|
||||
|
||||
moveit_msgs::DisplayRobotState drs;
|
||||
robot_state::robotStateToRobotStateMsg(grasp_state, drs.state);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user