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