From b19ea6355c79e9ae31f9030a2c4aa1295f15137e Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Thu, 28 May 2015 21:53:07 +0200 Subject: [PATCH] adds commander_atan node to replace commander node --- CMakeLists.txt | 6 ++ cfg/Commander.cfg | 14 ++--- cfg/Commander_atan.cfg | 16 +++++ src/commander_atan.cpp | 140 +++++++++++++++++++++++++++++++++++++++++ src/data.h | 9 +++ 5 files changed, 178 insertions(+), 7 deletions(-) create mode 100755 cfg/Commander_atan.cfg create mode 100644 src/commander_atan.cpp create mode 100644 src/data.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 570e32d..a3641c6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -53,6 +53,10 @@ target_link_libraries(keyboard_cmd display ${catkin_LIBRARIES} ${ncursesw_LIBRAR target_link_libraries(estimator ${catkin_LIBRARIES}) add_dependencies(estimator hand_control_generate_messages_cpp) + add_executable(commander_atan src/commander_atan.cpp) +target_link_libraries(commander_atan ${catkin_LIBRARIES}) +add_dependencies(commander_atan hand_control_generate_messages_cpp) + add_executable(commander src/commander.cpp) target_link_libraries(commander ${catkin_LIBRARIES}) add_dependencies(commander hand_control_generate_messages_cpp) @@ -62,11 +66,13 @@ generate_dynamic_reconfigure_options( cfg/Filter.cfg cfg/Commander.cfg cfg/Estimator.cfg + cfg/Commander_atan.cfg ) add_dependencies(filter ${PROJECT_NAME}_gencfg) add_dependencies(commander ${PROJECT_NAME}_gencfg) add_dependencies(estimator ${PROJECT_NAME}_gencfg) +add_dependencies(commander_atan ${PROJECT_NAME}_gencfg) catkin_package( CATKIN_DEPENDS message_runtime diff --git a/cfg/Commander.cfg b/cfg/Commander.cfg index fb0a605..f5b3b60 100755 --- a/cfg/Commander.cfg +++ b/cfg/Commander.cfg @@ -3,14 +3,14 @@ PACKAGE = "hand_control" from dynamic_reconfigure.parameter_generator_catkin import * gen = ParameterGenerator() gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.) -gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.05, 0.) -gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.05, 0.) +gen.add("x_minimal_deviation", double_t, 0, "Absolute horizontal movement detection treshold", 0.1, 0.) +gen.add("y_minimal_deviation", double_t, 0, "Absolute lateral movement detection treshold", 0.1, 0.) gen.add("z_minimal_deviation", double_t, 0, "Absolute vertical movement detection treshold", 0.1, 0.) gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.) -gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 2000, 0) +gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0) gen.add("up_fact", double_t, 0, "Upward command amplification factor", 1.5, 1) -gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 5., 0., 45.) -gen.add("angle_vel", double_t, 0, "Angular velocity", 0.002, 0., 10.) -gen.add("plan_vel", double_t, 0, "Translation velocity", 2, 0., 10.) -gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2, 0., 10.) +gen.add("theta_minimal_deviation", double_t, 0, "Absolute angular movement detection treshold", 15., 0., 45.) +gen.add("angle_vel", double_t, 0, "Angular velocity", 0.01, 0., 10.) +gen.add("plan_vel", double_t, 0, "Translation velocity", 0.5, 0., 10.) +gen.add("z_vel", double_t, 0, "Vertical translation velocity", 2., 0., 10.) exit(gen.generate(PACKAGE, "hand_control", "Commander")) diff --git a/cfg/Commander_atan.cfg b/cfg/Commander_atan.cfg new file mode 100755 index 0000000..0a27756 --- /dev/null +++ b/cfg/Commander_atan.cfg @@ -0,0 +1,16 @@ +#!/usr/bin/env python +PACKAGE = "hand_control" +from dynamic_reconfigure.parameter_generator_catkin import * +gen = ParameterGenerator() +gen.add("max_curvature", double_t, 0, "Maximum curvature of the estimated plane", 0.5, 0., 1.) +gen.add("neutral_alt", double_t, 0, "Reference altitude for vertical movement command", 0.8, 0.) +gen.add("min_points_number", int_t, 0, "Minimal number of plane points needed for a valid estimation", 1000, 0) +gen.add("p_x", double_t, 0, "x gradient in 0", 0.5, 0.) +gen.add("p_y", double_t, 0, "y gradient in 0", 0.5, 0.) +gen.add("p_z", double_t, 0, "z gradient in 0", 0.5, 0.) +gen.add("p_th", double_t, 0, "th gradient in 0", 0.01, 0.) +gen.add("x_min", double_t, 0, "min x coord to be published", 0.1, 0., 1.) +gen.add("y_min", double_t, 0, "min y coord to be published", 0.1, 0., 1.) +gen.add("z_min", double_t, 0, "min z coord to be published", 0.1, 0., 1.) +gen.add("th_min", double_t, 0, "min th coord to be published", 0.1, 0., 1.) +exit(gen.generate(PACKAGE, "hand_control", "Commander_atan")) diff --git a/src/commander_atan.cpp b/src/commander_atan.cpp new file mode 100644 index 0000000..1b1e66d --- /dev/null +++ b/src/commander_atan.cpp @@ -0,0 +1,140 @@ +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include "data.h" + +class Run +{ + private: + class Function + { + private: + float t; + static const float demi_pi; + public: + float operator()(float arg) + { + return atan(t * arg) / demi_pi; + } + Function() : t(demi_pi){} + void set_t(float tt) + { + t = tt; + } + void set_p(float pp) + // pp : gradient in 0 + { + t = pp*demi_pi; + } + }; + + Data f; + Data min; + float neutral_z; + float max_curv; + uint64_t min_number; + ros::Publisher pub; + + public: + Run(const ros::Publisher& cmd_publisher) : + pub(cmd_publisher) { + f.x = f.y = f.z = f.th = Function(); + min.x = min.y = min.z = min.th = 0.1; + } + + void callback(const hand_control::Plan::ConstPtr& msg) + { + ROS_INFO("plan received"); + Data in, out; + in.x = in.y = in.z = in.th = 0.; + out.x = out.y = out.z = out.th = 0; + if (msg->curvature < max_curv && msg->number > min_number) + { + if(msg->normal.z > 0) + { + in.y = msg->normal.x; + in.x = msg->normal.y; + } else { + in.y = - msg->normal.x; + in.x = - msg->normal.y; + } + in.z = msg->altitude - neutral_z; + in.th = msg->angle; + } + + geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); + + if (fabs(in.y) > fabs(in.x)) + { + out.y = f.y(in.y); + if (out.y > min.y) + mvt->linear.y = out.y; + } else { { + out.x = f.x(in.x); + if (out.x > min.x) + mvt->linear.x = out.x; + } + + out.z = f.z(in.z); + if (out.z > min.z) + mvt->linear.z = out.z; + + out.th = f.th(in.th); + if (out.th > min.th) + mvt->angular.z = out.th; + + pub.publish(mvt); + ROS_INFO("cmd published"); + } + } + + void reconfigure(const hand_control::Commander_atanConfig& c, const uint32_t& level) + { + max_curv = c.max_curvature; + neutral_z = c.neutral_alt; + min_number = c.min_points_number; + f.x.set_p(c.p_x); + f.y.set_p(c.p_y); + f.z.set_p(c.p_z); + f.th.set_p(c.p_th); + min.x = c.x_min; + min.y = c.y_min; + min.z = c.z_min; + min.th = c.th_min; + } + +}; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "commander_atan"); + ros::NodeHandle node("commander_atan"); + + ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); + Run run(cmd_pub); + ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); + + dynamic_reconfigure::Server server; + dynamic_reconfigure::Server::CallbackType f; + f = boost::bind(&Run::reconfigure, &run, _1, _2); + server.setCallback(f); + + ROS_INFO("start spinning"); + ros::spin(); + return 0; +} + +const float Run::Function::demi_pi = 2*atan(1.); diff --git a/src/data.h b/src/data.h new file mode 100644 index 0000000..8fe89bc --- /dev/null +++ b/src/data.h @@ -0,0 +1,9 @@ +#ifndef DATA_H +#define DATA_H +template +struct Data +{ + public: + T x, y, z, th; +}; +#endif