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; } }
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; }