From 02f2823ec7b70baf408608cffcced8e353484bdf Mon Sep 17 00:00:00 2001 From: Louis-Guillaume DUBOIS Date: Tue, 12 May 2015 20:35:01 +0200 Subject: [PATCH] adds other command nodes for testing --- CMakeLists.txt | 6 ++ launch/all-new-1d.launch | 34 +++++++ launch/all-new.launch | 34 +++++++ src/commande-new-1d.cpp | 211 +++++++++++++++++++++++++++++++++++++++ src/commande-new.cpp | 211 +++++++++++++++++++++++++++++++++++++++ 5 files changed, 496 insertions(+) create mode 100644 launch/all-new-1d.launch create mode 100644 launch/all-new.launch create mode 100644 src/commande-new-1d.cpp create mode 100644 src/commande-new.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 78bfffb..5597922 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -62,3 +62,9 @@ add_dependencies(normal_estimator hand_control_generate_messages_cpp) add_executable(commande src/commande.cpp) target_link_libraries(commande ${catkin_LIBRARIES}) + +add_executable(commande-new src/commande-new.cpp) +target_link_libraries(commande-new ${catkin_LIBRARIES}) + +add_executable(commande-new-1d src/commande-new-1d.cpp) +target_link_libraries(commande-new-1d ${catkin_LIBRARIES}) diff --git a/launch/all-new-1d.launch b/launch/all-new-1d.launch new file mode 100644 index 0000000..3704926 --- /dev/null +++ b/launch/all-new-1d.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/all-new.launch b/launch/all-new.launch new file mode 100644 index 0000000..49c3677 --- /dev/null +++ b/launch/all-new.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/commande-new-1d.cpp b/src/commande-new-1d.cpp new file mode 100644 index 0000000..65fedf6 --- /dev/null +++ b/src/commande-new-1d.cpp @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +class Run +{ + private: + float xx, yy, dz; + + // xx > 0 : forward + // xx < 0 : backward + + // yy > 0 : right + // yy < 0 : left + + // dz > 0 : up + // dz < 0 : down + + float plan_vel, z_vel; + + // to calculate dz + float z_current, z_previous; + ros::Time t_current, t_previous; + + // conditions to publish a message + float max_curv; + float dz_min, x_dev_min, y_dev_min; + uint64_t min_number; + + bool first_msg; + + ros::Publisher pub; + + void publish() + { + geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); + if (fabs(dz) > dz_min) + { + mvt->linear.z = dz * z_vel; + } + if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) + { + mvt->linear.y = yy * plan_vel; + } + else if (fabs(xx) > x_dev_min) + { + mvt->linear.x = xx * plan_vel; + } + pub.publish(mvt); + ROS_INFO("cmd published"); + }//end publish + + public: + Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, + const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation, + const float& dz_minimal_difference, const int& min_points_number) : + pub(cmd_publisher), + plan_vel(plan_velocity), + max_curv(max_curvature), + z_vel(z_velocity), + xx(0), + yy(0), + dz(0), + x_dev_min(x_minimal_deviation), + y_dev_min(y_minimal_deviation), + dz_min(dz_minimal_difference), + first_msg(true), + min_number(min_points_number){ + z_current = z_previous = std::numeric_limits::signaling_NaN(); + t_previous.nsec = t_previous.sec = + t_previous.nsec = t_previous.sec = std::numeric_limits::signaling_NaN(); + } + + void callback(const hand_control::Plan::ConstPtr& msg) + { + ROS_INFO("plan received"); + if (msg->curvature < max_curv && msg->number > min_number) + { + t_current = msg->header.stamp; + z_current = msg->altitude; + + if (!first_msg) + { + dz = (z_current - z_previous)/((t_current - t_previous).toSec()); + ROS_INFO("dz = %f", dz); + } + + if(msg->normal.z > 0) + { + yy = msg->normal.x; + xx = msg->normal.y; + } + else + { + yy = - msg->normal.x; + xx = - msg->normal.y; + } + + t_previous = t_current; + z_previous = z_current; + z_current = std::numeric_limits::signaling_NaN(); + t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN(); + if (first_msg) + { + first_msg = false; + ROS_INFO("first msg received"); + } + ROS_INFO("coords updated"); + } else { + xx = yy = dz = 0.; + } + publish(); + }; + + void run() + { + ros::spin(); + } + +}; + +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); + } + + 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 dz_dev_min(0); + if (node.getParam("dz_dev_min", dz_dev_min)) + { + ROS_INFO("dz_dev_min : %f" , dz_dev_min); + } else { + node.setParam("dz_dev_min", 0.05); + node.getParam("dz_dev_min", dz_dev_min); + ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min); + } + + ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); + Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number); + ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); + run.run(); + return 0; +} diff --git a/src/commande-new.cpp b/src/commande-new.cpp new file mode 100644 index 0000000..f84b743 --- /dev/null +++ b/src/commande-new.cpp @@ -0,0 +1,211 @@ +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +class Run +{ + private: + float xx, yy, dz; + + // xx > 0 : forward + // xx < 0 : backward + + // yy > 0 : right + // yy < 0 : left + + // dz > 0 : up + // dz < 0 : down + + float plan_vel, z_vel; + + // to calculate dz + float z_current, z_previous; + ros::Time t_current, t_previous; + + // conditions to publish a message + float max_curv; + float dz_min, x_dev_min, y_dev_min; + uint64_t min_number; + + bool first_msg; + + ros::Publisher pub; + + void publish() + { + geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); + if (fabs(dz) > dz_min) + { + mvt->linear.z = dz * z_vel; + } + if (fabs(xx) > x_dev_min) + { + mvt->linear.x = xx * plan_vel; + } + if (fabs(yy) > y_dev_min) + { + mvt->linear.y = yy * plan_vel; + } + pub.publish(mvt); + ROS_INFO("cmd published"); + }//end publish + + public: + Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, + const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation, + const float& dz_minimal_difference, const int& min_points_number) : + pub(cmd_publisher), + plan_vel(plan_velocity), + max_curv(max_curvature), + z_vel(z_velocity), + xx(0), + yy(0), + dz(0), + x_dev_min(x_minimal_deviation), + y_dev_min(y_minimal_deviation), + dz_min(dz_minimal_difference), + first_msg(true), + min_number(min_points_number){ + z_current = z_previous = std::numeric_limits::signaling_NaN(); + t_previous.nsec = t_previous.sec = + t_previous.nsec = t_previous.sec = std::numeric_limits::signaling_NaN(); + } + + void callback(const hand_control::Plan::ConstPtr& msg) + { + ROS_INFO("plan received"); + if (msg->curvature < max_curv && msg->number > min_number) + { + t_current = msg->header.stamp; + z_current = msg->altitude; + + if (!first_msg) + { + dz = (z_current - z_previous)/((t_current - t_previous).toSec()); + ROS_INFO("dz = %f", dz); + } + + if(msg->normal.z > 0) + { + yy = msg->normal.x; + xx = msg->normal.y; + } + else + { + yy = - msg->normal.x; + xx = - msg->normal.y; + } + + t_previous = t_current; + z_previous = z_current; + z_current = std::numeric_limits::signaling_NaN(); + t_current.nsec = t_current.sec = std::numeric_limits::signaling_NaN(); + if (first_msg) + { + first_msg = false; + ROS_INFO("first msg received"); + } + ROS_INFO("coords updated"); + } else { + xx = yy = dz = 0.; + } + publish(); + }; + + void run() + { + ros::spin(); + } + +}; + +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); + } + + 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 dz_dev_min(0); + if (node.getParam("dz_dev_min", dz_dev_min)) + { + ROS_INFO("dz_dev_min : %f" , dz_dev_min); + } else { + node.setParam("dz_dev_min", 0.05); + node.getParam("dz_dev_min", dz_dev_min); + ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min); + } + + ros::Publisher cmd_pub = node.advertise("/cmd_vel", 1); + Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number); + ros::Subscriber plan_sub = node.subscribe("input", 1, &Run::callback, &run); + run.run(); + return 0; +}