std::string CursynthGui::getPrevControl() {
control_index_ = (control_index_ + control_order_.size() - 1) %
control_order_.size();
return getCurrentControl();
}
AlgorithmManager::AlgorithmManager(ros::NodeHandle& node):
algorithm_mode_(UNINITIALIZED)
{
ros::NodeHandle private_nh("~");
edo_core_msgs::JointsNumber::Request req;
edo_core_msgs::JointsNumber::Response res;
//subscribe to /movement_comm, published by state machine and /jnt_state from rosserial
ros::Subscriber move_control_sub = node.subscribe("machine_move", 100, &AlgorithmManager::moveCallback, this);
ros::Subscriber jog_control_sub = node.subscribe("machine_jog", 100, &AlgorithmManager::jogCallback, this);
ros::Subscriber jnt_state_subscriber = node.subscribe("machine_algo_jnt_state", 100, &AlgorithmManager::stateCallback, this);
ros::Subscriber jnt_calib_subscriber = node.subscribe("machine_jnt_calib", 100, &AlgorithmManager::calibCallback, this);
feedback_publisher_ = node.advertise<edo_core_msgs::MovementFeedback>("algo_movement_ack", 200);
cartesian_pose_pub_ = node.advertise<edo_core_msgs::CartesianPose>("cartesian_pose", 100);
algorithm_state_pub_ = node.advertise<std_msgs::Int8>("algorithm_state", 100);
//create a ROS Service Server
ros::ServiceServer get_jnts_number_srv = node.advertiseService("algo_jnt_number_srv", &AlgorithmManager::getJointsNumber, this);
ros::ServiceServer get_direct_kinematics_srv = node.advertiseService("algo_direct_kinematics_srv", &AlgorithmManager::getDirectKinematics, this);
ros::ServiceServer get_inverse_kinematics_srv = node.advertiseService("algo_inverse_kinematics_srv", &AlgorithmManager::getInverseKinematics, this);
robot_switch_control_server = node.advertiseService("algo_control_switch_srv", &AlgorithmManager::SwitchControl, this);
//publish /jnt_ctrl to rosserial, then joints
ros::Publisher robot_control_publisher = node.advertise<edo_core_msgs::JointControlArray>("algo_jnt_ctrl", 100);
//memset(&next_move_request_, 0, sizeof(edo_core_msgs::MovementFeedback));
memset(&target_joint_, 0, sizeof(ORL_joint_value));
memset(&target_cart_, 0, sizeof(ORL_cartesian_position));
memset(&hold_position_, 0, sizeof(ORL_joint_value));
memset(¤t_state_, 0, sizeof(ORL_joint_value));
delay_ = 255;
waiting_ = false;
jog_state_ = false;
jog_carlin_state_ = false;
move_cb_state_ = false;
pause_state_ = false;
interpolation_data_.resize(3);
first_time_ = true;
pkg_path_ = ros::package::getPath("edo_core_pkg");
jointCalib_=0;
private_nh.param<double>("controller_frequency", controller_frequency_, 100);
private_nh.param<double>("state_saturation_threshold", state_saturation_threshold_, 0.5);
private_nh.param<int>("interpolation_time_step", interpolation_time_step_, 1);
// Duration, callback, calback-owner, oneshot, autostart
timerCalib_ = private_nh.createTimer(ros::Duration(0.5), &AlgorithmManager::timerCallback, this, true, false);
if (!initializeORL())
{
ROS_ERROR("Impossible to inizialize ORL");
return;
}
noCartPose = false;
if(!getJointsNumber(req, res))
{
ROS_ERROR("Impossible to get joints number");
return;
}
joints_number_ = res.counter;
ROS_INFO("ORL controller initialized...");
ros::Rate loop_rate(controller_frequency_);
get_Strk(strk_);
//test con spin asincrono
ros::AsyncSpinner aspin(2);
aspin.start();
while (ros::ok())
{
if (algorithm_mode_ != SWITCHED_OFF){
//update the control command
updateControl();
//publish the last control commands
robot_control_publisher.publish(getCurrentControl());
}
loop_rate.sleep();
}
}
