// 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);
        }
    }
Beispiel #3
0
  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;
    }
Beispiel #5
0
	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 
}
Beispiel #16
0
  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_);
    }
  }
Beispiel #19
0
 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);
}
Beispiel #23
0
  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_);
			}
		
		}
	
	
	}