From c286d2e37d95ed6f00281c561666e623c8e75722 Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Wed, 14 Mar 2018 13:27:57 -0300 Subject: [PATCH] Initial version. --- .gitignore | 51 +++++++++ CMakeLists.txt | 177 ++++++++++++++++++++++++++++++ include/pose2d_trajectories/circle_path.h | 77 +++++++++++++ include/pose2d_trajectories/eight_path.h | 61 ++++++++++ package.xml | 58 ++++++++++ src/circle_path.cpp | 72 ++++++++++++ src/eight_path.cpp | 47 ++++++++ src/eight_trajectory.cpp | 94 ++++++++++++++++ src/pose2d_stamp.cpp | 96 ++++++++++++++++ 9 files changed, 733 insertions(+) create mode 100644 .gitignore create mode 100644 CMakeLists.txt create mode 100644 include/pose2d_trajectories/circle_path.h create mode 100644 include/pose2d_trajectories/eight_path.h create mode 100644 package.xml create mode 100644 src/circle_path.cpp create mode 100644 src/eight_path.cpp create mode 100644 src/eight_trajectory.cpp create mode 100644 src/pose2d_stamp.cpp diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..35d74bb --- /dev/null +++ b/.gitignore @@ -0,0 +1,51 @@ +devel/ +logs/ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py +build_isolated/ +devel_isolated/ + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..bca23a4 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,177 @@ +cmake_minimum_required(VERSION 2.8.3) +project(pose2d_trajectories) + +## 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 COMPONENTS roscpp) + +find_package(cmake_modules REQUIRED) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen REQUIRED) + + +## 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() + +################################################ +## 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 and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_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 +# ) + +################################### +## 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 you 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 ${PROJECT_NAME} +# CATKIN_DEPENDS other_catkin_pkg + DEPENDS eigen +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +# TODO: Check names of system library include directories (eigen) +include_directories( + include ${catkin_INCLUDE_DIRS} + ${Eigen_INCLUDE_DIRS} +) + +## Declare a cpp library +add_library(${PROJECT_NAME} + src/circle_path.cpp + src/eight_path.cpp +) + +## Declare a cpp executable +add_executable(eight_trajectory src/eight_trajectory.cpp) +add_executable(pose2d_stamp src/pose2d_stamp.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(pose2d_trajectories_node pose2d_trajectories_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} +) + +target_link_libraries(eight_trajectory + ${catkin_LIBRARIES} + ${eigen_LIBRARIES} + ${PROJECT_NAME} +) + +target_link_libraries(pose2d_stamp + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Mark cpp header files for installation +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_pose2d_trajectories.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/include/pose2d_trajectories/circle_path.h b/include/pose2d_trajectories/circle_path.h new file mode 100644 index 0000000..755939f --- /dev/null +++ b/include/pose2d_trajectories/circle_path.h @@ -0,0 +1,77 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Circle Path Class + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef CIRCLE_PATH_H +#define CIRCLE_PATH_H + +#include + +/** Circle path +* @author Walter Fetter Lages +*/ +class CirclePath +{ + Eigen::Vector2d pc_; + double phi0_; + double r_; + double w_; + + public: + + /** Build a circle path. + * @param pc circle center point. + * @param phi0 initial phase. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w); + + /** Build a circle path. + * @param p0 starting pose. + * @param r circle radius. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector3d &p0,double r,double w); + + /** Build a circle path. + * @param pc circle center point. + * @param p0 starting point. + * @param w angular velocity. + */ + CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w); + + /** Destroy a circle path. + */ + ~CirclePath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/include/pose2d_trajectories/eight_path.h b/include/pose2d_trajectories/eight_path.h new file mode 100644 index 0000000..401a331 --- /dev/null +++ b/include/pose2d_trajectories/eight_path.h @@ -0,0 +1,61 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Eigth Path Class + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#ifndef EIGHT_PATH_H +#define EIGHT_PATH_H + +#include + +/** 8 path +* @author Walter Fetter Lages +*/ +class EightPath +{ + CirclePath c1_; + CirclePath c2_; + double period_; + + public: + + /** Build an 8 path. + * @param pc center point. + * @param r radius. + * @param w angular velocity. + */ + EightPath(const Eigen::Vector2d &pc,double r,double w); + + /** Destroy an 8 path. + */ + ~EightPath(void) { }; + + /** Get path point. + * @param t path time. + * @return path point. + */ + Eigen::Vector3d point(double t) const; + + /** Get path steering controls. + * @param t path time. + * @return steering controls. + */ + Eigen::Vector2d steering(double t) const; +}; +#endif diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..e1b2664 --- /dev/null +++ b/package.xml @@ -0,0 +1,58 @@ + + + pose2d_trajectories + 3.0.0 + The pose2d_trajectories package + + + + + Walter Fetter Lages + + + + + + GPLv3 + + + + + + + + + + + + + Walter Fetter Lages + + + + + + + + + + + + + + catkin + eigen + geometry_msgs + + eigen + geometry_msgs + + + + + + + + + + diff --git a/src/circle_path.cpp b/src/circle_path.cpp new file mode 100644 index 0000000..9899cb5 --- /dev/null +++ b/src/circle_path.cpp @@ -0,0 +1,72 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Circle Path Class + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#define sqr(x) (x*x) +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +CirclePath::CirclePath(const Eigen::Vector2d &pc,double phi0,double r,double w) +{ + pc_=pc; + phi0_=phi0; + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector3d &p0,double r,double w) +{ + phi0_=p0[2]-sgn(w)*M_PI_2; + pc_[0]=p0[0]-r*cos(phi0_); + pc_[1]=p0[1]-r*sin(phi0_); + r_=r; + w_=w; +} + +CirclePath::CirclePath(const Eigen::Vector2d &pc,Eigen::Vector2d &p0,double w) +{ + pc_=pc; + w_=w; + phi0_=atan2(p0[1]-pc[1],p0[0]-pc[0]); + r_=sqrt(sqr(p0[1]-pc[1])+sqr(p0[0]-pc[0])); +} + +Eigen::Vector3d CirclePath::point(double t) const +{ + double wt=w_*t; + Eigen::Vector3d p; + + p[0]=pc_[0]+r_*cos(wt+phi0_); + p[1]=pc_[1]+r_*sin(wt+phi0_); + p[2]=wt+phi0_+sgn(w_)*M_PI_2; + + return p; +} + +Eigen::Vector2d CirclePath::steering(double t) const +{ + Eigen::Vector2d e(2); + + e[0]=w_*r_; + e[1]=w_; + + return e; +} diff --git a/src/eight_path.cpp b/src/eight_path.cpp new file mode 100644 index 0000000..fec44df --- /dev/null +++ b/src/eight_path.cpp @@ -0,0 +1,47 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Eigth Path Class + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#define sgn(x) ((x == 0.0)? 0.0:(x/fabs(x))) + +EightPath::EightPath(const Eigen::Vector2d &pc,double r,double w): +c1_(Eigen::Vector2d(pc[0],pc[1]+r),-M_PI_2*sgn(w),r,w), +c2_(Eigen::Vector2d(pc[0],pc[1]-r),2.5*M_PI*sgn(w),r,-w) +{ + period_=4*M_PI/w; +}; + +Eigen::Vector3d EightPath::point(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.point(tc); + else return c2_.point(tc); +} + +Eigen::Vector2d EightPath::steering(double t) const +{ + double tc=fmod(t,period_/2.0); + + if(int(t/(period_/2.0)) % 2 == 0) return c1_.steering(tc); + else return c2_.steering(tc); +} diff --git a/src/eight_trajectory.cpp b/src/eight_trajectory.cpp new file mode 100644 index 0000000..b521aa1 --- /dev/null +++ b/src/eight_trajectory.cpp @@ -0,0 +1,94 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Eigth Trajectory Node + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include +#include + +#include + +class EightTrajectory +{ + public: + EightTrajectory(ros::NodeHandle node); + ~EightTrajectory(void); + void setCommand(ros::Duration t); + + private: + ros::NodeHandle node_; + const EightPath *path; + + ros::Publisher commandPublisher; +}; + + +EightTrajectory::EightTrajectory(ros::NodeHandle node) +{ + node_=node; + Eigen::Vector2d pc; + double w; + double r; + + ros::param::get("~x",pc[0]); + ros::param::get("~y",pc[1]); + ros::param::get("~radius",r); + ros::param::get("~ang_vel",w); + + path=new EightPath(pc,r,w); + commandPublisher=node_.advertise("command",1000); +} + +EightTrajectory::~EightTrajectory(void) +{ + commandPublisher.shutdown(); + delete path; +} + +void EightTrajectory::setCommand(ros::Duration t) +{ + Eigen::Vector3d p=path->point(t.toSec()); + + geometry_msgs::Pose2D command; + command.x=p[0]; + command.y=p[1]; + command.theta=p[2]; + commandPublisher.publish(command); +} + +int main(int argc,char* argv[]) +{ + + ros::init(argc,argv,"eight_trajectory"); + ros::NodeHandle node; + + EightTrajectory eightTrajectory(node); + + ros::Rate loop(100); + + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + eightTrajectory.setCommand(ros::Time::now()-t0); + + ros::spinOnce(); + loop.sleep(); + } + return 0; +} diff --git a/src/pose2d_stamp.cpp b/src/pose2d_stamp.cpp new file mode 100644 index 0000000..ae0dcc5 --- /dev/null +++ b/src/pose2d_stamp.cpp @@ -0,0 +1,96 @@ +/****************************************************************************** + ROS pose2d_trajectories Package + Pose 2D Stamp Node + Copyright (C) 2015..2018 Walter Fetter Lages + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, but + WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see + . + +*******************************************************************************/ + +#include + +#include + +#include +#include + +class Pose2DStamp +{ + public: + Pose2DStamp(ros::NodeHandle node); + ~Pose2DStamp(void); + + private: + ros::NodeHandle node_; + + ros::Subscriber poseSubscriber; + ros::Publisher posePublisher; + + int seq; + std::string frame_id; + + void poseCB(const geometry_msgs::Pose2D::ConstPtr &pose); +}; + + +Pose2DStamp::Pose2DStamp(ros::NodeHandle node) +{ + node_=node; + + poseSubscriber=node_.subscribe("command",1000,&Pose2DStamp::poseCB,this); + posePublisher=node_.advertise("command_stamped",1000); + + seq=0; + + ros::param::get("~frame_id",frame_id); +} + +Pose2DStamp::~Pose2DStamp(void) +{ + poseSubscriber.shutdown(); + posePublisher.shutdown(); +} + +void Pose2DStamp::poseCB(const geometry_msgs::Pose2D::ConstPtr &pose) +{ + geometry_msgs::PoseStamped stamped; + stamped.header.stamp=ros::Time::now(); + stamped.header.frame_id=frame_id; + stamped.pose.position.x=pose->x; + stamped.pose.position.y=pose->y; + stamped.pose.position.z=0; + stamped.pose.orientation.x=0; + stamped.pose.orientation.y=0; + stamped.pose.orientation.z=sin(pose->theta/2.0); + stamped.pose.orientation.w=cos(pose->theta/2.0); + posePublisher.publish(stamped); +} + +int main(int argc,char* argv[]) +{ + ros::init(argc,argv,"pose2d_stamp"); + ros::NodeHandle node; + + Pose2DStamp pose2DStamp(node); + + ros::Rate loop(100); + ros::Time t0=ros::Time::now(); + while(ros::ok()) + { + ros::spinOnce(); + loop.sleep(); + } + return 0; +} -- 2.12.0