bool checkIfSafe(){
	
	if (enabled){
		while(!fkine_client.exists()){
			ROS_INFO("Waiting for service");
			sleep(1.0);
		}

		/*
		 * Forward Kinematic service call
		 */

		moveit_msgs::GetPositionFK::Request fkine_request;
		moveit_msgs::GetPositionFK::Response fkine_response;

		sensor_msgs::JointState q_true = js_cur;

		//Load request with the desired link
		fkine_request.fk_link_names.push_back("mico_link_1");
		fkine_request.fk_link_names.push_back("mico_link_2");
		fkine_request.fk_link_names.push_back("mico_link_3");
		fkine_request.fk_link_names.push_back("mico_link_4");
		fkine_request.fk_link_names.push_back("mico_link_5");
		fkine_request.fk_link_names.push_back("mico_link_hand");
		fkine_request.fk_link_names.push_back("mico_link_finger_1");
		fkine_request.fk_link_names.push_back("mico_link_finger_2");

		if(fkine_client.call(fkine_request, fkine_response)){
			ros::spinOnce();
			ROS_DEBUG("IK call successful.");
		} else {
			ROS_ERROR("IK call failed. Moveit! probably can't be contacted. Preempting movement.");
			return false;
		}
		
		/*
		 * Check if any joints are outside the radius
		 */
		bool temp = true;
		for(int i = 0; i < fkine_response.pose_stamped.size(); i++){
			if(fkine_response.pose_stamped.at(i).pose.position.x > inflationRad ||
					fkine_response.pose_stamped.at(i).pose.position.y > inflationRad ||
					fkine_response.pose_stamped.at(i).pose.position.x < -inflationRad ||
					fkine_response.pose_stamped.at(i).pose.position.y < -inflationRad)
				temp = false;
		}
		safe = temp;
		pub_data.data = temp;
		pub.publish(pub_data);
		return temp;
	}
	else {
		//if disabled, just assume safe
		pub_data.data = true;
		pub.publish(pub_data);
		return true;
	}
}
Beispiel #2
0
int main(int argc, char **argv)
{
    ros::init(argc, argv, "pendulum_controller");
    //ros::init(argc, argv, "pendulum");
    ros::NodeHandle n("pendulum");
    ros::NodeHandle n_state("pendulum");

    command_pub = n.advertise<pendulum::command>( "command", 1000 );

    state_client = n_state.serviceClient<pendulum::state>( "state" );
    //state_client = n.serviceClient<pendulum::state>( "state" );
    //state_client = n.serviceClient<pendulum::state>( "state", true );
    if( !state_client.isValid( ) ) {
	ROS_INFO( "state_client not valid" );
        state_client.waitForExistence( );
    } 

    ros::Rate loop_rate( 25 );  
    //ros::Rate loop_rate( 10 );  

    ROS_INFO("Ready to standup pendulum.");

    command.time = 0.0;
    command.torque = 0.0;
    state.request.timestamp = 0.0;

    pendulum::state s;

    if( !state_client.isValid( ) ) {
	ROS_INFO( "state_client no longer valid" );
        state_client.waitForExistence( );
    } 

    int count = 0;

    ros::Time calibrate_time, start_time;

    while( ros::ok( ) ) {
    	calibrate_time = ros::Time::now( );

	if( calibrate_time.sec != 0 || calibrate_time.nsec != 0 )
	    break;
	
        ros::spinOnce( );		
    }

    start_time = ros::Time::now( );
    //ROS_INFO( "start_time: sec[%u] nsec[%u]", start_time.sec, start_time.nsec );
    ROS_INFO( "time, position, velocity, torque" );

    while( ros::ok( ) ) {

	//if( count == 1 ) break;

        if( state_client && state_client.exists( ) && state_client.isValid( ) ) {
            //ROS_INFO("Calling service");
            if( state_client.call( s ) ) {
                //ROS_INFO("Successful call");

    		ros::Time sim_time = ros::Time::now( );

    		//ROS_INFO( "sim_time: sec[%u] nsec[%u]", sim_time.sec, sim_time.nsec );

		//ros::Duration current_time = sim_time - start_time;

		//Real time = (Real) count / 1000.0;
		Real time = (sim_time - start_time).toSec();

	        command.torque = control( time, s.response.position, s.response.velocity );
  	        command.time = time;

		ROS_INFO( "%10.9f, %10.9f, %10.9f, %10.9f", time, s.response.position, s.response.velocity, command.torque );

                command_pub.publish( command );

	        //command.torque = control( state.response.time, state.response.position, state.response.velocity );
  	        //command.time = state.response.time;
            } else {
               ROS_INFO("Failed to call");
	    }
        } else {
            ROS_INFO("Persistent client is invalid");
        }
/*
        if( !state_client.isValid( ) ) {
	    ROS_INFO( "state_client not valid" );
	}

        if( !state_client.exists( ) )
            state_client.waitForExistence( );
	    
	if( state_client.call( s ) ) {
	//if( state_client.call( state ) ) {
	    ROS_INFO( "called state" );

	    command.torque = control( s.response.time, s.response.position, s.response.velocity );
  	    command.time = s.response.time;

	    //command.torque = control( state.response.time, state.response.position, state.response.velocity );
  	    //command.time = state.response.time;
        } else {
	    ROS_INFO( "calling state service failed" );
        }
        command_pub.publish( command );
*/
        ros::spinOnce( );
	count++;
    }

    ROS_INFO( "Controller shutting down" );

    return 0;
}
 cob_trajectory_controller_node():
 //as_(n_, "joint_trajectory_action", boost::bind(&cob_trajectory_controller_node::executeTrajectory, this, _1), true),
 as_follow_(n_, "follow_joint_trajectory", boost::bind(&cob_trajectory_controller_node::executeFollowTrajectory, this, _1), true),
 //action_name_("joint_trajectory_action"),
 action_name_follow_("follow_joint_trajectory")
 {
     joint_vel_pub_ = n_.advertise<brics_actuator::JointVelocities>("command_vel", 1);
     joint_state_sub_ = n_.subscribe("/joint_states", 1, &cob_trajectory_controller_node::joint_state_callback, this);
     controller_state_ = n_.subscribe("state", 1, &cob_trajectory_controller_node::state_callback, this);
     operation_mode_ = n_.subscribe("current_operationmode", 1, &cob_trajectory_controller_node::operationmode_callback, this);
     srvServer_Stop_ = n_.advertiseService("stop", &cob_trajectory_controller_node::srvCallback_Stop, this);
     srvServer_SetVel_ = n_.advertiseService("set_joint_velocity", &cob_trajectory_controller_node::srvCallback_setVel, this);
     srvServer_SetAcc_ = n_.advertiseService("set_joint_acceleration", &cob_trajectory_controller_node::srvCallback_setAcc, this);
     srvClient_SetOperationMode = n_.serviceClient<cob_srvs::SetOperationMode>("set_operation_mode");
     while(!srvClient_SetOperationMode.exists())
     {
         ROS_INFO("Waiting for operationmode service to become available");
         sleep(1);
     }
     executing_ = false;
     failure_ = false;
     rejected_ = false;
     preemted_ = false;
     watchdog_counter = 0;
     current_operation_mode_ = "undefined";
     double PTPvel = 0.7;
     double PTPacc = 0.2;
     double maxError = 0.7;
     double overlap_time = 0.4;
     velocity_timeout_ = 2.0;
     DOF = 7;
     // get JointNames from parameter server
     ROS_INFO("getting JointNames from parameter server");
     if (n_.hasParam("joint_names"))
     {
         n_.getParam("joint_names", JointNames_param_);
     }
     else
     {
         ROS_ERROR("Parameter joint_names not set");
     }
     JointNames_.resize(JointNames_param_.size());
     for (int i = 0; i<JointNames_param_.size(); i++ )
     {
         JointNames_[i] = (std::string)JointNames_param_[i];
     }
     DOF = JointNames_param_.size();
     if (n_.hasParam("ptp_vel"))
     {
         n_.getParam("ptp_vel", PTPvel);
     }
     if (n_.hasParam("ptp_acc"))
     {
         n_.getParam("ptp_acc", PTPacc);
     }
     if (n_.hasParam("max_error"))
     {
         n_.getParam("max_error", maxError);
     }
     if (n_.hasParam("overlap_time"))
     {
         n_.getParam("overlap_time", overlap_time);
     }
     q_current.resize(DOF);
     ROS_INFO("starting controller with DOF: %d PTPvel: %f PTPAcc: %f maxError %f", DOF, PTPvel, PTPacc, maxError);
     traj_generator_ = new genericArmCtrl(DOF, PTPvel, PTPacc, maxError);
     traj_generator_->overlap_time = overlap_time;
 }