diff --git a/src/commande-abs.cpp b/src/commande-abs.cpp index cc6cd61..fd36d36 100644 --- a/src/commande-abs.cpp +++ b/src/commande-abs.cpp @@ -71,26 +71,9 @@ class Run }//end publish public: - Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, const float& angle_velocity, - const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation, - const float& z_minimal_deviation, const float& neutral_alt, const float& theta_minimal_deviation, - const int& min_points_number, const float& up_fact) : - pub(cmd_publisher), - plan_vel(plan_velocity), - angle_vel(angle_velocity); - max_curv(max_curvature), - z_vel(z_velocity), - xx(0), - yy(0), - zz(0), - theta(0), - x_dev_min(x_minimal_deviation), - y_dev_min(y_minimal_deviation), - z_dev_min(z_minimal_deviation), - neutral_z(neutral_alt), - first_msg(true), - min_number(min_points_number), - up_factor(up_fact) + Run(const ros::Publisher& cmd_publisher) : + pub(cmd_publisher) + {} void callback(const hand_control::Plan::ConstPtr& msg) { @@ -148,118 +131,8 @@ int main(int argc, char** argv) ros::init(argc, argv, "commande"); ros::NodeHandle node("commande"); - double max_curv(0); - if (node.getParam("max_curv", max_curv)) - { - ROS_INFO("max_curv : %f" , max_curv); - } else { - node.setParam("max_curv", 0.08); - node.getParam("max_curv", max_curv); - ROS_INFO("max_curv : %f (default value)", max_curv); - } - - double plan_vel(0); - if (node.getParam("plan_vel", plan_vel)) - { - ROS_INFO("plan_vel : %f" , plan_vel); - } else { - node.setParam("plan_vel", 0.8); - node.getParam("plan_vel", plan_vel); - ROS_INFO("plan_vel : %f (default value)", plan_vel); - } - - double z_vel(0); - if (node.getParam("z_vel", z_vel)) - { - ROS_INFO("z_vel : %f" , z_vel); - } else { - node.setParam("z_vel", 0.8); - node.getParam("z_vel", z_vel); - ROS_INFO("z_vel : %f (default value)", z_vel); - } - - double angle_vel(0); - if (node.getParam("angle_vel", angle_vel)) - { - ROS_INFO("angle_vel : %f" , angle_vel); - } else { - node.setParam("angle_vel", 10); - node.getParam("angle_vel", angle_vel); - ROS_INFO("angle_vel : %f (default value)", angle_vel); - } - - int min_number(0); - if (node.getParam("min_number", min_number)) - { - ROS_INFO("min_number : %d" , min_number); - } else { - node.setParam("min_number", 500); - node.getParam("min_number", min_number); - ROS_INFO("min_number : %d (default value)", min_number); - } - - double x_dev_min(0); - if (node.getParam("x_dev_min", x_dev_min)) - { - ROS_INFO("x_dev_min : %f" , x_dev_min); - } else { - node.setParam("x_dev_min", 0.05); - node.getParam("x_dev_min", x_dev_min); - ROS_INFO("x_dev_min : %f (default value)", x_dev_min); - } - - double y_dev_min(0); - if (node.getParam("y_dev_min", y_dev_min)) - { - ROS_INFO("y_dev_min : %f" , y_dev_min); - } else { - node.setParam("y_dev_min", 0.05); - node.getParam("y_dev_min", y_dev_min); - ROS_INFO("y_dev_min : %f (default value)", y_dev_min); - } - - double z_dev_min(0); - if (node.getParam("z_dev_min", z_dev_min)) - { - ROS_INFO("z_dev_min : %f" , z_dev_min); - } else { - node.setParam("z_dev_min", 0.1); - node.getParam("z_dev_min", z_dev_min); - ROS_INFO("z_dev_min : %f (default value)", z_dev_min); - } - - double th_dev_min(0); - if (node.getParam("th_dev_min", th_dev_min)) - { - ROS_INFO("th_dev_min : %f" , th_dev_min); - } else { - node.setParam("th_dev_min", 15); - node.getParam("th_dev_min", th_dev_min); - ROS_INFO("th_dev_min : %f (default value)", th_dev_min); - } - - double neutral_z(0); - if (node.getParam("neutral_z", neutral_z)) - { - ROS_INFO("neutral_z : %f" , neutral_z); - } else { - node.setParam("neutral_z", 1.5); - node.getParam("neutral_z", neutral_z); - ROS_INFO("neutral_z : %f (default value)", neutral_z); - } - - double up_fact(0); - if (node.getParam("up_fact", up_fact)) - { - ROS_INFO("up_fact : %f" , up_fact); - } else { - node.setParam("up_fact", 1.5); - node.getParam("up_fact", up_fact); - ROS_INFO("up_fact : %f (default value)", up_fact); - } - ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); - Run run(cmd_pub, max_curv, plan_vel, z_vel, angle_vel, x_dev_min, y_dev_min, z_dev_min, th_dev_min, neutral_z, min_number, up_fact); + Run run(cmd_pub); ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); dynamic_reconfigure::Server server;