// Move the real block!
void feedbackCb( const InteractiveMarkerFeedbackConstPtr &feedback )
{
if (!action_server_.isActive())
{
ROS_INFO("[block logic] Got feedback but not active!");
return;
}
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
ROS_INFO_STREAM("[block logic] Staging " << feedback->marker_name);
old_pose_ = feedback->pose;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
ROS_INFO_STREAM("[block logic] Now moving " << feedback->marker_name);
moveBlock(old_pose_, feedback->pose);
break;
}
interactive_m_server_.applyChanges();
}
void jointStateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
size_t msgSize = msg->name.size(), jointInfoSize = _jointInfo.name.size(), msgSizeP = msg->position.size(),
msgSizeV = msg->velocity.size(), msgSizeE = msg->effort.size();
for (int i = 0; i < msgSize; i++) {
for (int j = 0; j < jointInfoSize; j++) {
if (msg->name[i] == _jointInfo.name[j]) {
if (i < msgSizeP) _jointInfo.position[j] = msg->position[i];
if (i < msgSizeV) _jointInfo.velocity[j] = msg->velocity[i];
if (i < msgSizeE) _jointInfo.effort[j] = msg->effort[i];
}
}
}
if(_publishFeedback) {
size_t size = _feedback.joint_names.size();
_feedback.actual.positions.clear();
_feedback.actual.velocities.clear();
_feedback.actual.effort.clear();
_feedback.error.positions.clear();
_feedback.error.velocities.clear();
_feedback.error.effort.clear();
for (int i = 0; i < size; ++i) {
feedback_t singleJointFeedback = getFeedback(_feedback.joint_names[i], _feedback.desired.positions[i],
0.0,
_feedback.desired.velocities[i]);
_feedback.actual.positions.push_back(singleJointFeedback.actual.position);
_feedback.actual.velocities.push_back(singleJointFeedback.actual.velocity);
_feedback.actual.effort.push_back(singleJointFeedback.actual.effort);
_feedback.error.positions.push_back(singleJointFeedback.error.position);
_feedback.error.velocities.push_back(singleJointFeedback.error.velocity);
_feedback.error.effort.push_back(singleJointFeedback.error.effort);
}
_actionServer.publishFeedback(_feedback);
}
}
Node()
: private_nh("~")
, actionserver(nh, "moveto", false)
, disabled(false)
, kill_listener(nh, "kill")
, waypoint_validity_(nh)
{
// Make sure alarm integration is ok
kill_listener.waitForUpdate(ros::Duration(10));
if (kill_listener.getNumConnections() < 1)
throw std::runtime_error("The kill listener isn't connected to the alarm server");
kill_listener.start(); // F**k.
fixed_frame = mil_tools::getParam<std::string>(private_nh, "fixed_frame");
body_frame = mil_tools::getParam<std::string>(private_nh, "body_frame");
limits.vmin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmin_b");
limits.vmax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "vmax_b");
limits.amin_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amin_b");
limits.amax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "amax_b");
limits.arevoffset_b = mil_tools::getParam<Eigen::Vector3d>(private_nh, "arevoffset_b");
limits.umax_b = mil_tools::getParam<subjugator::Vector6d>(private_nh, "umax_b");
traj_dt = mil_tools::getParam<ros::Duration>(private_nh, "traj_dt", ros::Duration(0.0001));
waypoint_check_ = mil_tools::getParam<bool>(private_nh, "waypoint_check");
odom_sub = nh.subscribe<Odometry>("odom", 1, boost::bind(&Node::odom_callback, this, _1));
trajectory_pub = nh.advertise<PoseTwistStamped>("trajectory", 1);
trajectory_vis_pub = private_nh.advertise<PoseStamped>("trajectory_v", 1);
waypoint_pose_pub = private_nh.advertise<PoseStamped>("waypoint", 1);
update_timer = nh.createTimer(ros::Duration(1. / 50), boost::bind(&Node::timer_callback, this, _1));
actionserver.start();
set_disabled_service = private_nh.advertiseService<SetDisabledRequest, SetDisabledResponse>(
"set_disabled", boost::bind(&Node::set_disabled, this, _1, _2));
}
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;
}
void executeCB(const pr2_controllers_msgs::JointTrajectoryGoalConstPtr &goal) {
if(isInitialized_) {
ROS_INFO("Received new goal trajectory with %d points",goal->trajectory.points.size());
// saving goal into local variables
traj_ = goal->trajectory;
traj_point_nr_ = 0;
traj_point_ = traj_.points[traj_point_nr_];
finished_ = false;
// stoping axis to prepare for new trajectory
CamAxis_->Stop();
// check that preempt has not been requested by the client
if (as_.isPreemptRequested())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
usleep(2000000); // needed sleep until powercubes starts to change status from idle to moving
while(finished_ == false)
{
if (as_.isNewGoalAvailable())
{
ROS_WARN("%s: Aborted", action_name_.c_str());
as_.setAborted();
return;
}
usleep(10000);
//feedback_ =
//as_.send feedback_
}
// set the action state to succeed
//result_.result.data = "executing trajectory";
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
} else {
as_.setAborted();
ROS_WARN("Camera_axis not initialized yet!");
}
}
FaceRecognition(std::string name) :
_as(_nh, name, boost::bind(&FaceRecognition::executeCB, this, _1), false),
_it(_nh),
_ph("~"),
_action_name(name),
_fd(),
_fr(NULL),
_fc(NULL),
_windowName("FaceRec"),
_window_rows(0),
_window_cols(0)
{
_as.start();
// Subscrive to input video feed and publish output video feed
// _image_sub = _it.subscribe("/camera/image_raw", 1, &FaceRecognition::imageCb, this);
// _image_sub = _it.subscribe("/usb_cam/image_raw", 1, &FaceRecognition::imageCb, this);
// Get params
_ph.param<std::string>("algorithm", _recognitionAlgo , "lbph");
_ph.param<int>("no_training_images", _noTrainingImgs , 20);
_ph.param<int>("max_faces", _maxFaces , 2);
_ph.param<bool>("display_window", _displayWindow, true);
_ph.param<string>("preprocessing", _preprocessing, "tantriggs");
try {
_fr = new FaceRec(true, _preprocessing);
ROS_INFO("Using %s for recognition.", _recognitionAlgo.c_str());
ROS_INFO("Using %s for preprocessing.", _preprocessing.c_str());
}
catch( cv::Exception& e ) {
const char* err_msg = e.what();
ROS_INFO("%s", err_msg);
ROS_INFO("Only ADD_IMAGES mode will work. Please add more subjects for recognition.");
}
cv::namedWindow(_windowName);
}
GoToSelectedBall(std::string name) :
as_(nh_, name, boost::bind(&GoToSelectedBall::executeCB, this, _1), false),
action_name_(name),
ac("move_base", true)
{
selected_ball_sub_ = nh_.subscribe < geometry_msgs::Point > ("/one_selected_ball", 1, &GoToSelectedBall::selectedBallCb, this);
hoover_state_pub_ = nh_.advertise<std_msgs::Int16> ("hoover_state",1);
go_forward_robot_pub_ = nh_.advertise< std_msgs::Float32>("/robot_go_straight",1);
go_forward_robot_state_sub_ = nh_.subscribe("/robot_go_straight_state", 1, &GoToSelectedBall::robotGoStraightStateCb, this);
alghoritm_state_pub_ = nh_.advertise<std_msgs::String> ("/alghoritm_state",1);
deadlock_service_state_sub = nh_.subscribe("/deadlock_service_state", 1, &GoToSelectedBall::deadlockServiceStateCb, this);
firstGoalSent = false;
isBallPoseSet = false;
moveStraightState = 0;
moveStraightStateChange = false;
is_deadlock_service_run = false;
as_.start();
state_ = STOP;
}
WaitUntilUnblockedAction (std::string name) :
as_(nh_, name, boost::bind(&WaitUntilUnblockedAction::executeCB, this, _1), false),
action_name_(name)
{
// Point head
//Initialize the client for the Action interface to the head controller
point_head_client_ = new
PointHeadClient("/head_traj_controller/point_head_action", true);
//wait for head controller action server to come up
while(!point_head_client_->waitForServer(ros::Duration(5.0)))
{
ROS_INFO("Waiting for the point_head_action server to come up");
}
// human detection
activated = false;
human_detected = false;
obstacle_detected = false;
marker_pub = nh_.advertise<visualization_msgs::Marker>("obstacle_boundingbox", 1);
as_.start();
}
void executeCB(const robot_actionlib::TestGoalConstPtr &goal)
{
// helper variables
ros::Rate loop_rate(25);
bool success = true;
// push_back the seeds for the fibonacci sequence
feedback_.sequence.clear();
feedback_.sequence.push_back(0);
feedback_.sequence.push_back(1);
// publish info to the console for the user
ROS_INFO("%s: Executing, creating fibonacci sequence of order %i with seeds %i, %i", action_name_.c_str(), goal->order, feedback_.sequence[0], feedback_.sequence[1]);
bool front = 0;
bool right = 0;
bool left = 0;
double right_average = 0;
double left_average = 0;
while(ros::ok())
{
ROS_INFO("Preempted ?: %d ", as_.isPreemptRequested());
ROS_INFO("Active ?: %d ", as_.isActive());
ROS_INFO("GOOALLLLLL: %d ", goal->order);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
success = false;
break;
}
front = (distance_front_left < front_limit) || (distance_front_right < front_limit);
right = (distance_rights_front < side_limit) && (distance_rights_back < side_limit);
left = (distance_lefts_front < side_limit) && (distance_lefts_back < side_limit);
right_average = (distance_rights_front + distance_rights_back)/2;
left_average = (distance_lefts_front + distance_lefts_back)/2;
flagReady = true;
if (!TestAction::flagReady) {
ros::spinOnce();
loop_rate.sleep();
continue;
}
// Update of the state
if(front){ // The front is the most prior
if(right_average > left_average){
side = 0;
}else{
side = 1;
}
state = FRONT;
std::cerr << "FRONT" << std::endl;
} else if (right) { // The right is prefered
/*if(state != RIGHT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
state = RIGHT;
std::cerr << "RIGHT" << std::endl;
} else if (left) {
/*if(state != LEFT){
direction = rand()%2;
std::cerr << direction << std::endl;
}*/
//direction = rand()%1;
state = LEFT;
std::cerr << "LEFT" << std::endl;
} else {
state = EMPTY;
std::cerr << "EMPTY" << std::endl;
}
// Action depending on the state
switch(state) {
case(EMPTY): // Just go straight
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = smoothUpdateVelocity(msg.angular.z, 0, 0.1);
break;
case(FRONT):
msg.linear.x = 0;
if(side == 1){
msg.angular.z = smoothUpdateVelocity(msg.angular.z, a_speed, 0.1);
}else{
msg.angular.z = smoothUpdateVelocity(msg.angular.z, -a_speed, 0.1);
}
break;
case(RIGHT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = - alpha * ( (double)distance_rights_front - (double)distance_rights_back);
break;
case(LEFT):
msg.linear.x = smoothUpdateVelocity(msg.linear.x, l_speed, 0.05);
msg.angular.z = alpha * ( (double)distance_lefts_front - (double)distance_lefts_back);
break;
default:
break;
}
// P-controller to generate the angular velocity of the robot, to follow the wall
//msg.angular.z = alpha * ( (double)distance_lefts_front- (double)distance_lefts_back);
//ROS_INFO("Linear velocity :%f", msg.linear.x);
//ROS_INFO("Angular velocity :%f", msg.angular.z);
if(counter > 10){
twist_pub.publish(msg);
}else {
counter += 1;
}
feedback_.sequence.push_back(feedback_.sequence[1] + feedback_.sequence[0]);
// publish the feedback
as_.publishFeedback(feedback_);
ros::spinOnce();
loop_rate.sleep();
}
if(success)
{
result_.sequence = feedback_.sequence;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
}
void goalCB()
{
// accept the new goal
has_goal_ = as_.acceptNewGoal()->approach;
}
void trackPointCB(const geometry_msgs::Point::ConstPtr& msg)
{
// make sure that the action hasn't been canceled
if (!as_.isActive())
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( !has_goal_)
{
twist_.angular.z = 0;
twist_.linear.x = 0;
twist_pub_.publish(twist_);
return;
}
if( obstacle_detected_ && msg->y < SVTIUS)
{
result_.success = false;
ROS_INFO("%s: Obsticle Detected", action_name_.c_str());
// set the action state to succeeded
as_.setSucceeded(result_);
}
feedback_.success = false;
twist_.angular.z = 0;
twist_.linear.x = 0;
if ( msg->y >= TOP_T
&& msg->x <= RIGHT_T
&& msg->x >= LEFT_T )
{
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
as_.setSucceeded(result_);
}
if(drop_flag) {
twist_pub_.publish(twist_);
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
result_.success = true;
drop_flag = false;
as_.setSucceeded(result_);
}
if (msg->y < TOP_T)
{
ROS_INFO("MOVE FORWARD");
twist_.linear.x = getLinVel(msg->y, TOP_T);
}
if (msg->x < LEFT_T)
{
ROS_INFO("TURN RIGHT");
twist_.angular.z = getAngVel(msg->x, LEFT_T);
}
else if (msg->x > RIGHT_T)
{
ROS_INFO("TURN LEFT");
twist_.angular.z = -1.0 * getAngVel(msg->x, RIGHT_T);
}
twist_pub_.publish(twist_);
as_.publishFeedback(feedback_);
}
/*!
* \brief Run the controller.
*
* The Controller generates desired velocities for every joints from the current positions.
*
*/
void run()
{
if(executing_)
{
watchdog_counter = 0;
if (as_.isPreemptRequested() || !ros::ok() || current_operation_mode_ != "velocity")
{
/// set the action state to preempted
executing_ = false;
traj_generator_->isMoving = false;
ROS_INFO("Preempted trajectory action");
return;
}
std::vector<double> des_vel;
if(traj_generator_->step(q_current, des_vel))
{
if(!traj_generator_->isMoving) //Trajectory is finished
{
executing_ = false;
}
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for(unsigned int i=0; i<7; i++)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = des_vel.at(i);
}
/// send everything
joint_vel_pub_.publish(target_joint_vel);
}
else
{
ROS_INFO("An controller error occured!");
executing_ = false;
}
}
else
{
/// WATCHDOG TODO: don't always send
if(watchdog_counter < 10)
{
sensor_msgs::JointState target_joint_position;
target_joint_position.position.resize(7);
brics_actuator::JointVelocities target_joint_vel;
target_joint_vel.velocities.resize(7);
for (unsigned int i = 0; i < 7; i += 1)
{
std::stringstream joint_name;
joint_name << "arm_" << (i+1) << "_joint";
target_joint_vel.velocities[i].joint_uri = joint_name.str();
target_joint_position.position[i] = 0;
target_joint_vel.velocities[i].unit = "rad";
target_joint_vel.velocities[i].value = 0;
}
joint_vel_pub_.publish(target_joint_vel);
joint_pos_pub_.publish(target_joint_position);
}
watchdog_counter++;
}
}
bool isActionServerActive(){return as_.isActive();};
// Constructor
PickPlaceMoveItServer(const std::string name):
nh_("~"),
movegroup_action_("pickup", true),
clam_arm_client_("clam_arm", true),
ee_marker_is_loaded_(false),
block_published_(false),
action_server_(name, false)
{
base_link_ = "/base_link";
// -----------------------------------------------------------------------------------------------
// Adding collision objects
collision_obj_pub_ = nh_.advertise<moveit_msgs::CollisionObject>(COLLISION_TOPIC, 1);
// -----------------------------------------------------------------------------------------------
// Connect to move_group/Pickup action server
while(!movegroup_action_.waitForServer(ros::Duration(4.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the move_group/Pickup action server");
}
// ---------------------------------------------------------------------------------------------
// Connect to ClamArm action server
while(!clam_arm_client_.waitForServer(ros::Duration(5.0))){ // wait for server to start
ROS_INFO_STREAM_NAMED("pick_place_moveit","Waiting for the clam_arm action server");
}
// ---------------------------------------------------------------------------------------------
// Create planning scene monitor
planning_scene_monitor_.reset(new planning_scene_monitor::PlanningSceneMonitor(ROBOT_DESCRIPTION));
if (planning_scene_monitor_->getPlanningScene())
{
//planning_scene_monitor_->startWorldGeometryMonitor();
//planning_scene_monitor_->startSceneMonitor("/move_group/monitored_planning_scene");
//planning_scene_monitor_->startStateMonitor("/joint_states", "/attached_collision_object");
}
else
{
ROS_FATAL_STREAM_NAMED("pick_place_moveit","Planning scene not configured");
}
// ---------------------------------------------------------------------------------------------
// Load the Robot Viz Tools for publishing to Rviz
rviz_tools_.reset(new block_grasp_generator::RobotVizTools(RVIZ_MARKER_TOPIC, EE_GROUP, PLANNING_GROUP_NAME, base_link_, planning_scene_monitor_));
// ---------------------------------------------------------------------------------------------
// Register the goal and preempt callbacks
action_server_.registerGoalCallback(boost::bind(&PickPlaceMoveItServer::goalCB, this));
action_server_.registerPreemptCallback(boost::bind(&PickPlaceMoveItServer::preemptCB, this));
action_server_.start();
// Announce state
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Server ready.");
ROS_INFO_STREAM_NAMED("pick_place_moveit", "Waiting for pick command...");
// ---------------------------------------------------------------------------------------------
// Send home
ROS_INFO_STREAM_NAMED("pick_place_moveit","Sending home");
clam_arm_goal_.command = clam_msgs::ClamArmGoal::RESET;
clam_arm_client_.sendGoal(clam_arm_goal_);
while(!clam_arm_client_.getState().isDone() && ros::ok())
ros::Duration(0.1).sleep();
// ---------------------------------------------------------------------------------------------
// Send fake command
fake_goalCB();
}
//this is where the bulk of the work is done, interpolating between potentially coarse joint-space poses
// using the specified arrival times
void trajActionServer::executeCB(const actionlib::SimpleActionServer<baxter_trajectory_streamer::trajAction>::GoalConstPtr& goal) {
double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
int isegment;
trajectory_msgs::JointTrajectoryPoint trajectory_point0;
Vectorq7x1 qvec, qvec0, qvec_prev, qvec_new;
//ROS_INFO("in executeCB");
//ROS_INFO("goal input is: %d", goal->input);
g_count++; // keep track of total number of goals serviced since this server was started
result_.return_val = g_count; // we'll use the member variable result_, defined in our class
//result_.traj_id = goal->traj_id;
//cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
// copy trajectory to global var:
new_trajectory = goal->trajectory; //
// insist that a traj have at least 2 pts
int npts = new_trajectory.points.size();
if (npts < 2) {
ROS_WARN("too few points; aborting goal");
as_.setAborted(result_);
} else { //OK...have a valid trajectory goal; execute it
//got_new_goal = true;
//got_new_trajectory = true;
ROS_INFO("Cb received traj w/ npts = %d",npts);
//cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
//debug output...
/*
cout << "subgoals: " << endl;
for (int i = 0; i < npts; i++) {
for (int j = 0; j < 7; j++) { //copy from traj point to 7x1 vector
cout << new_trajectory.points[i].positions[j] << ", ";
}
cout << endl;
}
*/
as_.isActive();
working_on_trajectory = true;
traj_clock = 0.0; // initialize clock for trajectory;
isegment = 0; //initialize the segment count
trajectory_point0 = new_trajectory.points[0]; //start trajectory from first point...should be at least close to
//current state of system; SHOULD CHECK THIS
for (int i = 0; i < 7; i++) { //copy from traj point to 7x1 Eigen-type vector
qvec0[i] = trajectory_point0.positions[i];
}
cmd_pose_left(qvec0); //populate and send out first command
qvec_prev = qvec0;
cout << "start pt: " << qvec0.transpose() << endl;
}
while (working_on_trajectory) {
traj_clock += dt_traj;
// update isegment and qvec according to traj_clock;
//if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false
working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
cmd_pose_left(qvec_new); // use qvec to populate object and send it to robot
qvec_prev = qvec_new;
//cout << "traj_clock: " << traj_clock << "; vec:" << qvec_new.transpose() << endl;
ros::spinOnce();
ros::Duration(dt_traj).sleep(); //update the outputs at time-step resolution specified by dt_traj
}
ROS_INFO("completed execution of a trajectory" );
as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message
}
void executeCB(const hector_quadrotor_msgs::followerGoalConstPtr &goal)
{
ROS_INFO("Client is registered, lets start the executing");
publisher = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
gms_c = nh_.serviceClient<gazebo_msgs::GetModelState>("/gazebo/get_model_state");
// smsl = m.serviceClient<gazebo_msgs::SetModelState>("/gazebo/set_model_state");
getmodelstate.request.model_name="quadrotor";
geometry_msgs::Twist tw;
publisher.publish(tw);
ros::Duration(5.0).sleep();
gms_c.call(getmodelstate);
double now_x = getmodelstate.response.pose.position.x;
double now_y = getmodelstate.response.pose.position.y;
double now_z = getmodelstate.response.pose.position.z;
double new_x = goal->goal.position.x;
double new_y = goal->goal.position.y;
double new_z = goal->goal.position.z;
double x_diff, y_diff, z_diff;
double tmp_x, tmp_y, tmp_z;
double var_x, var_y, var_z;
ros::Rate r(1);
bool success = true;
// publish info to the console for the user
ROS_INFO("%s: Executing!", action_name_.c_str());
if (action_.isPreemptRequested() || !ros::ok())
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
action_.setPreempted();
success = false;
//break;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
x_diff=new_x - now_x;
y_diff=new_y - now_y;
z_diff=new_z - now_z;
// if(now_z < 0)
// {
// now_z=now_z*(-1);
// z_diff=new_z - now_z;
// z_diff=z_diff*(-1);
// }else
// z_diff = new_z - now_z;
// tmp_x= modf(new_x, &var_x);
// tmp_y= modf(new_y, &var_y);
// tmp_z= modf(new_z, &var_z);
ROS_INFO(" Come Up Hector! ");
if(now_z < 0.5)
{
tw.linear.z = 0.6;
}
else
{
tw.linear.z = 0.0;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
// tw.angular.z = -0.5;
tw.linear.z = 0;
tw.linear.x = 0;
tw.linear.y = 0;
// publisher.publish(tw);
// ros::Duration(2.0).sleep();
// tw.angular.z = 0;
publisher.publish(tw);
ros::Duration(2.0).sleep();
ROS_INFO(" Good Boy, let's go further! ");
while( x_diff < -0.3 || x_diff > 0.3 || y_diff < -0.3 || y_diff > 0.3)
{
if(now_x < var_x || now_y < var_y)
{
tw.linear.x = x_diff * 0.1;
tw.linear.y = y_diff * 0.1;
}else
if(now_x > var_x || now_y > var_y)
{
tw.linear.x =x_diff * 0.1;
tw.linear.y =y_diff * 0.1;
}
publisher.publish(tw);
ros::Duration(2.0).sleep();
gms_c.call(getmodelstate);
now_y = getmodelstate.response.pose.position.y;
y_diff=new_y - now_y;
now_x = getmodelstate.response.pose.position.x;
x_diff=new_x - now_x;
}
ros::Duration(2.0).sleep();
tw.linear.z = 0;
tw.linear.x = 0;
tw.linear.y = 0;
// tw.angular.z = -0.5;
publisher.publish(tw);
ros::Duration(5.0).sleep();
// tw.angular.z = 0;
// publisher.publish(tw);
// ros::Duration(5.0).sleep();
action_.publishFeedback(feedback_);
// this sleep is not necessary, the sequence is computed at 1 }
if(success)
{
result_.result = feedback_.feedback;
ROS_INFO("%s: Succeeded", action_name_.c_str());
// set the action state to succeeded
action_.setSucceeded(result_);
}}
// Do stuff with learned models
// Phase - Reach, Roll or Back?
// Dynamics type - to select the type of master/slave dynamics (linear/model etc.)
// reachingThreshold - you know
// model_dt - you know
bool learned_model_execution(DoughTaskPhase phase, CDSController::DynamicsType masterType, CDSController::DynamicsType slaveType, double reachingThreshold, double model_dt, tf::Transform trans_obj, tf::Transform trans_att, std::string model_base_path, std::string force_gmm_id) {
ROS_INFO_STREAM(" Model Path "<<model_base_path);
ROS_INFO_STREAM(" Learned model execution with phase "<<phase);
ROS_INFO_STREAM(" Reaching threshold "<<reachingThreshold);
ROS_INFO_STREAM(" Model DT "<<model_dt);
if (force_gmm_id!="")
ROS_INFO_STREAM(" Force GMM ID: "<< force_gmm_id);
ros::Rate wait(1);
tf::Transform trans_final_target, ee_pos_att;
// To Visualize EE Frames
static tf::TransformBroadcaster br;
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
br.sendTransform(tf::StampedTransform(trans_obj, ros::Time::now(), world_frame, "/object_frame"));
// convert attractor information to world frame
trans_final_target.mult(trans_obj, trans_att);
ROS_INFO_STREAM("Final target origin "<<trans_final_target.getOrigin().getX()<<","<<trans_final_target.getOrigin().getY()<<","<<trans_final_target.getOrigin().getZ());
ROS_INFO_STREAM("Final target orient "<<trans_final_target.getRotation().getX()<<","<<trans_final_target.getRotation().getY()<<","<<trans_final_target.getRotation().getZ()<<","<<trans_final_target.getRotation().getW());
// Publish attractors if running in simulation or with fixed values
if ((action_mode == ACTION_LASA_FIXED) || (action_mode == ACTION_BOXY_FIXED)) {
br.sendTransform(tf::StampedTransform(trans_final_target, ros::Time::now(), world_frame, "/attractor"));
}
// Initialize CDS
CDSExecution *cdsRun = new CDSExecution;
cdsRun->initSimple(model_base_path, phase, force_gmm_id);
cdsRun->setObjectFrame(toMatrix4(trans_obj));
cdsRun->setAttractorFrame(toMatrix4(trans_att));
cdsRun->setCurrentEEPose(toMatrix4(ee_pose));
cdsRun->setDT(model_dt);
if (phase==PHASEBACK || phase==PHASEROLL)
masterType = CDSController::LINEAR_DYNAMICS;
// Roll slow but move fast for reaching and back phases.
// If models have proper speed, this whole block can go!
if(phase == PHASEROLL) {
//cdsRun->setMotionParameters(1,1,0.5,reachingThreshold, masterType, slaveType);
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
// large threshold to avoid blocking forever
// TODO: should rely on preempt in action client.
// reachingThreshold = 0.02;
reachingThreshold = 0.025;
} else {
cdsRun->setMotionParameters(1,1,2,reachingThreshold, masterType, slaveType);
}
cdsRun->postInit();
// If phase is rolling, need force model as well
GMR* gmr_perr_force = NULL;
if(phase == PHASEROLL) {
gmr_perr_force = getNewGMRMappingModel(model_base_path, force_gmm_id);
if(!gmr_perr_force) {
ROS_ERROR("Cannot initialize GMR model");
return false;
}
}
ros::Duration loop_rate(model_dt);
tf::Pose mNextRobotEEPose = ee_pose;
std::vector<double> gmr_in, gmr_out;
gmr_in.resize(1);gmr_out.resize(1);
double prog_curr, full_err, pos_err, ori_err, new_err, gmr_err;
tf::Vector3 att_t, curr_t;
ROS_INFO("Execution started");
tf::Pose p;
bool bfirst = true;
std_msgs::String string_msg, action_name_msg, model_fname_msg;
std::stringstream ss, ss_model;
if(phase == PHASEREACH) {
ss << "reach ";
}
else if (phase == PHASEROLL){
ss << "roll";
}
else if (phase == PHASEBACK){
ss << "back";
}
if (!homing){
string_msg.data = ss.str();
pub_action_state_.publish(string_msg);
// ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
if (force_gmm_id=="")
ss_model << path_matlab_plot << "/Phase" << phase << "/masterGMM.fig";
else
ss_model << path_matlab_plot << "/Phase" << phase << "/ForceProfile_" << force_gmm_id << ".fig";
//ss_model << "/Phase" << phase << "/masterGMM.fig";
model_fname_msg.data = ss_model.str();
pub_model_fname_.publish(model_fname_msg);
action_name_msg.data = ss.str();
pub_action_name_.publish(action_name_msg);
plot_dyn = 1;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
}
while(ros::ok()) {
if (!homing)
plot_dyn = 1;
else
plot_dyn = 0;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
// Nadia's stuff
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
pos_err = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
//Real Orientation Error qdiff = acos(dot(q1_norm,q2_norm))*180/pi
ori_err = acos(abs(trans_final_target.getRotation().dot(ee_pose.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Position Threshold : " << reachingThreshold << " ... Current Error: "<<pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Orientation Threshold : " << 0.01 << " ... Current Error: "<<ori_err);
/*double att_pos_err = (trans_final_target.getOrigin() - p.getOrigin()).length();
double att_ori_err = acos(abs(trans_final_target.getRotation().dot(p.getRotation())));
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Position Error: " << att_pos_err);
ROS_INFO_STREAM_THROTTLE(0.5,"Des-Att Orientation Error: " << att_ori_err);*/
switch (phase) {
// Home, reach and back are the same control-wise
case PHASEREACH:
case PHASEBACK:
// Current progress variable (position/orientation error).
// TODO: send this back to action client as current progress
prog_curr = 0.5*((trans_final_target.getOrigin() - ee_pose.getOrigin()).length() + (trans_final_target.getRotation()-ee_pose.getRotation()).length());
// Compute Next Desired EE Pose
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Aswhini's Hack! Dont rely on model's orientation interpolation. Set it equal to target orientation to avoid
// going the wrong way around
p.setRotation(trans_final_target.getRotation());
//Publish desired force
gmr_msg.data = 0.0;
pub_gmr_out_.publish(gmr_msg);
break;
case PHASEROLL:
// Current progress in rolling phase is simply the position error
prog_curr = (trans_final_target.getOrigin() - ee_pose.getOrigin()).length();
// New position error being fed to GMR Force Model
att_t = tf::Vector3(trans_final_target.getOrigin().getX(),trans_final_target.getOrigin().getY(),0.0);
curr_t = tf::Vector3(ee_pose.getOrigin().getX(),ee_pose.getOrigin().getY(),0.0);
new_err = (att_t - curr_t).length();
gmr_err = new_err;
//Hack! Truncate errors to corresponding models
if ((strncmp(force_gmm_id.c_str(),"first",5)==0) && (new_err > 0.03)){
gmr_err = 0.03;
}
if ((strncmp(force_gmm_id.c_str(),"mid",3)==0) && (new_err > 0.07)){
gmr_err = 0.07;
}
if ((strncmp(force_gmm_id.c_str(),"last",4)==0) && (new_err > 0.13)){
gmr_err = 0.13;
}
//pos_err = prog_curr;
ori_err = 0;
gmr_err = gmr_err;
gmr_in[0] = gmr_err; // distance between EE and attractor
// Query the model for desired force
getGMRResult(gmr_perr_force, gmr_in, gmr_out);
/* double fz_plot;
getGMRResult(gmr_perr_force, -gmr_in, fz_plot);*/
//-> INSTEAD OF SENDING GMR OUTPUT / SEND EST_EE_FT(Z)
// Send fz and distance to attractor for plotting
msg_ft.wrench.force.x = gmr_err;
msg_ft.wrench.force.y = gmr_out[0]; //ee_ft[2]
msg_ft.wrench.force.z = 0;
msg_ft.wrench.torque.x = 0;
msg_ft.wrench.torque.y = 0;
msg_ft.wrench.torque.z = 0;
pub_ee_ft_att_.publish(msg_ft);
// Hack! Scale the force to be in reasonable values
gmr_out[0] = FORCE_SCALING*fabs(gmr_out[0]);
ROS_INFO_STREAM_THROTTLE(0.5,"Distance to Attractor: " << new_err << " GMR output (N): " << gmr_out[0]);
gmr_msg.data = gmr_out[0];
pub_gmr_out_.publish(gmr_msg);
// Hack! Safety first!
if(gmr_out[0] > MAX_ROLLING_FORCE) {
gmr_out[0] = MAX_ROLLING_FORCE;
}
// Give some time for the force to catch up the first time. Then roll with constant force thereafter.
if(bfirst) {
sendAndWaitForNormalForce(-gmr_out[0]);
bfirst = false;
} else {
sendNormalForce(-gmr_out[0]);
}
ROS_INFO_STREAM_THROTTLE(0.5, "Force applied: "<<gmr_out[0]);
cdsRun->setCurrentEEPose(toMatrix4(mNextRobotEEPose));
toPose(cdsRun->getNextEEPose(), mNextRobotEEPose);
p = mNextRobotEEPose;
// Hack! Dont rely on model orientation. Use target orientation instead.
p.setRotation(trans_final_target.getRotation());
// Hack! Dont rely on the Z component of the model. It might go below the table!
p.getOrigin().setZ(trans_final_target.getOrigin().getZ());
homing=false;
break;
default:
ROS_ERROR_STREAM("No such phase defined "<<phase);
return false;
}
// Add rotation of Tool wrt. flange_link for BOXY
/*if (use_boxy_tool){
tf::Matrix3x3 R = p.getBasis();
Eigen::Matrix3d R_ee;
tf::matrixTFToEigen(R,R_ee);
Eigen::Matrix3d R_tool;
R_tool << -0.7071, -0.7071, 0.0,
0.7071,-0.7071, 0.0,
0.0, 0.0, 1.0;
//multiply tool rot
R_ee = R_ee*R_tool;
tf::matrixEigenToTF(R_ee,R);
p.setBasis(R);
}*/
// Send the computed pose from one of the above phases
sendPose(p);
// convert and send ee pose to attractor frame to plots
ee_pos_att.mult(trans_final_target.inverse(), p);
geometry_msgs::PoseStamped msg;
msg.pose.position.x = ee_pos_att.getOrigin().x();
msg.pose.position.y = ee_pos_att.getOrigin().y();
msg.pose.position.z = ee_pos_att.getOrigin().z();
msg.pose.orientation.x = ee_pos_att.getRotation().x();
msg.pose.orientation.y = ee_pos_att.getRotation().y();
msg.pose.orientation.z = ee_pos_att.getRotation().z();
msg.pose.orientation.w = ee_pos_att.getRotation().w();
pub_ee_pos_att_.publish(msg);
//ROS_INFO_STREAM_THROTTLE(0.5,"Error "<<prog_curr);
// check that preempt has not been requested by the client
if (as_.isPreemptRequested() || !ros::ok())
{
sendPose(ee_pose);
sendNormalForce(0);
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
return false;
}
feedback_.progress = prog_curr;
as_.publishFeedback(feedback_);
/* if(prog_curr < reachingThreshold) {
break;
}*/
//Orientation error 0.05
if(pos_err < reachingThreshold && (ori_err < 0.05 || isnan(ori_err))) {
break;
}
loop_rate.sleep();
}
delete cdsRun;
if(phase == PHASEREACH) {
// Hack! If phase is "reach", find the table right after reaching
if (bWaitForForces && !homing) {
bool x = find_table_for_rolling(0.35, 0.05, 5);
// bool x = find_table_for_rolling(0.35, 0.1, 5);
//-> Send command to dynamics plotter to stop logging
return x;
}
if (!homing){
//-> Send command to dynamics plotter to stop logging
}
} else if (phase == PHASEROLL){
// Hack! wait for zero force before getting ready to recieve further commands.
// This is to avoid dragging the dough.
sendAndWaitForNormalForce(0);
//-> Send command to dynamics plotter to start plotting
return ros::ok();
} else {
//->Send command to dynamics plotter to start plotting
return ros::ok();
}
}
void pickAndPlace(const geometry_msgs::Pose& start_pose, const geometry_msgs::Pose& end_pose)
{
ROS_INFO("[pick and place] Picking. Also placing.");
simple_arm_server::MoveArmGoal goal;
simple_arm_server::ArmAction action;
simple_arm_server::ArmAction grip;
/* open gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.move_time.sec = 1.0;
grip.command = gripper_open;
goal.motions.push_back(grip);
/* arm straight up */
tf::Quaternion temp;
temp.setRPY(0,1.57,0);
action.goal.orientation.x = temp.getX();
action.goal.orientation.y = temp.getY();
action.goal.orientation.z = temp.getZ();
action.goal.orientation.w = temp.getW();
/* hover over */
action.goal.position.x = start_pose.position.x;
action.goal.position.y = start_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 0.25;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = start_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* close gripper */
grip.type = simple_arm_server::ArmAction::MOVE_GRIPPER;
grip.command = gripper_closed;
grip.move_time.sec = 1.0;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* hover over */
action.goal.position.x = end_pose.position.x;
action.goal.position.y = end_pose.position.y;
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
/* go down */
action.goal.position.z = end_pose.position.z;
action.move_time.sec = 1.5;
goal.motions.push_back(action);
/* open gripper */
grip.command = gripper_open;
goal.motions.push_back(grip);
/* go up */
action.goal.position.z = z_up;
action.move_time.sec = 1.0;
goal.motions.push_back(action);
goal.header.frame_id = arm_link;
client_.sendGoal(goal);
ROS_INFO("[pick and place] Sent goal. Waiting.");
client_.waitForResult(ros::Duration(30.0));
ROS_INFO("[pick and place] Received result.");
if (client_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
as_.setSucceeded(result_);
else
{
ROS_INFO("Actual state: %s", client_.getState().toString().c_str());
as_.setAborted(result_);
}
}
FibonacciAction(std::string name)
: as_(nh_, name, boost::bind(&FibonacciAction::executeCB, this, _1),
false),
action_name_(name) {
as_.start();
}
void executeActionCB(const OrientToBaseGoalConstPtr& goal) {
BaseScanLinearRegression srv;
double min_angle = -M_PI/8.0, max_angle=M_PI/8.0, min_dist=0.02, max_dist=0.8;
nh_.getParamCached("/laser_linear_regression/min_angle", min_angle);
nh_.getParamCached("/laser_linear_regression/max_angle", max_angle);
nh_.getParamCached("/laser_linear_regression/min_dist", min_dist);
nh_.getParamCached("/laser_linear_regression/max_dist", max_dist);
srv.request.filter_minAngle = angles::from_degrees(min_angle);
srv.request.filter_maxAngle = angles::from_degrees(max_angle);
srv.request.filter_minDistance = min_dist;
srv.request.filter_maxDistance = max_dist;
std::cout << "min_dist " << min_dist << ", max_dist " << max_dist << std::endl;
//srv.request.filter_minAngle = -M_PI / 8.0;
//srv.request.filter_maxAngle = M_PI / 8.0;
//srv.request.filter_minDistance = 0.02;
//srv.request.filter_maxDistance = 0.80;
target_distance = goal->distance;
ros::Duration max_time(50.0);
ros::Time stamp = ros::Time::now();
OrientToBaseResult result;
bool oriented = false;
int iterator = 0;
while (ros::ok()) {
ROS_INFO("Call service Client");
if(client.call(srv)) {
ROS_INFO("Called service LaserScanLinearRegressionService");
//std::cout << "result: " << srv.response.center << ", " << srv.response.a << ", " << srv.response.b << std::endl;
geometry_msgs::Twist cmd = calculateVelocityCommand(srv.response.center, srv.response.a, srv.response.b,oriented,iterator);
cmd_pub.publish(cmd);
std::cout << "cmd x:" << cmd.linear.x << ", y: " << cmd.linear.y << ", z: " << cmd.angular.z << std::endl;
if ((fabs(cmd.angular.z) + fabs(cmd.linear.x) ) < 0.001) {
ROS_INFO("Point reached");
result.succeed = true;
as_.setSucceeded(result);
geometry_msgs::Twist zero_vel;
cmd_pub.publish(zero_vel);
break;
}
if (stamp + max_time < ros::Time::now()) {
result.succeed = false;
as_.setAborted(result);
geometry_msgs::Twist zero_vel;
cmd_pub.publish(zero_vel);
break;
}
} else {
ROS_ERROR("Failed to call service LaserScanLinearRegressionService");
if (stamp + max_time < ros::Time::now()) {
result.succeed = false;
as_.setAborted(result);
break;
}
}
}
}
void CartMoveActionServer::executeCB(const actionlib::SimpleActionServer<cwru_action::cart_moveAction>::GoalConstPtr& goal) {
ROS_INFO("in executeCB of CartMoveActionServer");
cart_result_.err_code=0;
cart_move_as_.isActive();
//unpack the necessary info:
gripper_ang1_ = goal->gripper_jaw_angle1;
gripper_ang2_ = goal->gripper_jaw_angle2;
arrival_time_ = goal->move_time;
// interpret the desired gripper poses:
geometry_msgs::PoseStamped des_pose_gripper1 = goal->des_pose_gripper1;
geometry_msgs::PoseStamped des_pose_gripper2 = goal->des_pose_gripper2;
// convert the above to affine objects:
des_gripper1_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper1.pose);
cout<<"gripper1 desired pose; "<<endl;
cout<<des_gripper1_affine_wrt_lcamera_.linear()<<endl;
cout<<"origin: "<<des_gripper1_affine_wrt_lcamera_.translation().transpose()<<endl;
des_gripper2_affine_wrt_lcamera_ = transformPoseToEigenAffine3d(des_pose_gripper2.pose);
cout<<"gripper2 desired pose; "<<endl;
cout<<des_gripper2_affine_wrt_lcamera_.linear()<<endl;
cout<<"origin: "<<des_gripper2_affine_wrt_lcamera_.translation().transpose()<<endl;
//do IK to convert these to joint angles:
//Eigen::VectorXd q_vec1,q_vec2;
Vectorq7x1 q_vec1,q_vec2;
q_vec1.resize(7);
q_vec2.resize(7);
trajectory_msgs::JointTrajectory des_trajectory; // an empty trajectory
des_trajectory.points.clear(); // can clear components, but not entire trajectory_msgs
des_trajectory.joint_names.clear(); //could put joint names in...but I assume a fixed order and fixed size, so this is unnecessary
// if using wsn's trajectory streamer action server
des_trajectory.header.stamp = ros::Time::now();
trajectory_msgs::JointTrajectoryPoint trajectory_point; //,trajectory_point2;
trajectory_point.positions.resize(14);
ROS_INFO("\n");
ROS_INFO("stored previous command to gripper one: ");
cout<<gripper1_affine_last_commanded_pose_.linear()<<endl;
cout<<"origin: "<<gripper1_affine_last_commanded_pose_.translation().transpose()<<endl;
// first, reiterate previous command:
// this could be easier, if saved previous joint-space trajectory point...
des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*gripper1_affine_last_commanded_pose_; //previous pose
ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
q_vec1 = ik_solver_.get_soln();
q_vec1(6) = last_gripper_ang1_; // include desired gripper opening angle
des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*gripper2_affine_last_commanded_pose_; //previous pose
ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
q_vec2 = ik_solver_.get_soln();
cout<<"q_vec1 of stored pose: "<<endl;
for (int i=0;i<6;i++) {
cout<<q_vec1[i]<<", ";
}
cout<<endl;
q_vec2(6) = last_gripper_ang2_; // include desired gripper opening angle
for (int i=0;i<7;i++) {
trajectory_point.positions[i] = q_vec1(i);
trajectory_point.positions[i+7] = q_vec2(i);
}
cout<<"start traj pt: "<<endl;
for (int i=0;i<14;i++) {
cout<<trajectory_point.positions[i]<<", ";
}
cout<<endl;
trajectory_point.time_from_start = ros::Duration(0.0); // start time set to 0
// PUSH IN THE START POINT:
des_trajectory.points.push_back(trajectory_point);
// compute and append the goal point, in joint space trajectory:
des_gripper_affine1_ = affine_lcamera_to_psm_one_.inverse()*des_gripper1_affine_wrt_lcamera_;
ROS_INFO("desired gripper one location in base frame: ");
cout<<"gripper1 desired pose; "<<endl;
cout<<des_gripper_affine1_.linear()<<endl;
cout<<"origin: "<<des_gripper_affine1_.translation().transpose()<<endl;
ik_solver_.ik_solve(des_gripper_affine1_); //convert desired pose into equiv joint displacements
q_vec1 = ik_solver_.get_soln();
q_vec1(6) = gripper_ang1_; // include desired gripper opening angle
des_gripper_affine2_ = affine_lcamera_to_psm_two_.inverse()*des_gripper2_affine_wrt_lcamera_;
ik_solver_.ik_solve(des_gripper_affine2_); //convert desired pose into equiv joint displacements
q_vec2 = ik_solver_.get_soln();
cout<<"q_vec1 of goal pose: "<<endl;
for (int i=0;i<6;i++) {
cout<<q_vec1[i]<<", ";
}
cout<<endl;
q_vec2(6) = gripper_ang2_;
for (int i=0;i<7;i++) {
trajectory_point.positions[i] = q_vec1(i);
trajectory_point.positions[i+7] = q_vec2(i);
}
trajectory_point.time_from_start = ros::Duration(arrival_time_);
cout<<"goal traj pt: "<<endl;
for (int i=0;i<14;i++) {
cout<<trajectory_point.positions[i]<<", ";
}
cout<<endl;
des_trajectory.points.push_back(trajectory_point);
js_goal_.trajectory = des_trajectory;
// Need boost::bind to pass in the 'this' pointer
// see example: http://library.isr.ist.utl.pt/docs/roswiki/actionlib_tutorials%282f%29Tutorials%282f%29Writing%2820%29a%2820%29Callback%2820%29Based%2820%29Simple%2820%29Action%2820%29Client.html
// ac.sendGoal(goal,
// boost::bind(&MyNode::doneCb, this, _1, _2),
// Client::SimpleActiveCallback(),
// Client::SimpleFeedbackCallback());
js_action_client_.sendGoal(js_goal_, boost::bind(&CartMoveActionServer::js_doneCb_,this,_1,_2)); // we could also name additional callback functions here, if desired
// action_client.sendGoal(goal, &doneCb, &activeCb, &feedbackCb); //alt--more callback funcs possible
double t_timeout=arrival_time_+2.0; //wait 2 sec longer than expected duration of move
bool finished_before_timeout = js_action_client_.waitForResult(ros::Duration(t_timeout));
//bool finished_before_timeout = action_client.waitForResult(); // wait forever...
if (!finished_before_timeout) {
ROS_WARN("joint-space interpolation result is overdue ");
} else {
ROS_INFO("finished before timeout");
}
ROS_INFO("completed callback" );
cart_move_as_.setSucceeded(cart_result_); // tell the client that we were successful acting on the request, and return the "result" message
//let's remember the last pose commanded, so we can use it as start pose for next move
gripper1_affine_last_commanded_pose_ = des_gripper1_affine_wrt_lcamera_;
gripper2_affine_last_commanded_pose_ = des_gripper2_affine_wrt_lcamera_;
//and the jaw opening angles:
last_gripper_ang1_=gripper_ang1_;
last_gripper_ang2_=gripper_ang2_;
}
void Trajectory_Tracker::track_trajectory(const planner::gen_trajGoalConstPtr &goal)
{
ROS_INFO("TRACKING TRAJECTORY");
float x = goal->x;
float y = goal->y;
x = x;
y = y;
if(goal->frame != "robot"){
geometry_msgs::Point goal_point = transform_goal_to_robot(x,y,goal->frame);
x = goal_point.x;
y = goal_point.y;
}
bool following_trajectory = true;
tf::StampedTransform frame_transform = broadcast_new_traj_frame();
current_trajectory.generate_trajectory(x,y);
cout << "generated trajectory" << endl;
ros::Rate loop_rate(10);
tf::StampedTransform transform;
tf::Quaternion q;
ros::Time start_time = ros::Time::now();
float t = 0;
while(t<=(current_trajectory.t_end)+1)
{
if (action_server.isPreemptRequested() || !ros::ok())
{
float u_v = 0;
float u_w = 0;
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
result.success = 0;
action_server.setSucceeded(result);
cout << "CANCELING GOAL" << endl;
return;
}
t = (ros::Time::now()-start_time).toSec();
feedback.time = t;
action_server.publishFeedback(feedback);
Eigen::ArrayXf traj_at_t = current_trajectory.traj_time_lookup(t);
marker_pub.publish(current_trajectory.points);
marker_pub.publish(current_trajectory.line_strip);
// marker_pub.publish(current_trajectory.line_list);
transform.setOrigin( tf::Vector3(traj_at_t(1), traj_at_t(2), 0));
q.setRPY(0, 0, traj_at_t(3));
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "/traj_frame", "/target"));
Eigen::ArrayXf error = get_error();
cout << error << endl;
// TODO: FEEDFORWARD
float u_v = traj_at_t(4);
float u_w = traj_at_t(5);
u_v += Kp_x*error(0);
u_w += Kp_y*error(1) + Kp_w*error(2);
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = u_v;
cmd_vector.y = 0;
cmd_vector.theta = u_w;
cmd_pub.publish(cmd_vector);
loop_rate.sleep();
if(!ros::ok())
{
result.success = 0;
action_server.setSucceeded(result);
return;
}
br.sendTransform(tf::StampedTransform(frame_transform, ros::Time::now(), "/world", "/traj_frame"));
}
geometry_msgs::Pose2D cmd_vector;
cmd_vector.x = 0;
cmd_vector.y = 0;
cmd_vector.theta = 0;
cmd_pub.publish(cmd_vector);
result.success = 1;
action_server.setSucceeded(result);
}
void timer_callback(const ros::TimerEvent &)
{
mil_msgs::MoveToResult actionresult;
// Handle disabled, killed, or no odom before attempting to produce trajectory
std::string err = "";
if (disabled)
err = "c3 disabled";
else if (kill_listener.isRaised())
err = "killed";
else if (!c3trajectory)
err = "no odom";
if (!err.empty())
{
if (c3trajectory)
c3trajectory.reset(); // On revive/enable, wait for odom before station keeping
// Cancel all goals while killed/disabled/no odom
if (actionserver.isNewGoalAvailable())
actionserver.acceptNewGoal();
if (actionserver.isActive())
{
actionresult.error = err;
actionresult.success = false;
actionserver.setAborted(actionresult);
}
return;
}
ros::Time now = ros::Time::now();
auto old_waypoint = current_waypoint;
if (actionserver.isNewGoalAvailable())
{
boost::shared_ptr<const mil_msgs::MoveToGoal> goal = actionserver.acceptNewGoal();
current_waypoint =
subjugator::C3Trajectory::Waypoint(Point_from_PoseTwist(goal->posetwist.pose, goal->posetwist.twist),
goal->speed, !goal->uncoordinated, !goal->blind);
current_waypoint_t = now;
this->linear_tolerance = goal->linear_tolerance;
this->angular_tolerance = goal->angular_tolerance;
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::GREEN);
// Check if waypoint is valid
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult = waypoint_validity_.is_waypoint_valid(
Pose_from_Waypoint(current_waypoint), current_waypoint.do_waypoint_validation);
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(checkWPResult.second);
actionresult.success = checkWPResult.first;
if (checkWPResult.first == false && waypoint_check_) // got a point that we should not move to
{
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(current_waypoint), (int)OGRID_COLOR::RED);
if (checkWPResult.second ==
WAYPOINT_ERROR_TYPE::UNKNOWN) // if unknown, check if there's a huge displacement with the new waypoint
{
auto a_point = Pose_from_Waypoint(current_waypoint);
auto b_point = Pose_from_Waypoint(old_waypoint);
// If moved more than .5m, then don't allow
if (abs(a_point.position.x - b_point.position.x) > .5 || abs(a_point.position.y - b_point.position.y) > .5)
{
ROS_ERROR("can't move there! - need to rotate");
current_waypoint = old_waypoint;
}
}
// if point is occupied, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED)
{
ROS_ERROR("can't move there! - waypoint is occupied");
current_waypoint = old_waypoint;
}
// if point is above water, reject move
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::ABOVE_WATER)
{
ROS_ERROR("can't move there! - waypoint is above water");
current_waypoint = old_waypoint;
}
if (checkWPResult.second == WAYPOINT_ERROR_TYPE::NO_OGRID)
{
ROS_ERROR("WaypointValidity - Did not recieve any ogrid");
}
}
}
if (actionserver.isPreemptRequested())
{
current_waypoint = c3trajectory->getCurrentPoint();
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
// don't try to make output c3 continuous when cancelled - instead stop as quickly as possible
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
}
// Remember the previous trajectory
auto old_trajectory = c3trajectory->getCurrentPoint();
while (c3trajectory_t + traj_dt < now)
{
c3trajectory->update(traj_dt.toSec(), current_waypoint, (c3trajectory_t - current_waypoint_t).toSec());
c3trajectory_t += traj_dt;
}
// Check if we will hit something while in trajectory the new trajectory
geometry_msgs::Pose traj_point; // Convert messages to correct type
auto p = c3trajectory->getCurrentPoint();
traj_point.position = vec2xyz<Point>(p.q.head(3));
quaternionTFToMsg(tf::createQuaternionFromRPY(p.q[3], p.q[4], p.q[5]), traj_point.orientation);
std::pair<bool, WAYPOINT_ERROR_TYPE> checkWPResult =
waypoint_validity_.is_waypoint_valid(Pose_from_Waypoint(p), c3trajectory->do_waypoint_validation);
if (checkWPResult.first == false && checkWPResult.second == WAYPOINT_ERROR_TYPE::OCCUPIED && waypoint_check_)
{ // New trajectory will hit an occupied goal, so reject
ROS_ERROR("can't move there! - bad trajectory");
current_waypoint = old_trajectory;
current_waypoint.do_waypoint_validation = false;
current_waypoint.r.qdot = subjugator::Vector6d::Zero(); // zero velocities
current_waypoint_t = now;
c3trajectory.reset(new subjugator::C3Trajectory(current_waypoint.r, limits));
c3trajectory_t = now;
actionresult.success = false;
actionresult.error = WAYPOINT_ERROR_TO_STRING.at(WAYPOINT_ERROR_TYPE::OCCUPIED_TRAJECTORY);
}
PoseTwistStamped msg;
msg.header.stamp = c3trajectory_t;
msg.header.frame_id = fixed_frame;
msg.posetwist = PoseTwist_from_PointWithAcceleration(c3trajectory->getCurrentPoint());
trajectory_pub.publish(msg);
waypoint_validity_.pub_size_ogrid(Pose_from_Waypoint(c3trajectory->getCurrentPoint()), 200);
PoseStamped msgVis;
msgVis.header = msg.header;
msgVis.pose = msg.posetwist.pose;
trajectory_vis_pub.publish(msgVis);
PoseStamped posemsg;
posemsg.header.stamp = c3trajectory_t;
posemsg.header.frame_id = fixed_frame;
posemsg.pose = Pose_from_Waypoint(current_waypoint);
waypoint_pose_pub.publish(posemsg);
if (actionserver.isActive() &&
c3trajectory->getCurrentPoint().is_approximately(current_waypoint.r, max(1e-3, linear_tolerance),
max(1e-3, angular_tolerance)) &&
current_waypoint.r.qdot == subjugator::Vector6d::Zero())
{
actionresult.error = "";
actionresult.success = true;
actionserver.setSucceeded(actionresult);
}
}
void executeCB(const lasa_action_planners::PLAN2CTRLGoalConstPtr &goal)
{
std::string desired_action = goal->action_type;
ROS_INFO_STREAM( "Desired Action is " << desired_action);
isOkay = false, isFTOkay = false;
ros::Rate r(10);
ROS_INFO("Waiting for EE pose/ft topic...");
while(ros::ok() && (!isOkay || !isFTOkay)) {
r.sleep();
}
if(!ros::ok()) {
result_.success = 0;
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
return;
}
// initialize action progress as null
feedback_.progress = 0;
bool success = false;
///////////////////////////////////////////////
/////----- EXECUTE REQUESTED ACTION ------/////
///////////////////////////////////////////////
if (goal->action_type=="HOME"){
// TODO: do something meaningful here
tf::Pose pose(ee_pose);
success = go_home(pose);
} else if(goal->action_type == "HOME_POUR") {
// Home for pouring with lasa robot
tf::Pose pose(ee_pose);
pose.setOrigin(tf::Vector3(-0.65, -0.11, 0.474));
pose.setRotation(tf::Quaternion(-0.006, 0.907, -0.420, -0.114));
success = go_home(pose);
}
if(goal->action_type=="FIND_TABLE"){
// Go down until table found
tf::Pose pose(ee_pose);
success = go_home(ee_pose);
if(success) {
success = find_table_for_rolling(goal->height, 0.03, goal->threshold);
}
}
if(goal->action_type=="ROLL_TEST"){
/**** For now use this instead **/
tf::Pose p(ee_pose);
/*********************************/
success = go_home(p);
if(success) {
success = find_table_for_rolling(0.15, 0.03, 5);
if(success) {
success = rolling(goal->force, goal->speed, 0.1);
}
}
}
// Use learned models to do shit
if(goal->action_type=="LEARNED_MODEL"){
DoughTaskPhase phase;
if(goal->action_name == "roll") {
phase = PHASEROLL;
} else if(goal->action_name == "reach") {
phase = PHASEREACH;
} else if(goal->action_name == "back") {
phase = PHASEBACK;
} else if(goal->action_name == "home") {
phase = PHASEHOME;
} else {
ROS_ERROR_STREAM("Unidentified action name "<<goal->action_name.c_str());
result_.success = 0;
as_.setAborted(result_);
return;
}
//TODO: update these from action
double reachingThreshold = 0.02, model_dt = 0.01;
//CDSController::DynamicsType masterType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType masterType = CDSController::MODEL_DYNAMICS;
//CDSController::DynamicsType slaveType = CDSController::LINEAR_DYNAMICS;
CDSController::DynamicsType slaveType = CDSController::UTHETA;
tf::Transform trans_obj, trans_att;
//Little hack for using reaching phase for HOMING
if (phase==PHASEHOME){
phase=PHASEREACH;
homing = true;
}
switch (action_mode) {
case ACTION_BOXY_FIXED:
/*if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(0.7, -0.43, 0.64)); // TODO: remember to add 0.15 to tf values as well
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(0.85, -0.43, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(0.73, -0.63, 0.84));
trans_obj.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
trans_att.setIdentity();*/
trans_obj.setIdentity();
trans_obj.setOrigin(tf::Vector3(0.8, -0.43, 0.64));
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(-0.05, 0,0)); // TODO: remember to add 0.15 to tf values as well
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEROLL) {
trans_att.setOrigin(tf::Vector3(0.05, 0, 0));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
} else if(phase == PHASEBACK) {
trans_att.setOrigin(tf::Vector3(-0.15, -0.2, 0.15));
trans_att.setRotation(tf::Quaternion(-0.01, 0.99, 0.005, -0.009));
}
break;
case ACTION_LASA_FIXED:
if(phase == PHASEREACH) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.10, 0.3)); // TODO: remember to add 0.15 to tf values as well (z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, ee_pose.getOrigin().z()));
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
} else if(phase == PHASEBACK) {
trans_obj.setOrigin(tf::Vector3(-0.55, -0.25, 0.3)); //(z was 0.15)
trans_obj.setRotation(tf::Quaternion(0.0, 1.0, 0.0, 0.0));
}
trans_att.setIdentity();
break;
case ACTION_VISION:
trans_obj.setRotation(tf::Quaternion(goal->object_frame.rotation.x,goal->object_frame.rotation.y,
goal->object_frame.rotation.z,goal->object_frame.rotation.w));
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,
goal->object_frame.translation.z));
trans_att.setRotation(tf::Quaternion(goal->attractor_frame.rotation.x,goal->attractor_frame.rotation.y,
goal->attractor_frame.rotation.z,goal->attractor_frame.rotation.w));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,
goal->attractor_frame.translation.z));
if (bWaitForForces){ //HACK FOR BOXY
// Hack! For setting heights for reach and add offset of roller
if(phase == PHASEREACH) {
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y,goal->attractor_frame.translation.z + 0.1));
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.setOrigin(tf::Vector3(goal->object_frame.translation.x, goal->object_frame.translation.y,ee_pose.getOrigin().getZ()));
trans_att.setOrigin(tf::Vector3(goal->attractor_frame.translation.x, goal->attractor_frame.translation.y, 0.0));
}
}
/*if (bWaitForForces){ //HACK FOR LASA
// Hack! For setting heights for reach and back
if(phase == PHASEREACH || phase == PHASEBACK) {
trans_obj.getOrigin().setZ(0.15);
trans_att.getOrigin().setZ(0.0);
}
// Hack! safety for rolling
if(phase == PHASEROLL) {
trans_obj.getOrigin().setZ(ee_pose.getOrigin().getZ());
trans_att.getOrigin().setZ(0.0);
}
}*/
break;
default:
break;
}
std::string force_gmm_id = goal->force_gmm_id;
success = learned_model_execution(phase, masterType, slaveType, reachingThreshold,
model_dt, trans_obj, trans_att, base_path, force_gmm_id);
}
result_.success = success;
homing=false;
if(success)
{
if (!homing){
ROS_WARN_STREAM("STORE PLOT");
plot_dyn = 2;
plot_dyn_msg.data = plot_dyn;
pub_plot_dyn_.publish(plot_dyn_msg);
ros::Rate r(1);
r.sleep();
}
//Wait for message of "plot stored"
/*ros::Rate wait(1000);
while(ros::ok() && (plot_published!=1)) {
ros::spinOnce();
wait.sleep();
}*/
ROS_INFO("%s: Succeeded", action_name_.c_str());
as_.setSucceeded(result_);
} else {
ROS_INFO("%s: Failed", action_name_.c_str());
as_.setAborted(result_);
}
}
void TrajActionServer::executeCB(const actionlib::SimpleActionServer<davinci_traj_streamer::trajAction>::GoalConstPtr& goal) {
double traj_clock, dt_segment, dq_segment, delta_q_segment, traj_final_time;
int isegment;
trajectory_msgs::JointTrajectoryPoint trajectory_point0;
Eigen::VectorXd qvec, qvec0, qvec_prev, qvec_new;
// TEST TEST TEST
//Eigen::VectorXd q_vec;
//q_vec<<0.1,0.2,0.15,0.4,0.5,0.6,0.7;
ROS_INFO("in executeCB");
g_count++; // keep track of total number of goals serviced since this server was started
result_.return_val = g_count; // we'll use the member variable result_, defined in our class
result_.traj_id = goal->traj_id;
cout<<"received trajectory w/ "<<goal->trajectory.points.size()<<" points"<<endl;
// copy trajectory to global var:
new_trajectory = goal->trajectory; //
// insist that a traj have at least 2 pts
int npts = new_trajectory.points.size();
if (npts < 2) {
ROS_WARN("too few points; aborting goal");
as_.setAborted(result_);
} else { //OK...have a valid trajectory goal; execute it
//got_new_goal = true;
//got_new_trajectory = true;
ROS_INFO("Cb received traj w/ npts = %d",npts);
//cout << "Cb received traj w/ npts = " << new_trajectory.points.size() << endl;
//trajectory_msgs::JointTrajectoryPoint trajectory_point0;
//trajectory_point0 = new_trajectory.points[0];
//trajectory_point0 = tj_msg.points[0];
//cout<<new_trajectory.points[0].positions.size()<<" = new_trajectory.points[0].positions.size()"<<endl;
//cout<<"size of positions[]: "<<trajectory_point0.positions.size()<<endl;
cout << "subgoals: " << endl;
int njnts;
for (int i = 0; i < npts; i++) {
njnts = new_trajectory.points[i].positions.size();
cout<<"njnts: "<<njnts<<endl;
for (int j = 0; j < njnts; j++) { //copy from traj point to 7x1 vector
cout << new_trajectory.points[i].positions[j] << ", ";
}
cout<<endl;
cout<<"time from start: "<<new_trajectory.points[i].time_from_start.toSec()<<endl;
cout << endl;
}
as_.isActive();
working_on_trajectory = true;
//got_new_trajectory=false;
traj_clock = 0.0; // initialize clock for trajectory;
isegment = 0;
trajectory_point0 = new_trajectory.points[0];
njnts = new_trajectory.points[0].positions.size();
int njnts_new;
qvec_prev.resize(njnts);
qvec_new.resize(njnts);
ROS_INFO("populating qvec_prev: ");
for (int i = 0; i < njnts; i++) { //copy from traj point to Eigen type vector
qvec_prev[i] = trajectory_point0.positions[i];
}
//cmd_pose_right(qvec0); //populate and send out first command
//qvec_prev = qvec0;
cout << "start pt: " << qvec_prev.transpose() << endl;
}
while (working_on_trajectory) {
traj_clock += dt_traj;
// update isegment and qvec according to traj_clock;
//if traj_clock>= final_time, use exact end coords and set "working_on_trajectory" to false
//ROS_INFO("traj_clock = %f; updating qvec_new",traj_clock);
working_on_trajectory = update_trajectory(traj_clock, new_trajectory, qvec_prev, isegment, qvec_new);
//cmd_pose_right(qvec_new); // use qvec to populate object and send it to robot
//ROS_INFO("publishing qvec_new as command");
//davinciJointPublisher.pubJointStatesAll(qvec_new);
command_joints(qvec_new); //map these to all gazebo joints and publish as commands
qvec_prev = qvec_new;
//cout << "traj_clock: " << traj_clock << "; vec:" << qvec_new.transpose() << endl;
ros::spinOnce();
ros::Duration(dt_traj).sleep();
}
ROS_INFO("completed execution of a trajectory" );
as_.setSucceeded(result_); // tell the client that we were successful acting on the request, and return the "result" message
}
// Cancel the action
void preemptCB()
{
ROS_INFO_STREAM_NAMED("pick_place_moveit","Preempted");
action_server_.setPreempted();
}
void preemptCB()
{
ROS_INFO("%s: Preempted", action_name_.c_str());
// set the action state to preempted
as_.setPreempted();
}
void Navigator::executeCB(const actionlib::SimpleActionServer<navigator::navigatorAction>::GoalConstPtr& goal) {
int destination_id = goal->location_code;
geometry_msgs::PoseStamped destination_pose;
int navigation_status;
if (destination_id==navigator::navigatorGoal::COORDS) {
destination_pose=goal->desired_pose;
}
switch(destination_id) {
case navigator::navigatorGoal::HOME:
//specialized function to navigate to pre-defined HOME coords
navigation_status = navigate_home();
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached home");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate home!");
result_.return_code = navigator::navigatorResult::FAILED_CANNOT_REACH_DES_POSE;
navigator_as_.setAborted(result_);
}
break;
case navigator::navigatorGoal::TABLE:
//specialized function to navigate to pre-defined TABLE coords
navigation_status = navigate_to_table();
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached table");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate to table!");
result_.return_code = navigator::navigatorResult::FAILED_CANNOT_REACH_DES_POSE;
navigator_as_.setAborted(result_);
}
break;
case navigator::navigatorGoal::COORDS:
//more general function to navigate to specified pose:
destination_pose=goal->desired_pose;
navigation_status = navigate_to_pose(destination_pose);
if (navigation_status==navigator::navigatorResult::DESIRED_POSE_ACHIEVED) {
ROS_INFO("reached desired pose");
result_.return_code = navigator::navigatorResult::DESIRED_POSE_ACHIEVED;
navigator_as_.setSucceeded(result_);
}
else {
ROS_WARN("could not navigate to desired pose!");
result_.return_code = navigator::navigatorResult::FAILED_CANNOT_REACH_DES_POSE;
navigator_as_.setAborted(result_);
}
break;
default:
ROS_WARN("this location ID is not implemented");
result_.return_code = navigator::navigatorResult::DESTINATION_CODE_UNRECOGNIZED;
navigator_as_.setAborted(result_);
}
}
void preemptCB()
{
ROS_INFO("robot_mover: Preempted");
// set the action state to preempted
act_srv_.setPreempted();
}
void executeCB(const segbot_arm_manipulation::TabletopGraspGoalConstPtr &goal)
{
if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::GRASP){
if (goal->cloud_clusters.size() == 0){
ROS_INFO("[segbot_tabletop_grap_as.cpp] No object point clouds received...aborting");
as_.setAborted(result_);
return;
}
ROS_INFO("[segbot_tabletop_grap_as.cpp] Received action request...proceeding.");
listenForArmData(40.0);
//the result
segbot_arm_manipulation::TabletopGraspResult result;
//get the data out of the goal
Eigen::Vector4f plane_coef_vector;
for (int i = 0; i < 4; i ++)
plane_coef_vector(i)=goal->cloud_plane_coef[i];
int selected_object = goal->target_object_cluster_index;
PointCloudT::Ptr target_object (new PointCloudT);
pcl::PCLPointCloud2 target_object_pc2;
pcl_conversions::toPCL(goal->cloud_clusters.at(goal->target_object_cluster_index),target_object_pc2);
pcl::fromPCLPointCloud2(target_object_pc2,*target_object);
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Publishing point cloud...");
cloud_grasp_pub.publish(goal->cloud_clusters.at(goal->target_object_cluster_index));
//wait for response at 5 Hz
listenForGrasps(40.0);
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Heard %i grasps",(int)current_grasps.grasps.size());
//next, compute approach and grasp poses for each detected grasp
//wait for transform from visual space to arm space
std::string sensor_frame_id = goal->cloud_clusters.at(goal->target_object_cluster_index).header.frame_id;
listener.waitForTransform(sensor_frame_id, "mico_link_base", ros::Time(0), ros::Duration(3.0));
//here, we'll store all grasp options that pass the filters
std::vector<GraspCartesianCommand> grasp_commands;
for (unsigned int i = 0; i < current_grasps.grasps.size(); i++){
GraspCartesianCommand gc_i = segbot_arm_manipulation::grasp_utils::constructGraspCommand(current_grasps.grasps.at(i),HAND_OFFSET_APPROACH,HAND_OFFSET_GRASP, sensor_frame_id);
//filter 1: if the grasp is too close to plane, reject it
bool ok_with_plane = segbot_arm_manipulation::grasp_utils::checkPlaneConflict(gc_i,plane_coef_vector,MIN_DISTANCE_TO_PLANE);
//for filter 2, the grasps need to be in the arm's frame of reference
listener.transformPose("mico_link_base", gc_i.approach_pose, gc_i.approach_pose);
listener.transformPose("mico_link_base", gc_i.grasp_pose, gc_i.grasp_pose);
//filter 2: apply grasp filter method in request
bool passed_filter = passesFilter(goal->grasp_filter_method,gc_i);
if (passed_filter && ok_with_plane){
ROS_INFO("Found grasp fine with filter and plane");
//filter two -- if IK fails
moveit_msgs::GetPositionIK::Response ik_response_approach = segbot_arm_manipulation::computeIK(nh_,gc_i.approach_pose);
if (ik_response_approach.error_code.val == 1){
moveit_msgs::GetPositionIK::Response ik_response_grasp = segbot_arm_manipulation::computeIK(nh_,gc_i.grasp_pose);
if (ik_response_grasp.error_code.val == 1){
ROS_INFO("...grasp fine with IK");
//now check to see how close the two sets of joint angles are -- if the joint configurations for the approach and grasp poses differ by too much, the grasp will not be accepted
std::vector<double> D = segbot_arm_manipulation::getJointAngleDifferences(ik_response_approach.solution.joint_state, ik_response_grasp.solution.joint_state);
double sum_d = 0;
for (int p = 0; p < D.size(); p++){
sum_d += D[p];
}
if (sum_d < ANGULAR_DIFF_THRESHOLD){
//ROS_INFO("Angle diffs for grasp %i: %f, %f, %f, %f, %f, %f",(int)grasp_commands.size(),D[0],D[1],D[2],D[3],D[4],D[5]);
//ROS_INFO("Sum diff: %f",sum_d);
//store the IK results
gc_i.approach_q = ik_response_approach.solution.joint_state;
gc_i.grasp_q = ik_response_grasp.solution.joint_state;
grasp_commands.push_back(gc_i);
ROS_INFO("...fine with continuity");
}
}
}
}
}
//check to see if all potential grasps have been filtered out
if (grasp_commands.size() == 0){
ROS_WARN("[segbot_tabletop_grasp_as.cpp] No feasible grasps found. Aborting.");
as_.setAborted(result_);
return;
}
//make sure we're working with the correct tool pose
listenForArmData(30.0);
int selected_grasp_index = -1;
if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_ORIENTATION_SELECTION){
//find the grasp with closest orientatino to current pose
double min_diff = 1000000.0;
for (unsigned int i = 0; i < grasp_commands.size(); i++){
double d_i = segbot_arm_manipulation::grasp_utils::quat_angular_difference(grasp_commands.at(i).approach_pose.pose.orientation, current_pose.pose.orientation);
ROS_INFO("Distance for pose %i:\t%f",(int)i,d_i);
if (d_i < min_diff){
selected_grasp_index = (int)i;
min_diff = d_i;
}
}
}
else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::RANDOM_SELECTION){
srand (time(NULL));
selected_grasp_index = rand() % grasp_commands.size();
ROS_INFO("Randomly selected grasp = %i",selected_grasp_index);
}
else if (goal->grasp_selection_method == segbot_arm_manipulation::TabletopGraspGoal::CLOSEST_JOINTSPACE_SELECTION){
double min_diff = 1000000.0;
for (unsigned int i = 0; i < grasp_commands.size(); i++){
std::vector<double> D_i = segbot_arm_manipulation::getJointAngleDifferences(grasp_commands.at(i).approach_q, current_state);
double sum_d = 0;
for (int p = 0; p < D_i.size(); p++)
sum_d += D_i[p];
if (sum_d < min_diff){
selected_grasp_index = (int)i;
min_diff = sum_d;
}
}
}
if (selected_grasp_index == -1){
ROS_WARN("[segbot_tabletop_grasp_as.cpp] Grasp selection failed. Aborting.");
as_.setAborted(result_);
return;
}
//compute RPY for target pose
ROS_INFO("Selected approach pose:");
ROS_INFO_STREAM(grasp_commands.at(selected_grasp_index).approach_pose);
//publish individual pose for visualization purposes
pose_pub.publish(grasp_commands.at(selected_grasp_index).approach_pose);
//close fingers while moving
segbot_arm_manipulation::closeHand();
//move to approach pose -- do it twice to correct
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).approach_pose);
//open fingers
segbot_arm_manipulation::openHand();
//move to grasp pose
segbot_arm_manipulation::moveToPoseMoveIt(nh_,grasp_commands.at(selected_grasp_index).grasp_pose);
//close hand
segbot_arm_manipulation::closeHand();
result_.success = true;
as_.setSucceeded(result_);
return;
}
else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER){
//TO DO: move to handover position
ROS_INFO("Starting handover action...");
//listen for haptic feedback
double rate = 40;
ros::Rate r(rate);
double total_grav_free_effort = 0;
double total_delta;
double delta_effort[6];
listenForArmData(40.0);
sensor_msgs::JointState prev_effort_state = current_effort;
double elapsed_time = 0;
while (ros::ok()){
ros::spinOnce();
total_delta=0.0;
for (int i = 0; i < 6; i ++){
delta_effort[i] = fabs(current_effort.effort[i]-prev_effort_state.effort[i]);
total_delta+=delta_effort[i];
ROS_INFO("Total delta=%f",total_delta);
}
if (total_delta > fabs(FORCE_HANDOVER_THRESHOLD)){
ROS_INFO("[segbot_tabletop_grasp_as.cpp] Force detected");
//now open the hand
segbot_arm_manipulation::openHand();
result_.success = true;
as_.setSucceeded(result_);
return;
}
r.sleep();
elapsed_time+=(1.0)/rate;
if (goal->timeout_seconds > 0 && elapsed_time > goal->timeout_seconds){
ROS_WARN("Handover action timed out...");
result_.success = false;
as_.setAborted(result_);
return;
}
}
result_.success = true;
as_.setSucceeded(result_);
}
else if (goal->action_name == segbot_arm_manipulation::TabletopGraspGoal::HANDOVER_FROM_HUMAN){
//open fingers
segbot_arm_manipulation::openHand();
//update readings
listenForArmData(40.0);
bool result = waitForForce(FORCE_HANDOVER_THRESHOLD,goal->timeout_seconds);
if (result){
segbot_arm_manipulation::closeHand();
result_.success = true;
as_.setSucceeded(result_);
}
else {
result_.success = false;
as_.setAborted(result_);
}
}
}
