本文整理汇总了C++中actionlib::SimpleActionClient类的典型用法代码示例。如果您正苦于以下问题:C++ SimpleActionClient类的具体用法?C++ SimpleActionClient怎么用?C++ SimpleActionClient使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了SimpleActionClient类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: moveGoalCB
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
开发者ID:inesc-tec-robotics,项目名称:carlos_motion,代码行数:42,代码来源:move_platform_server.cpp
示例2: planningDoneCB
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
{
planning_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());
move_result_.result_state = result->result_state;
if (move_result_.result_state) //if plan OK
{
planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner
if (ctrl_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
controlling_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Controller is down!!!";
as_.setAborted(move_result_);
}
else
{
ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
controlling_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
}
}
else //if plan NOT OK
{
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.error_string = "Planning Failed: " + result->error_string;
ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);
as_.setAborted(move_result_);
}
return;
}
开发者ID:inesc-tec-robotics,项目名称:carlos_motion,代码行数:40,代码来源:move_platform_server.cpp
示例3: actualizeGoal
void actualizeGoal(ros::NodeHandle& nh,
actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction>& client,
arm_navigation_msgs::MoveArmGoal goal) {
if (nh.ok())
{
bool finished_within_time = false;
client.sendGoal(goal);
finished_within_time = client.waitForResult(ros::Duration(200.0));
if (!finished_within_time)
{
client.cancelGoal();
ROS_INFO("Timed out achieving goal");
}
else
{
actionlib::SimpleClientGoalState state = client.getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action finished: %s",state.toString().c_str());
else
ROS_INFO("Action failed: %s",state.toString().c_str());
}
}
}
开发者ID:hicannon,项目名称:scrubber,代码行数:24,代码来源:move_arm_simple_pose_goal.cpp
示例4: rt_arm_plan_jspace_path_current_to_qgoal
int ArmMotionCommander::rt_arm_plan_jspace_path_current_to_qgoal(Eigen::VectorXd q_des_vec) {
ROS_INFO("requesting a joint-space motion plan");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_JSPACE_PATH_CURRENT_TO_QGOAL;
cart_goal_.q_goal_right.resize(7);
for (int i=0;i<7;i++) cart_goal_.q_goal_right[i] = q_des_vec[i]; //specify the goal js pose
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0));
ROS_INFO("return code: %d",cart_result_.return_code);
if (!finished_before_timeout_) {
ROS_WARN("giving up waiting on result");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
ROS_INFO("finished before timeout");
if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) {
ROS_WARN("right arm plan not valid");
return (int) cart_result_.return_code;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("unknown return code... not SUCCESS");
return (int) cart_result_.return_code;
}
//here if success return code
ROS_INFO("returned SUCCESS from planning request");
computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time();
ROS_INFO("computed move time: %f",computed_arrival_time_);
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:30,代码来源:table_wiper.cpp
示例5: rt_arm_plan_path_current_to_goal_pose
int ArmMotionCommander::rt_arm_plan_path_current_to_goal_pose(geometry_msgs::PoseStamped des_pose) {
ROS_INFO("requesting a cartesian-space motion plan");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_POSE;
cart_goal_.des_pose_gripper_right = des_pose;
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0));
ROS_INFO("return code: %d",cart_result_.return_code);
if (!finished_before_timeout_) {
ROS_WARN("giving up waiting on result");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
ROS_INFO("finished before timeout");
if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) {
ROS_WARN("right arm plan not valid");
return (int) cart_result_.return_code;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("unknown return code... not SUCCESS");
return (int) cart_result_.return_code;
}
//here if success return code
ROS_INFO("returned SUCCESS from planning request");
computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time();
ROS_INFO("computed move time: %f",computed_arrival_time_);
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:29,代码来源:table_wiper.cpp
示例6: rt_arm_plan_path_current_to_goal_dp_xyz
int ArmMotionCommander::rt_arm_plan_path_current_to_goal_dp_xyz(Eigen::Vector3d dp_displacement) {
ROS_INFO("requesting a cartesian-space motion plan along vector");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_PLAN_PATH_CURRENT_TO_GOAL_DP_XYZ;
//must fill in desired vector displacement
cart_goal_.arm_dp_right.resize(3);
for (int i=0;i<3;i++) cart_goal_.arm_dp_right[i] = dp_displacement[i];
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0));
ROS_INFO("return code: %d",cart_result_.return_code);
if (!finished_before_timeout_) {
ROS_WARN("giving up waiting on result");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
ROS_INFO("finished before timeout");
if (cart_result_.return_code==cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID) {
ROS_WARN("right arm plan not valid");
return (int) cart_result_.return_code;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("unknown return code... not SUCCESS");
return (int) cart_result_.return_code;
}
//here if success return code
ROS_INFO("returned SUCCESS from planning request");
computed_arrival_time_= cart_result_.computed_arrival_time; //action_client.get_computed_arrival_time();
ROS_INFO("computed move time: %f",computed_arrival_time_);
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:31,代码来源:table_wiper.cpp
示例7: rt_arm_request_tool_pose_wrt_torso
int ArmMotionCommander::rt_arm_request_tool_pose_wrt_torso(void) {
// debug: compare this to output of:
//rosrun tf tf_echo torso yale_gripper_frame
ROS_INFO("requesting right-arm tool pose");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_TOOL_POSE;
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0));
if (!finished_before_timeout_) {
ROS_WARN("did not respond within timeout");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("move did not return success; code = %d",cart_result_.return_code);
return (int) cart_result_.return_code;
}
tool_pose_stamped_ = cart_result_.current_pose_gripper_right;
ROS_INFO("move returned success; right arm tool pose: ");
ROS_INFO("origin w/rt torso = %f, %f, %f ",tool_pose_stamped_.pose.position.x,
tool_pose_stamped_.pose.position.y,tool_pose_stamped_.pose.position.z);
ROS_INFO("quaternion x,y,z,w: %f, %f, %f, %f",tool_pose_stamped_.pose.orientation.x,
tool_pose_stamped_.pose.orientation.y,tool_pose_stamped_.pose.orientation.z,
tool_pose_stamped_.pose.orientation.w);
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:25,代码来源:table_wiper.cpp
示例8: MovePlatformAction
MovePlatformAction() :
as_(n_, CARLOS_MOVE_ACTION, false), //movePlatform action SERVER
ac_planner_("/plan_action", true), // Planner action CLIENT
ac_control_("/control_action", true) // Controller action CLIENT
{
n_.param("/move_platform_server/debug", debug_, false);
std::string name = ROSCONSOLE_DEFAULT_NAME; //ros.carlos_motion_action_server
name = name + ".debug";
logger_name_ = "debug";
//logger is ros.carlos_motion_action_server.debug
if (debug_)
{
// if we use ROSCONSOLE_DEFAULT_NAME we'll get a ton of debug messages from actionlib which is annoying!!!
// so for debug we'll use a named logger
if(ros::console::set_logger_level(name, ros::console::levels::Debug)) //name
ros::console::notifyLoggerLevelsChanged();
}
else // if not DEBUG we want INFO
{
if(ros::console::set_logger_level(name, ros::console::levels::Info)) //name
ros::console::notifyLoggerLevelsChanged();
}
ROS_DEBUG_NAMED(logger_name_, "Starting Move Platform Server");
as_.registerGoalCallback(boost::bind(&MovePlatformAction::moveGoalCB, this));
as_.registerPreemptCallback(boost::bind(&MovePlatformAction::movePreemptCB, this));
//start the move server
as_.start();
ROS_DEBUG_NAMED(logger_name_, "Move Platform Action Server Started");
// now wait for the other servers (planner + controller) to start
ROS_WARN_NAMED(logger_name_, "Waiting for planner server to start");
ac_planner_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Planner server started: " << ac_planner_.isServerConnected());
ROS_WARN_NAMED(logger_name_, "Waiting for controller server to start");
ac_control_.waitForServer();
ROS_INFO_STREAM_NAMED(logger_name_, "Controller server started: " << ac_control_.isServerConnected());
n_.param("/carlos/fsm_frequency", frequency_, DEFAULT_STATE_FREQ);
state_pub_timer_ = n_.createTimer(frequency_, &MovePlatformAction::state_pub_timerCB, this);
state_pub_ = n_.advertise<mission_ctrl_msgs::hardware_state>(CARLOS_BASE_STATE_MSG,1);
planning_ = false;
controlling_ = false;
//set_terminal_state_;
ctrl_state_sub = n_.subscribe<std_msgs::String>("/oea_controller/controller_state", 5, &MovePlatformAction::control_stateCB, this);
planner_state_sub = n_.subscribe<std_msgs::UInt8>("/oea_planner/state", 5, &MovePlatformAction::planner_stateCB, this);
}
开发者ID:inesc-tec-robotics,项目名称:carlos_motion,代码行数:55,代码来源:move_platform_server.cpp
示例9: BlockManipulationAction
BlockManipulationAction() :
block_detection_action_("block_detection", true),
interactive_manipulation_action_("interactive_manipulation", true),
pick_and_place_action_("pick_and_place", true),
reset_arm_action_("reset_arm", true)
{
// Load parameters
nh_.param<std::string>("/block_manipulation_action_demo/arm_link", arm_link, "/base_link");
nh_.param<double>("/block_manipulation_action_demo/gripper_open", gripper_open, 0.042);
nh_.param<double>("/block_manipulation_action_demo/gripper_closed", gripper_closed, 0.024);
nh_.param<double>("/block_manipulation_action_demo/z_up", z_up, 0.12);
nh_.param<double>("/block_manipulation_action_demo/table_height", z_down, 0.01);
nh_.param<double>("/block_manipulation_action_demo/block_size", block_size, 0.03);
nh_.param<bool>("once", once, false);
ROS_INFO("Block size %f", block_size);
ROS_INFO("Table height %f", z_down);
// Initialize goals
block_detection_goal_.frame = arm_link;
block_detection_goal_.table_height = z_down;
block_detection_goal_.block_size = block_size;
pick_and_place_goal_.frame = arm_link;
pick_and_place_goal_.z_up = z_up;
pick_and_place_goal_.gripper_open = gripper_open;
pick_and_place_goal_.gripper_closed = gripper_closed;
pick_and_place_goal_.topic = pick_and_place_topic;
interactive_manipulation_goal_.block_size = block_size;
interactive_manipulation_goal_.frame = arm_link;
ROS_INFO("Finished initializing, waiting for servers...");
block_detection_action_.waitForServer();
ROS_INFO("Found block detection server.");
interactive_manipulation_action_.waitForServer();
ROS_INFO("Found interactive manipulation.");
pick_and_place_action_.waitForServer();
ROS_INFO("Found pick and place server.");
ROS_INFO("Found servers.");
reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal());
ROS_INFO("Reseted arm action.");
detectBlocks();
}
开发者ID:scottmishra,项目名称:clam,代码行数:54,代码来源:block_manipulation_action_demo.cpp
示例10: send_test_goal
void ArmMotionCommander::send_test_goal(void) {
ROS_INFO("sending a test goal");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::ARM_TEST_MODE;
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(2.0));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout_) {
ROS_WARN("giving up waiting on result");
} else {
ROS_INFO("finished before timeout");
ROS_INFO("return code: %d",cart_result_.return_code);
}
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:14,代码来源:table_wiper.cpp
示例11: rt_arm_execute_planned_path
int ArmMotionCommander::rt_arm_execute_planned_path(void) {
ROS_INFO("requesting execution of planned path");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_EXECUTE_PLANNED_PATH;
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(computed_arrival_time_+2.0));
if (!finished_before_timeout_) {
ROS_WARN("did not complete move in expected time");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("move did not return success; code = %d",cart_result_.return_code);
return (int) cart_result_.return_code;
}
ROS_INFO("move returned success");
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:17,代码来源:table_wiper.cpp
示例12: turn
void turn() {
turtlebot_actions::TurtlebotMoveGoal goal;
goal.forward_distance = 0;
goal.turn_distance = M_PI;
action_client.waitForServer();
action_client.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = action_client.waitForResult(
ros::Duration(30.0));
if (finished_before_timeout) {
actionlib::SimpleClientGoalState state = action_client.getState();
ROS_INFO("Action finished: %s", state.toString().c_str());
} else
ROS_INFO("Action did not finish before the time out.");
}
开发者ID:Aerobota,项目名称:rapyuta-mapping,代码行数:18,代码来源:test_keyframe_extraction.cpp
示例13: rt_arm_request_q_data
//send goal command to request right-arm joint angles; these will be stored in internal variable
int ArmMotionCommander::rt_arm_request_q_data(void) {
ROS_INFO("requesting right-arm joint angles");
cart_goal_.command_code = cwru_action::cwru_baxter_cart_moveGoal::RT_ARM_GET_Q_DATA;
cart_move_action_client_.sendGoal(cart_goal_, boost::bind(&ArmMotionCommander::doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = cart_move_action_client_.waitForResult(ros::Duration(computed_arrival_time_+2.0));
if (!finished_before_timeout_) {
ROS_WARN("did not respond within timeout");
return (int) cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
}
if (cart_result_.return_code!=cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
ROS_WARN("move did not return success; code = %d",cart_result_.return_code);
return (int) cart_result_.return_code;
}
q_vec_ = cart_result_.q_arm_right;
ROS_INFO("move returned success; right arm angles: ");
ROS_INFO("%f; %f; %f; %f; %f; %f; %f",q_vec_[0],q_vec_[1],q_vec_[2],q_vec_[3],q_vec_[4],q_vec_[5],q_vec_[6]);
return (int) cart_result_.return_code;
}
开发者ID:mwswartwout,项目名称:table_wiper,代码行数:20,代码来源:table_wiper.cpp
示例14: preemptCb
/**
* @brief Preempt callback for the server, cancels the current running goal and all associated movement actions.
*/
void preemptCb(){
boost::unique_lock<boost::mutex> lock(move_client_lock_);
move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
ROS_WARN("Current exploration task cancelled");
if(as_.isActive()){
as_.setPreempted();
}
}
开发者ID:LuisServin,项目名称:catkin_ws,代码行数:14,代码来源:explore_server.cpp
示例15: movePreemptCB
void movePreemptCB()
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested");
if (planning_)
{
ROS_DEBUG_NAMED(logger_name_, "Planning - cancelling all plan goals");
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
if (controlling_)
{
ROS_DEBUG_NAMED(logger_name_, "Controlling - cancelling all ctrl goals");
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
move_result_.result_state = 0;
move_result_.error_string = "Move Platform Preempt Request!";
as_.setPreempted(move_result_);
set_terminal_state_ = false;
return;
}
开发者ID:inesc-tec-robotics,项目名称:carlos_motion,代码行数:22,代码来源:move_platform_server.cpp
示例16: pickAndPlace
void pickAndPlace(const actionlib::SimpleClientGoalState& state, const InteractiveBlockManipulationResultConstPtr& result)
{
ROS_INFO("Got interactive marker callback. Picking and placing.");
if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Succeeded!");
else
{
ROS_INFO("Did not succeed! %s", state.toString().c_str());
ros::shutdown();
}
pick_and_place_action_.sendGoal(pick_and_place_goal_, boost::bind( &BlockManipulationAction::finish, this, _1, _2));
}
开发者ID:scottmishra,项目名称:clam,代码行数:13,代码来源:block_manipulation_action_demo.cpp
示例17: addBlocks
void addBlocks(const actionlib::SimpleClientGoalState& state, const BlockDetectionResultConstPtr& result)
{
ROS_INFO("Got block detection callback. Adding blocks.");
geometry_msgs::Pose block;
if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Succeeded!");
else
{
ROS_INFO("Did not succeed! %s", state.toString().c_str());
ros::shutdown();
}
interactive_manipulation_action_.sendGoal(interactive_manipulation_goal_, boost::bind( &BlockManipulationAction::pickAndPlace, this, _1, _2));
}
开发者ID:scottmishra,项目名称:clam,代码行数:14,代码来源:block_manipulation_action_demo.cpp
示例18: execute_planned_move
void ArmMotionInterface::execute_planned_move(void) {
if (!path_is_valid_) {
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::RT_ARM_PATH_NOT_VALID;
ROS_WARN("attempted to execute invalid path!");
cart_move_as_.setAborted(cart_result_); // tell the client we have given up on this goal; send the result message as well
}
// convert path to a trajectory:
//baxter_traj_streamer_.stuff_trajectory(optimal_path_, des_trajectory_); //convert from vector of poses to trajectory message
js_goal_.trajectory = des_trajectory_;
//computed_arrival_time_ = des_trajectory_.points.back().time_from_start.toSec();
ROS_INFO("sending action request to traj streamer node");
ROS_INFO("computed arrival time is %f", computed_arrival_time_);
busy_working_on_a_request_ = true;
traj_streamer_action_client_.sendGoal(js_goal_, boost::bind(&ArmMotionInterface::js_doneCb_, this, _1, _2)); // we could also name additional callback functions here, if desired
finished_before_timeout_ = traj_streamer_action_client_.waitForResult(ros::Duration(computed_arrival_time_ + 2.0));
if (!finished_before_timeout_) {
ROS_WARN("EXECUTE_PLANNED_PATH: giving up waiting on result");
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::NOT_FINISHED_BEFORE_TIMEOUT;
cart_move_as_.setSucceeded(cart_result_); //could say "aborted"
} else {
ROS_INFO("finished before timeout");
cart_result_.return_code = cwru_action::cwru_baxter_cart_moveResult::SUCCESS;
cart_move_as_.setSucceeded(cart_result_);
}
path_is_valid_ = false; // reset--require new path before next move
busy_working_on_a_request_ = false;
//save the last point commanded, for future reference
std::vector <double> last_pt;
last_pt = des_trajectory_.points.back().positions;
int njnts = last_pt.size();
for (int i=0;i<njnts;i++) {
last_arm_jnt_cmd_right_[i] = last_pt[i];
}
}
开发者ID:ShaunHoward,项目名称:cwru_baxter,代码行数:37,代码来源:baxter_cart_move_as.cpp
示例19: move_straight
void move_straight() {
float current_angle = 0;
float desired_angle = 0;
/*
if (map->frames.size() > 0) {
Sophus::SE3f position =
map->frames[map->frames.size() - 1]->get_pos();
Eigen::Vector3f current_heading = position.unit_quaternion()
* Eigen::Vector3f::UnitZ();
current_angle = std::atan2(-current_heading(1), current_heading(0));
desired_angle = std::asin(position.translation()(1)/10.0);
}*/
turtlebot_actions::TurtlebotMoveGoal goal;
goal.forward_distance = 25.0;
goal.turn_distance = current_angle - desired_angle;
action_client.waitForServer();
action_client.sendGoal(goal);
//wait for the action to return
bool finished_before_timeout = action_client.waitForResult(
ros::Duration(500.0));
if (finished_before_timeout) {
actionlib::SimpleClientGoalState state = action_client.getState();
ROS_INFO("Action finished: %s", state.toString().c_str());
} else
ROS_INFO("Action did not finish before the time out.");
map->save("corridor_map");
}
开发者ID:Aerobota,项目名称:rapyuta-mapping,代码行数:36,代码来源:test_keyframe_extraction.cpp
示例20: finish
void finish(const actionlib::SimpleClientGoalState& state, const PickAndPlaceResultConstPtr& result)
{
ROS_INFO("Got pick and place callback. Finished!");
if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Succeeded!");
else
ROS_INFO("Did not succeed! %s", state.toString().c_str());
reset_arm_action_.sendGoal(simple_arm_actions::ResetArmGoal());
if (once)
ros::shutdown();
else
detectBlocks();
}
开发者ID:scottmishra,项目名称:clam,代码行数:15,代码来源:block_manipulation_action_demo.cpp
注:本文中的actionlib::SimpleActionClient类示例由纯净天空整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。 |
请发表评论