arm_movements_by_pose_alg_node.cpp

Go to the documentation of this file.

#include "arm_movements_by_pose_alg_node.h"

ArmMovementsByPoseAlgNode::ArmMovementsByPoseAlgNode(void) :
  algorithm_base::IriBaseAlgorithm<ArmMovementsByPoseAlgorithm>(),
  move_right_arm_client_("move_right_arm", true),
  move_left_arm_client_("move_left_arm", true)
{
        //init class attributes if necessary
        //this->loop_rate_ = 2;//in [Hz]

        // [init publishers]
        this->visualization_movement_markers_publisher_ = this->public_node_handle_.advertise<visualization_msgs::MarkerArray>("visualization_movement_markers", 1);
        
        // [init subscribers]
        
        // [init services]
        this->clean_movement_server_ = this->public_node_handle_.advertiseService("clean_movement", &ArmMovementsByPoseAlgNode::clean_movementCallback, this);
        
        // [init clients]
        wam_pose_move_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::pose_move>("wam_pose_move");
        move_in_joints_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::joints_move>("move_in_joints");
        get_joints_from_pose_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::wamInverseKinematics>("get_joints_from_pose");
        pose_move_client_ = this->public_node_handle_.serviceClient<iri_wam_common_msgs::pose_move>("pose_move");
        
        // [init action servers]
        
        // [init action clients]
        
        // Other
        if (this->log_movements) {
                logfile.open ("movements.txt");
                logfile << "Pose (x, y, z) | Quaternion (x, y, z, w) | Status \n";
        }
        //private_node_handle_.param<bool>("cleaning_fake_movements", cleaning_fake_movements, 0);
}

ArmMovementsByPoseAlgNode::~ArmMovementsByPoseAlgNode(void)
{
  // [free dynamic memory]
  
  logfile.close();
}

void ArmMovementsByPoseAlgNode::mainNodeThread(void)
{
  // [fill msg structures]
  //this->MarkerArray_msg_.data = my_var;
  
  // [fill srv structure and make request to the server]
  //wam_pose_move_srv_.request.data = my_var; 
  //ROS_INFO("ArmMovementsByPoseAlgNode:: Sending New Request!"); 
  //if (wam_pose_move_client_.call(wam_pose_move_srv_)) 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Response: %s", wam_pose_move_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic wam_pose_move "); 
  //}
  //move_in_joints_srv_.request.data = my_var; 
  //ROS_INFO("ArmMovementsByPoseAlgNode:: Sending New Request!"); 
  //if (move_in_joints_client_.call(move_in_joints_srv_)) 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Response: %s", move_in_joints_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic move_in_joints "); 
  //}
  
  //get_joints_from_pose_srv_.request.data = my_var; 
  //ROS_INFO("ArmMovementsByPoseAlgNode:: Sending New Request!"); 
  //if (get_joints_from_pose_client_.call(get_joints_from_pose_srv_)) 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Response: %s", get_joints_from_pose_srv_.response.result); 
  //} 
  //else 
  //{ 
    //ROS_INFO("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic get_joints_from_pose "); 
  //}
  
  // [fill action structure and make request to the action server]
  //move_right_armMakeActionRequest();
  //move_left_armMakeActionRequest();

  // [publish messages]
  //this->visualization_movement_markers_publisher_.publish(this->MarkerArray_msg_);
}

/*  [subscriber callbacks] */

/*  [service callbacks] */
bool ArmMovementsByPoseAlgNode::clean_movementCallback(estirabot_msgs::ArmMovementsPosesSrv::Request &req, 
                                                       estirabot_msgs::ArmMovementsPosesSrv::Response &res) 
{ 
        std::vector<geometry_msgs::PoseStamped> movement_poses;
        ROS_DEBUG("ArmMovementsByPoseAlgNode::clean_movementCallback: New Request Received!"); 

        //use appropiate mutex to shared variables if necessary 
        //this->alg_.lock(); 
        //this->clean_movement_mutex_.enter(); 
        
        if (req.movement_type == 1) {// standard move
                movement_poses = this->alg_.get_transformed_poses(req.poses, req.plane_coefficients);
                get_poses_markers(req.poses, req.secondary_arm);
                if(this->visualization_movement_markers_publisher_.getNumSubscribers())
                {
                        this->visualization_movement_markers_publisher_.publish(this->MarkerArray_msg_);
                }
        }
        else { // move back to initial position
                movement_poses = this->alg_.get_initial_position();
                req.secondary_arm.push_back(false);
        }
        
        res.success = true;
        this->MarkerArray_msg_.markers.clear();
        for (size_t i = 0; i < movement_poses.size(); i++) {
                geometry_msgs::PoseStamped target_pose = movement_poses[i];
                target_pose = movement_poses[i];
                target_pose.pose.orientation = this->alg_.rotate_roll(target_pose.pose.orientation, roll_rotation);
                target_pose.pose.orientation = this->alg_.rotate_yaw(target_pose.pose.orientation, yaw_rotation);
                target_pose.pose.orientation = this->alg_.rotate_pitch(target_pose.pose.orientation, pitch_rotation);
                
                if (i==0)
                        ROS_INFO_STREAM("Moving to " << target_pose);
                else
                        ROS_DEBUG_STREAM("Moving to " << target_pose);
                
                if (fake_movements) { //FAKE MOVEMENTS
                        if (i==0)
                                ROS_WARN_STREAM("Fake movement is active");
                }
                // TODO future feature: remove this "else if" when moving WAM without planning using poses is available
                else if (robot_wam) { // WAM
                        if (req.secondary_arm[i] == false) {
                                // Moving using barret cartesian movement
                                // WARNING cartesian move has some unexpected accelerations
//                              wam_pose_move_srv_.request.pose = target_pose.pose; 
//                              ROS_DEBUG("ArmMovementsByPoseAlgNode:: Sending New Request wam_pose_move!"); 
//                              if (wam_pose_move_client_.call(wam_pose_move_srv_)) 
//                              { 
//                                      ROS_DEBUG_STREAM("ArmMovementsByPoseAlgNode:: pose_move in cartesian successful: " << wam_pose_move_srv_.response.success); 
//                              } 
//                              else 
//                              { 
//                                      ROS_ERROR("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic wam_pose_move "); 
//                              }
                                
                                // Moving using IK + joints
                                // TODO It is very slow.
                                // WARNING Path are non-linear in cartesian space
                                get_joints_from_pose_srv_.request.pose = target_pose;
                                ROS_DEBUG("ArmMovementsByPoseAlgNode:: Sending New Request!");
                                if (get_joints_from_pose_client_.call(get_joints_from_pose_srv_))
                                {
                                        ROS_DEBUG("ArmMovementsByPoseAlgNode:: IK ok.");
                                        
                                        if (get_joints_from_pose_srv_.response.joints.position.size() > 0) {
                                                move_in_joints_srv_.request.joints = get_joints_from_pose_srv_.response.joints.position;
                                                move_in_joints_srv_.request.velocities = get_wam_movement_speed();
                                                move_in_joints_srv_.request.accelerations = get_wam_movement_speed();
                                                ROS_DEBUG("ArmMovementsByPoseAlgNode:: Sending New Request!");
                                                if (move_in_joints_client_.call(move_in_joints_srv_))
                                                { 
                                                        ROS_DEBUG_STREAM("ArmMovementsByPoseAlgNode:: Movement sucess: " << move_in_joints_srv_.response.success);
                                                } 
                                                else 
                                                { 
                                                        ROS_WARN("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic move_in_joints ");
                                                }
                                        }
                                        else
                                                ROS_WARN("ArmMovementsByPoseAlgNode:: Obtained no joints positions from IK.");
                                } 
                                else 
                                { 
                                        ROS_WARN("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic get_joints_from_pose ");
                                }
                                
                                /* DEPRECATED: old IRI WAM
                                pose_move_srv_.request.pose = target_pose.pose;
                                
                                if (req.secondary_arm[i] == false) {
                                        if (pose_move_client_.call(pose_move_srv_))  {
                                                ROS_INFO_STREAM("Movement result: " << pose_move_srv_.response.success);
                                        }
                                        else 
                                        { 
                                                ROS_WARN("ArmMovementsByPoseAlgNode:: Failed to Call Server on topic pose_move ");
                                                res.success = false;
                                                res.too_far_points_indexes.push_back(i);
                                        }
                                }
                                */
                        }
                        else {
                                // TODO
                                ROS_ERROR_STREAM("WAM secondary arm not implemented yet!!!");
                                ros::Duration(3).sleep();
                        }
                }
                else { // REEM
                        if (req.secondary_arm[i] == false) {
                                if (move_right_armMakeActionRequest(target_pose))
                                        ROS_DEBUG("Movement successful");
                                else {
                                        res.success = false;
                                        res.too_far_points_indexes.push_back(i);
                                }
                        }
                        else {
                                if (move_left_armMakeActionRequest(target_pose))
                                        ROS_DEBUG("Movement successful");
                                else {
                                        res.success = false;
                                        res.too_far_points_indexes.push_back(i);
                                }
                        }
                }
        }
        
        
        //unlock previously blocked shared variables 
        //this->alg_.unlock(); 
        //this->clean_movement_mutex_.exit(); 

        return true; 
}



/*  [action callbacks] */
void ArmMovementsByPoseAlgNode::move_right_armDone(const actionlib::SimpleClientGoalState& state,  const arm_navigation_msgs::MoveArmResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ArmMovementsByPoseAlgNode::move_right_armDone: Goal Achieved!"); 
  else 
    ROS_INFO("ArmMovementsByPoseAlgNode::move_right_armDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void ArmMovementsByPoseAlgNode::move_right_armActive() 
{ 
  //ROS_INFO("ArmMovementsByPoseAlgNode::move_right_armActive: Goal just went active!"); 
} 

void ArmMovementsByPoseAlgNode::move_right_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ArmMovementsByPoseAlgNode::move_right_armFeedback: Got Feedback!"); 

  bool feedback_is_ok = true; 

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    move_right_arm_client_.cancelGoal(); 
    //ROS_INFO("ArmMovementsByPoseAlgNode::move_right_armFeedback: Cancelling Action!"); 
  } 
}
void ArmMovementsByPoseAlgNode::move_left_armDone(const actionlib::SimpleClientGoalState& state,  const arm_navigation_msgs::MoveArmResultConstPtr& result) 
{ 
  if( state.toString().compare("SUCCEEDED") == 0 ) 
    ROS_INFO("ArmMovementsByPoseAlgNode::move_left_armDone: Goal Achieved!"); 
  else 
    ROS_INFO("ArmMovementsByPoseAlgNode::move_left_armDone: %s", state.toString().c_str()); 

  //copy & work with requested result 
} 

void ArmMovementsByPoseAlgNode::move_left_armActive() 
{ 
  //ROS_INFO("ArmMovementsByPoseAlgNode::move_left_armActive: Goal just went active!"); 
} 

void ArmMovementsByPoseAlgNode::move_left_armFeedback(const arm_navigation_msgs::MoveArmFeedbackConstPtr& feedback) 
{ 
  //ROS_INFO("ArmMovementsByPoseAlgNode::move_left_armFeedback: Got Feedback!"); 

  bool feedback_is_ok = true;

  //if feedback is not what expected, cancel requested goal 
  if( !feedback_is_ok ) 
  { 
    move_left_arm_client_.cancelGoal(); 
    //ROS_INFO("ArmMovementsByPoseAlgNode::move_left_armFeedback: Cancelling Action!"); 
  } 
}



/*  [action requests] */
bool ArmMovementsByPoseAlgNode::move_right_armMakeActionRequest(geometry_msgs::PoseStamped pose_st) 
{ 
        ROS_DEBUG("ArmMovementsByPoseAlgNode::move_right_armMakeActionRequest: Starting New Request!"); 
        
        arm_navigation_msgs::SimplePoseConstraint desired_pose;
        std::string group_name;
        
        desired_pose.header.frame_id = pose_st.header.frame_id;
        desired_pose.pose = pose_st.pose;
        
        if (robot_wam) {
                group_name = "iri_wam";
                desired_pose.link_name = "lk_wam7";
                
                desired_pose.absolute_position_tolerance.x = 0.01;
                desired_pose.absolute_position_tolerance.y = 0.01;
                desired_pose.absolute_position_tolerance.z = 0.01;
                
                desired_pose.absolute_roll_tolerance = 0.2;
                desired_pose.absolute_pitch_tolerance = 0.2;
                desired_pose.absolute_yaw_tolerance = 0.2;
        }
        else {
                group_name = "right_arm_torso";
                desired_pose.link_name = "arm_right_7_link";
                
                desired_pose.absolute_position_tolerance.x = 0.03;
                desired_pose.absolute_position_tolerance.y = 0.03;
                desired_pose.absolute_position_tolerance.z = 0.03;
                
                desired_pose.absolute_roll_tolerance = 0.3;
                desired_pose.absolute_pitch_tolerance = 0.3;
                desired_pose.absolute_yaw_tolerance = 0.3;
        }
        
        return move_armMakeActionRequest(true, desired_pose, group_name);
}


bool ArmMovementsByPoseAlgNode::move_left_armMakeActionRequest(geometry_msgs::PoseStamped pose_st) 
{ 
        ROS_DEBUG("ArmMovementsByPoseAlgNode::move_left_armMakeActionRequest: Starting New Request!"); 
        
        arm_navigation_msgs::SimplePoseConstraint desired_pose;
        std::string group_name;
        
        desired_pose.header.frame_id = pose_st.header.frame_id;
        desired_pose.pose = pose_st.pose;
        
        if (robot_wam) {
                group_name = "iri_wam";
                desired_pose.link_name = "lk_wam7";
                
                desired_pose.absolute_position_tolerance.x = 0.01;
                desired_pose.absolute_position_tolerance.y = 0.01;
                desired_pose.absolute_position_tolerance.z = 0.01;
                
                desired_pose.absolute_roll_tolerance = 0.2;
                desired_pose.absolute_pitch_tolerance = 0.2;
                desired_pose.absolute_yaw_tolerance = 0.2;
        }
        else {
                group_name = "left_arm";
                desired_pose.link_name = "arm_left_7_link";
                
                // invert z axis
                // Barret hand and Reem's right hand is looking in Z-axis direction
                // Reem's left hand is looking at the oposite direction
                desired_pose.pose.orientation = this->alg_.rotate_pitch(pose_st.pose.orientation, M_PI);
                desired_pose.pose.position = pose_st.pose.position;
                
                desired_pose.absolute_position_tolerance.x = 0.03;
                desired_pose.absolute_position_tolerance.y = 0.03;
                desired_pose.absolute_position_tolerance.z = 0.03;
                
                desired_pose.absolute_roll_tolerance = 0.3;
                desired_pose.absolute_pitch_tolerance = 0.3;
                desired_pose.absolute_yaw_tolerance = 0.3;
        }
        
        return move_armMakeActionRequest(false, desired_pose, group_name);
}

bool ArmMovementsByPoseAlgNode::move_armMakeActionRequest(bool right_arm, arm_navigation_msgs::SimplePoseConstraint desired_pose, std::string group_name) 
{
        //wait for the action server to start 
        //will wait for infinite time 
        actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> *move_arm_client;
        
        if (right_arm)
                move_arm_client = &move_right_arm_client_;
        else
                move_arm_client = &move_left_arm_client_;
        
        move_arm_client->waitForServer();
        ROS_DEBUG("ArmMovementsByPoseAlgNode::move_armMakeActionRequest: Server is Available!"); 
        
        arm_navigation_msgs::MoveArmGoal my_goal;
        
        //send a goal to the action 
        my_goal.motion_plan_request.group_name=group_name;
        my_goal.motion_plan_request.num_planning_attempts = 1;
        my_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
        my_goal.motion_plan_request.planner_id = std::string("");
        my_goal.planner_service_name=std::string("ompl_planning/plan_kinematic_path");
        my_goal.motion_plan_request.expected_path_dt = ros::Duration(3.0);
        
        arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,my_goal);
        
        if (this->log_movements) {
                logfile << group_name << "  " << desired_pose.pose.position.x << ", " << desired_pose.pose.position.y << ", " << desired_pose.pose.position.z << ", " << " | " ;
                logfile << desired_pose.pose.orientation.x << ", " << desired_pose.pose.orientation.y << ", " << 
                           desired_pose.pose.orientation.z << ", " << desired_pose.pose.orientation.w << ", " << " | ";
        }
        
        if (right_arm) 
                move_left_arm_client_.sendGoal(my_goal, 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_left_armDone,     this, _1, _2), 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_left_armActive,   this), 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_left_armFeedback, this, _1));
        else
                move_right_arm_client_.sendGoal(my_goal, 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_right_armDone,     this, _1, _2), 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_right_armActive,   this), 
                        boost::bind(&ArmMovementsByPoseAlgNode::move_right_armFeedback, this, _1));
        
        ROS_DEBUG("ArmMovementsByPoseAlgNode::move_armMakeActionRequest: Goal Sent. Wait for Result!");
        
        bool finished_within_time = false;
        finished_within_time = move_arm_client->waitForResult(ros::Duration(6.0));
        if (!finished_within_time)
        {
                ROS_WARN_STREAM("Timed out achieving arm goal for " << group_name);
                move_arm_client->cancelGoal();
                actionlib::SimpleClientGoalState state = move_arm_client->getState();
                
                ROS_WARN_STREAM("After cancel, State is " << state.toString());
                
                if (this->log_movements) {
                        logfile << "Timed out: " << state.toString() << " and ";
                } // continues after cancelling it
                
                move_arm_client->waitForResult(ros::Duration(10.0));
                state = move_arm_client->getState();
                ROS_WARN_STREAM("After time out, State is " << state.toString());
                
                if (this->log_movements) {
                        logfile << state.toString() << "\n";
                }
        }
        else
        {
                actionlib::SimpleClientGoalState state = move_arm_client->getState();
                bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
                if(success) {
                        ROS_DEBUG_STREAM("Move " << group_name << " action finished: " << state.toString().c_str());
                        if (this->log_movements) {
                                logfile << "Ok: " << state.toString() << "\n";
                        }
                }
                else {
                        ROS_WARN_STREAM("Move " << group_name << " action failed: " << state.toString().c_str());
                        if (this->log_movements) {
                                logfile << "Failed: " << state.toString() << "\n";
                        }
                        if (state.toString().compare("ABORTED") == 0) // if aborted return false (probably IK not found)
                                return false; 
                }
        }
        
        return true;
}



void ArmMovementsByPoseAlgNode::addNodeDiagnostics(void)
{
}

/* main function */
int main(int argc,char *argv[])
{
        return algorithm_base::main<ArmMovementsByPoseAlgNode>(argc, argv, "arm_movements_by_pose_alg_node");
}


std::vector< double > ArmMovementsByPoseAlgNode::get_wam_movement_speed() {
        std::vector< double > res;
        for (size_t i = 0; i < 7; i++)
                res.push_back(this->robot_movement_speed_);
        return res;
}


void ArmMovementsByPoseAlgNode::get_poses_markers(std::vector<geometry_msgs::PoseStamped> poses, std::vector< uint8_t> secondary_arm) {
        // publishing markers
        visualization_msgs::MarkerArray markers_msg;
        size_t last_main_idx = 1000, last_secondary_idx = 1000, last_idx; // 1000 means it hasn't been set
        for (size_t i = 0; i < poses.size(); i++) {
                // marker
                visualization_msgs::Marker marker;
                marker.header.frame_id = poses[i].header.frame_id;
                marker.ns = "WAM movements";
                marker.id = i;
                marker.header.stamp = ros::Time();
                marker.type = visualization_msgs::Marker::ARROW;
                marker.action = visualization_msgs::Marker::ADD;
                
                // scale
                marker.scale.x = 0.02;
                marker.scale.y = 0.02;
                marker.scale.z = 0.02;
                // visible
                marker.color.a = 1.0;
                
                if (secondary_arm[i] == false) {
                        last_idx = last_main_idx;
                        last_main_idx = i;
                        
                        marker.color.r = 0.0;
                        marker.color.g = 1.0;
                        marker.color.b = 0.0;
                }
                else {
                        last_idx = last_secondary_idx;
                        last_secondary_idx = i;
                        
                        marker.color.r = 1.0;
                        marker.color.g = 0.0;
                        marker.color.b = 0.0;
                }
                
                if (last_idx == 1000) { // first one of the arm || 1000 means it hasn't been set
                        marker.type = visualization_msgs::Marker::SPHERE;
                        marker.pose = poses[i].pose;
                }
                else {
                        marker.type = visualization_msgs::Marker::ARROW;
                        marker.points.push_back(poses[last_idx].pose.position);
                        marker.points.push_back(poses[i].pose.position);
                }
                
                // add to array
                markers_msg.markers.push_back(marker);
        }
        
        // remove posible remaining markers
        for (size_t i = markers_msg.markers.size(); i < 50; i++) {
                // marker
                visualization_msgs::Marker marker;
                
                marker.header.frame_id = poses[0].header.frame_id;
                marker.header.stamp = ros::Time();
                marker.ns = "WAM movements";
                marker.id = i;
                marker.action = visualization_msgs::Marker::ADD;
                marker.type = visualization_msgs::Marker::SPHERE;
                
                marker.pose = poses[0].pose;
                
                marker.scale.x = 0.01;
                marker.scale.y = 0.01;
                marker.scale.z = 0.01;
                
                marker.color.a = 0.0;
                marker.color.r = 1.0;
                marker.color.g = 1.0;
                marker.color.b = 1.0;
                
                markers_msg.markers.push_back(marker);
        }
        
        this->MarkerArray_msg_ = markers_msg;
}


// config
void ArmMovementsByPoseAlgNode::node_config_update(Config &config, uint32_t level)
{
        this->alg_.lock();
        
        this->fake_movements = config.fake_movements;
        this->robot_wam = config.robot_wam;
        this->roll_rotation = config.roll_rotation;
        this->yaw_rotation = config.yaw_rotation;
        this->pitch_rotation = config.pitch_rotation;
        this->robot_movement_speed_ = config.robot_movement_speed;

        if ( (not this->log_movements) && config.log_movements ) { // added log_movements
                logfile.open ("movements.txt");
                logfile << "Pose (x, y, z) | Quaternion (x, y, z, w) | Status \n";
        }
        else if ( this->log_movements && (not config.log_movements) ) { // removed log_movements
                logfile.close();
        }
        
        this->log_movements = config.log_movements;
        
        this->alg_.unlock();
}