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