diff --git a/waypoint_nav/param/test.yaml b/waypoint_nav/param/test.yaml new file mode 100644 index 0000000..2a2d2a1 --- /dev/null +++ b/waypoint_nav/param/test.yaml @@ -0,0 +1,7 @@ +waypoints: +- point: {x: 0, y: 0, z: 0, vel: 1, rad: 0.3} +finish_pose: + header: {seq: 0, stamp: 0.000000000, frame_id: } + pose: + position: {x: 0, y: 0, z: 0} + orientation: {x: 0, y: 0, z: 0, w: 0} diff --git a/waypoint_nav/param/waypoints.yaml b/waypoint_nav/param/waypoints.yaml index efa7a38..e061bb1 100644 --- a/waypoint_nav/param/waypoints.yaml +++ b/waypoint_nav/param/waypoints.yaml @@ -1,8 +1,8 @@ waypoints: -- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad:0.5} -- point: {x: 0.203745, y: -0.496466, z: 0.0, vel: 0.5, rad:0.2} -- point: {x: 0.666450, y: 0.504773, z: 0.0, vel: 0.5, rad:0.3} -- point: {x: 0.515219, y: 0.515219, z: 0.0, vel: 1.0, rad:0.5} +- point: {x: -0.996902, y: -0.499032, z: 0.0, vel: 1.0, rad: 0.5} +- point: {x: 0.203745, y: -0.496466, z: 0.0, vel: 0.5, rad: 0.2} +- point: {x: 0.666450, y: 0.504773, z: 0.0, vel: 0.5, rad: 0.3} +- point: {x: 0.515219, y: 0.515219, z: 0.0, vel: 1.0, rad: 0.5} finish_pose: header: {seq: 0.0, stamp: 1.6203674303803937E9, frame_id: base_link} pose: diff --git a/waypoint_nav/src/waypoint_nav.cpp b/waypoint_nav/src/waypoint_nav.cpp index e69de29..ffed191 100644 --- a/waypoint_nav/src/waypoint_nav.cpp +++ b/waypoint_nav/src/waypoint_nav.cpp @@ -0,0 +1,10 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include \ No newline at end of file diff --git a/waypoint_saver/CMakeLists.txt b/waypoint_saver/CMakeLists.txt index 497b438..aac7f4d 100644 --- a/waypoint_saver/CMakeLists.txt +++ b/waypoint_saver/CMakeLists.txt @@ -1,150 +1,36 @@ cmake_minimum_required(VERSION 3.0.2) project(waypoint_saver) -## Compile as C++11, supported in ROS Kinetic and newer -# add_compile_options(-std=c++11) - -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED) - -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +find_package(catkin REQUIRED + geometry_msgs + roscpp + tf +) -################################################ -## Declare ROS messages, services and actions ## -################################################ -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( -# INCLUDE_DIRS include -# LIBRARIES waypoint_saver -# CATKIN_DEPENDS other_catkin_pkg -# DEPENDS system_lib + #INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ) -########### -## Build ## -########### -## Specify additional locations of header files -## Your package locations should be listed before other locations + include_directories( -# include -# ${catkin_INCLUDE_DIRS} + #include + ${catkin_INCLUDE_DIRS} + ${yaml-cpp_INCLUDE_DIRS} ) -## Declare a C++ library -# add_library(${PROJECT_NAME} -# src/${PROJECT_NAME}/waypoint_saver.cpp -# ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/waypoint_saver_node.cpp) -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") +add_executable(waypoint_saver src/waypoint_saver.cpp) -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) +target_link_libraries(waypoint_saver + ${catkin_LIBRARIES} +) ############# ## Install ## diff --git a/waypoint_saver/launch/waypoint_saver.launch b/waypoint_saver/launch/waypoint_saver.launch index e69de29..c119722 100644 --- a/waypoint_saver/launch/waypoint_saver.launch +++ b/waypoint_saver/launch/waypoint_saver.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/waypoint_saver/package.xml b/waypoint_saver/package.xml index e64c74b..ad87480 100644 --- a/waypoint_saver/package.xml +++ b/waypoint_saver/package.xml @@ -16,41 +16,18 @@ TODO - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - catkin + geometry_msgs + roscpp + tf + + geometry_msgs + roscpp + tf + + diff --git a/waypoint_saver/src/waypoint_saver.cpp b/waypoint_saver/src/waypoint_saver.cpp index e69de29..566ea05 100644 --- a/waypoint_saver/src/waypoint_saver.cpp +++ b/waypoint_saver/src/waypoint_saver.cpp @@ -0,0 +1,201 @@ +#include +#include + +#include +#include +#include + +#include +#include +#include + + +class Waypoint : public geometry_msgs::PointStamped { +public: + float vel; + float rad; + + Waypoint() : vel(1.0), rad(0.8) {} + + double get_x() { return point.x; } + double get_y() { return point.y; } + double get_z() { return point.z; } + float get_vel() { return vel; } + float get_rad() { return rad; } + + void set_x(double x) { point.x = x; } + void set_y(double y) { point.x = y; } + void set_z(double z) { point.x = z; } + void set_vel(float v) { vel = v; } + void set_rad(float r) { rad = r; } +}; + + + +class WaypointsSaver{ +public: + WaypointsSaver() : + filename_("waypoints.yaml") + { + waypoints_viz_sub_ = nh_.subscribe("waypoints_viz", 1, &WaypointsSaver::waypointsVizCallback, this); + waypoints_joy_sub_ = nh_.subscribe("waypoints_joy", 1, &WaypointsSaver::waypointsJoyCallback, this); + finish_pose_sub_ = nh_.subscribe("finish_pose", 1, &WaypointsSaver::finishPoseCallback, this); + + ros::NodeHandle private_nh("~"); + private_nh.param("filename", filename_, filename_); + private_nh.param("save_joy_button", save_joy_button_, 0); + private_nh.param("robot_frame", robot_frame_, std::string("base_link")); + private_nh.param("world_frame", world_frame_, std::string("map")); + private_nh.param("default_rad", default_rad_, 0.8); + } + + + void waypointsJoyCallback(const sensor_msgs::Joy &msg) { + static ros::Time saved_time(0.0); + //ROS_INFO_STREAM("joy = " << msg); + if(msg.buttons[save_joy_button_] == 1 && (ros::Time::now() - saved_time).toSec() > 3.0){ + tf::StampedTransform robot_gl; + try{ + tf_listener_.lookupTransform(world_frame_, robot_frame_, ros::Time(0.0), robot_gl); + Waypoint point; + point.set_x(robot_gl.getOrigin().x()); // point.point.x = robot_gl.getOrigin().x(); + point.set_y(robot_gl.getOrigin().y()); //point.point.y = robot_gl.getOrigin().y(); + point.set_z(robot_gl.getOrigin().z()); //point.point.z = robot_gl.getOrigin().z(); + waypoints_.push_back(point); + saved_time = ros::Time::now(); + }catch(tf::TransformException &e){ + ROS_WARN_STREAM("tf::TransformException: " << e.what()); + } + } + } + + + void waypointsVizCallback(const geometry_msgs::PointStamped &msg) { + Waypoint point; + point.set_x(msg.point.x); + point.set_y(msg.point.y); + point.set_z(msg.point.z); + ROS_INFO_STREAM("point = " << point); + waypoints_.push_back(point); + } + + + void finishPoseCallback(const geometry_msgs::PoseStamped &msg) { + finish_pose_ = msg; + save2(); + waypoints_.clear(); + } + + + void save() { + std::ofstream ofs(filename_.c_str(), std::ios::out); + + ofs << "waypoints:" << std::endl; + for(int i=0; i < waypoints_.size(); i++){ + ofs << " " << "- point:" << std::endl; + ofs << " x: " << waypoints_[i].point.x << std::endl; + ofs << " y: " << waypoints_[i].point.y << std::endl; + ofs << " z: " << waypoints_[i].point.z << std::endl; + } + + ofs << "finish_pose:" << std::endl; + ofs << " header:" << std::endl; + ofs << " seq: " << finish_pose_.header.seq << std::endl; + ofs << " stamp: " << finish_pose_.header.stamp << std::endl; + ofs << " frame_id: " << finish_pose_.header.frame_id << std::endl;; + ofs << " pose:" << std::endl; + ofs << " position:" << std::endl; + ofs << " x: " << finish_pose_.pose.position.x << std::endl; + ofs << " y: " << finish_pose_.pose.position.y << std::endl; + ofs << " z: " << finish_pose_.pose.position.z << std::endl; + ofs << " orientation:" << std::endl; + ofs << " x: " << finish_pose_.pose.orientation.x << std::endl; + ofs << " y: " << finish_pose_.pose.orientation.y << std::endl; + ofs << " z: " << finish_pose_.pose.orientation.z << std::endl; + ofs << " w: " << finish_pose_.pose.orientation.w << std::endl; + + ofs.close(); + + ROS_INFO_STREAM("write success"); + } + + + void save2() { + Waypoint point; + point.set_x(0.0); + point.set_y(0.0); + point.set_z(0.0); + waypoints_.push_back(point); + + std::ofstream ofs(filename_.c_str(), std::ios::out); + /* + waypoints: + - point: {x: *, y: *, z: *, vel: *, rad: *} + */ + ofs << "waypoints:" << std::endl; + for(int i=0; i < waypoints_.size(); i++) { + ofs << "- point: {" ; + ofs << "x: " << waypoints_[i].get_x() << ", " ; + ofs << "y: " << waypoints_[i].get_y() << ", " ; + ofs << "z: " << waypoints_[i].get_z() << ", " ; + ofs << "vel: " << waypoints_[i].get_vel() << ", " ; + ofs << "rad: " << waypoints_[i].get_rad() << "}" << std::endl; + } + /* + finish_pose: + header: {seq: *, stamp: *, frame_id: *} + pose: + positioin: {x: *, y: *, z: *} + orientation: {x: *, y: *, z: *, w: *} + */ + ofs << "finish_pose:" << std::endl; + ofs << " header: {" ; + ofs << "seq: " << finish_pose_.header.seq << ", " ; + ofs << "stamp: " << finish_pose_.header.stamp << ", " ; + ofs << "frame_id: " << finish_pose_.header.frame_id << "}" << std::endl; + ofs << " pose: " << std::endl; + ofs << " position: {" ; + ofs << "x: " << finish_pose_.pose.position.x << ", " ; + ofs << "y: " << finish_pose_.pose.position.y << ", " ; + ofs << "z: " << finish_pose_.pose.position.z << "}" << std::endl; + ofs << " orientation: {" ; + ofs << "x: " << finish_pose_.pose.orientation.x << ", " ; + ofs << "y: " << finish_pose_.pose.orientation.y << ", " ; + ofs << "z: " << finish_pose_.pose.orientation.z << ", " ; + ofs << "w: " << finish_pose_.pose.orientation.w << "}" << std::endl; + + ofs.close(); + ROS_INFO_STREAM("write success"); + } + + + void run() { + ros::spin(); + } + + +private: + ros::Subscriber waypoints_viz_sub_; + ros::Subscriber waypoints_joy_sub_; + ros::Subscriber finish_pose_sub_; + std::vector waypoints_; + geometry_msgs::PoseStamped finish_pose_; + tf::TransformListener tf_listener_; + int save_joy_button_; + ros::NodeHandle nh_; + std::string filename_; + std::string world_frame_; + std::string robot_frame_; + float default_rad_; +}; + + + +int main(int argc, char *argv[]) { + ros::init(argc, argv, "waypoints_saver"); + WaypointsSaver saver; + saver.save2(); + saver.run(); + + return 0; +} \ No newline at end of file