--- /dev/null
+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
--- /dev/null
+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)
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Circle Path Class
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef CIRCLE_PATH_H
+#define CIRCLE_PATH_H
+
+#include <Eigen/Dense>
+
+/** Circle path
+* @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+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
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Eigth Path Class
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#ifndef EIGHT_PATH_H
+#define EIGHT_PATH_H
+
+#include <pose2d_trajectories/circle_path.h>
+
+/** 8 path
+* @author Walter Fetter Lages <w.fetter@ieee.org>
+*/
+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
--- /dev/null
+<?xml version="1.0"?>
+<package>
+ <name>pose2d_trajectories</name>
+ <version>3.0.0</version>
+ <description>The pose2d_trajectories package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="fetter@ece.ufrgs.br">Walter Fetter Lages</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>GPLv3</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/twil_ident</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author email="fetter@ece.ufrgs.br">Walter Fetter Lages</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>eigen</build_depend>
+ <build_depend>geometry_msgs</build_depend>
+
+ <run_depend>eigen</run_depend>
+ <run_depend>geometry_msgs</run_depend>
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- You can specify that this package is a metapackage here: -->
+ <!-- <metapackage/> -->
+
+ <!-- Other tools can request additional information be placed here -->
+
+ </export>
+</package>
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Circle Path Class
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <pose2d_trajectories/circle_path.h>
+
+#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;
+}
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Eigth Path Class
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <pose2d_trajectories/eight_path.h>
+
+#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);
+}
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Eigth Trajectory Node
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <ros/ros.h>
+#include <geometry_msgs/Pose2D.h>
+
+#include <pose2d_trajectories/eight_path.h>
+
+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<geometry_msgs::Pose2D>("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;
+}
--- /dev/null
+/******************************************************************************
+ ROS pose2d_trajectories Package
+ Pose 2D Stamp Node
+ Copyright (C) 2015..2018 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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
+ <http://www.gnu.org/licenses/>.
+
+*******************************************************************************/
+
+#include <ros/ros.h>
+
+#include <Eigen/Dense>
+
+#include <geometry_msgs/Pose2D.h>
+#include <geometry_msgs/PoseStamped.h>
+
+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<geometry_msgs::PoseStamped>("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;
+}
projects / pose2d_trajectories.git / commitdiff