├── .gitlab-ci.yml ├── CMakeLists.txt ├── README.md ├── action └── Trajectory.action ├── cfg └── pose_param.cfg ├── config └── trajectory_generator_params.yaml ├── include └── cartesian_trajectory_generator │ ├── cartesian_trajectory_generator_base.h │ ├── cartesian_trajectory_generator_ros.h │ ├── overlay_functions.h │ └── velocity_functions.h ├── launch ├── publisher_demo.launch └── trajectory_generator.launch ├── msg └── OverlayMotionConf.msg ├── package.xml ├── res ├── interactive_marker.png └── tf.rviz ├── src ├── cartesian_trajectory_generator_ros.cpp └── cartesian_trajectory_generator_ros_node.cpp └── srv └── OverlayMotion.srv /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | image: osrf/ros:melodic-desktop-full 2 | 3 | stages: # List of stages for jobs, and their order of execution 4 | - build 5 | - depends 6 | 7 | variables: 8 | DEBIAN_FRONTEND: noninteractive 9 | TZ: Europe/Berlin 10 | ROS_DISTRO: melodic 11 | 12 | 13 | before_script: 14 | ## 15 | ## Install ssh-agent if not already installed, it is required by Docker. 16 | ## (change apt-get to yum if you use an RPM-based image) 17 | ## 18 | - 'command -v ssh-agent >/dev/null || ( apt-get update -y && apt-get install openssh-client -y )' 19 | 20 | ## 21 | ## Run ssh-agent (inside the build environment) 22 | ## 23 | - eval $(ssh-agent -s) 24 | 25 | ## 26 | ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store 27 | ## We're using tr to fix line endings which makes ed25519 keys work 28 | ## without extra base64 encoding. 29 | ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556 30 | ## 31 | - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - 32 | 33 | ## 34 | ## Create the SSH directory and give it the right permissions 35 | ## 36 | - mkdir -p ~/.ssh 37 | - chmod 700 ~/.ssh 38 | 39 | # For Docker builds disable host key checking. Be aware that by adding that 40 | # you are suspectible to man-in-the-middle attacks. 41 | # WARNING: Use this only with the Docker executor, if you use it with shell 42 | # you will overwrite your user's SSH config. 43 | - '[[ -f /.dockerenv ]] && echo -e "Host *\n\tStrictHostKeyChecking no\n\n" > ~/.ssh/config' 44 | 45 | 46 | build-catkin: # This job runs in the build stage, which runs first. 47 | stage: build 48 | script: 49 | - apt-get update 50 | - mkdir -p ~/catkin_ws/src; cd ~/catkin_ws 51 | - git clone --branch $CI_COMMIT_BRANCH git@git.cs.lth.se:robotlab/cartesian_trajectory_generator.git ~/catkin_ws/src/cartesian_trajectory_generator 52 | - apt-get install -y python-catkin-tools python-rosdep 53 | - rosdep update 54 | - source /opt/ros/$ROS_DISTRO/setup.bash 55 | - rosdep install --from-paths src --ignore-src --rosdistro=$ROS_DISTRO -y 56 | - catkin init 57 | - catkin config --extend /opt/ros/$ROS_DISTRO 58 | - catkin build 59 | - echo "Compile complete." 60 | 61 | 62 | trigger-heron: 63 | stage: depends 64 | trigger: 65 | project: robotlab/heron/heron_workspace_setup 66 | branch: master 67 | strategy: depend -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(cartesian_trajectory_generator) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++14) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED 11 | actionlib_msgs 12 | dynamic_reconfigure 13 | eigen_conversions 14 | geometry_msgs 15 | interactive_markers 16 | message_generation 17 | roscpp 18 | tf 19 | tf_conversions 20 | visualization_msgs 21 | ) 22 | 23 | find_package(Eigen3 REQUIRED) 24 | 25 | ## Generate actions in the 'action' folder 26 | add_action_files( 27 | FILES 28 | Trajectory.action 29 | ) 30 | 31 | add_message_files( 32 | FILES 33 | OverlayMotionConf.msg 34 | ) 35 | 36 | add_service_files( 37 | FILES 38 | OverlayMotion.srv 39 | ) 40 | 41 | generate_dynamic_reconfigure_options( 42 | cfg/pose_param.cfg 43 | ) 44 | 45 | generate_messages( 46 | DEPENDENCIES 47 | actionlib_msgs 48 | geometry_msgs 49 | std_msgs 50 | ) 51 | 52 | 53 | catkin_package( 54 | INCLUDE_DIRS include 55 | # LIBRARIES cartesian_trajectory_generator 56 | CATKIN_DEPENDS actionlib_msgs dynamic_reconfigure eigen_conversions geometry_msgs interactive_markers message_runtime roscpp tf tf_conversions visualization_msgs 57 | DEPENDS EIGEN3 58 | ) 59 | 60 | ########### 61 | ## Build ## 62 | ########### 63 | 64 | ## Specify additional locations of header files 65 | ## Your package locations should be listed before other locations 66 | include_directories( 67 | include 68 | ${catkin_INCLUDE_DIRS} 69 | ${Eigen_INCLUDE_DIRS} 70 | ) 71 | 72 | add_executable(${PROJECT_NAME} src/cartesian_trajectory_generator_ros_node.cpp src/cartesian_trajectory_generator_ros.cpp) 73 | 74 | add_dependencies( 75 | ${PROJECT_NAME} 76 | ${${PROJECT_NAME}_EXPORTED_TARGETS} 77 | ${catkin_EXPORTED_TARGETS} 78 | ${PROJECT_NAME}_gencfg 79 | ) 80 | 81 | ## Specify libraries to link a library or executable target against 82 | target_link_libraries(${PROJECT_NAME} 83 | ${catkin_LIBRARIES} 84 | ${Eigen_LIBRARIES} 85 | ${ros_logger_LIBRARIES} 86 | ) 87 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Cartesian Trajectory Generator 2 | 3 | A simple trajectory generator that creates and publishes a linear trajectory in Cartesian space. 4 | 5 | ## Features: 6 | - Publish a linear trajectory based on the current pose of a frame and a goal 7 | - Optional synchronization of rotation and translation 8 | - Implements a velocity profile with 9 | - User-defined separate values for constant acceleration and deceleration 10 | - Maximum rotational and translational velocity 11 | - Allows implementation of other velocity profiles 12 | - dynamic reconfigure configuration 13 | 14 | ## Limitations: 15 | - No collision checks 16 | - No feasibility checks 17 | - No handling of existing velocity of the end effector 18 | 19 | ## How to use: 20 | Open three terminals: 21 | 22 | 1. $ `roscore` 23 | 2. $ `mon launch cartesian_trajectory_generator publisher_demo.launch` 24 | 3. $ `mon launch cartesian_trajectory_generator trajectory_generator.launch` 25 | 26 | The demo will use three topics: 27 | 1. `/cartesian_trajectory_generator/current_goal`: Publishes the current goal 28 | 2. `/cartesian_trajectory_generator/new_goal`: Accepts new goals 29 | 3. `/cartesian_trajectory_generator/ref_pose`: Publishes the current reference pose 30 | 31 | ## Update the Goal 32 | ### With Dynamic Reconfigure 33 | 34 | $ `rosrun rqt_reconfigure rqt_reconfigure` 35 | 36 | In the rqt-gui open the menu for this pakcage, choose a new goal pose and click the "ready_to_send" button. 37 | 38 | ### With messages 39 | You can send new goal to `/cartesian_trajectory_generator/new_goal` like this: 40 | ``` 41 | $ rostopic pub /cartesian_trajectory_generator/new_goal geometry_msgs/PoseStamped "header: 42 | seq: 0 43 | stamp: 44 | secs: 0 45 | nsecs: 0 46 | frame_id: 'world' 47 | pose: 48 | position: 49 | x: 0.0 50 | y: 0.0 51 | z: 2.0 52 | orientation: 53 | x: 0.0 54 | y: 0.0 55 | z: 0.0 56 | w: 1.0" 57 | ``` 58 | 59 | ### With RViz 60 | This package publishes an interactive marker. You can move it to your desired goal pose. A right click opens a context menu that has an option "Send Pose". 61 | 62 | The marker position is updated when new goals arrive via messages or dynamic reconfigure. 63 | ![image info](./res/interactive_marker.png) 64 | 65 | ### With an action 66 | The ROS node will spawn an action server at `/cartesian_trajectory_generator/goal_action` that can be used to send an action. 67 | 68 | The action will succeed once the reference trajectory has finished and the end-effector is below both the specified tolerances. With the action it is also possible to specify custom ones. Otherwise, the values of the parameters `trans_goal_threshold_default` and `rot_goal_threshold_default` are used. 69 | 70 | ## Apply an Overlay Motion 71 | 72 | The trajectory generator announces a service at `/cartesian_trajectory_generator/overlay_motion`. 73 | 74 | An example call can look like this: 75 | 76 | ``` 77 | rosservice call /cartesian_trajectory_generator/overlay_motion "header: 78 | seq: 0 79 | stamp: {secs: 0, nsecs: 0} 80 | frame_id: 'world' 81 | motion: 1 82 | radius: 0.2 83 | path_distance: 0.01 84 | path_velocity: 0.1 85 | allow_decrease: false 86 | dir: {x: 0.0, y: 0.0, z: 1.0}" 87 | ``` 88 | 89 | The static values for the motion are defined in the [service message definition](srv/OverlayMotion.srv). 90 | 91 | ### Cancelling an Overlay Motion 92 | 93 | A call with `motion = 0`, that corresponds to `NONE` in the service message definition, or an invalid value will cancel the current motion.\ 94 | The reference pose will smoothly transition back to the last goal. -------------------------------------------------------------------------------- /action/Trajectory.action: -------------------------------------------------------------------------------- 1 | # Goal 2 | Header header 3 | # Start pose is optional. Empty pose uses current TF transformation 4 | geometry_msgs/PoseStamped start 5 | geometry_msgs/PoseStamped goal 6 | 7 | float32 trans_goal_tolerance 8 | float32 rot_goal_tolerance 9 | --- 10 | # result 11 | int32 error_code 12 | int32 SUCCESSFUL = 0 13 | int32 INVALID_GOAL = -1 # e.g. illegal quaternions in poses 14 | int32 TF_FAILED = -2 15 | 16 | string error_string 17 | --- 18 | # feedback 19 | # Between 0 and 1 20 | float32 path_percentage 21 | float32 time_percentage 22 | float32 trans_goal_error 23 | float32 rot_goal_error -------------------------------------------------------------------------------- /cfg/pose_param.cfg: -------------------------------------------------------------------------------- 1 | PACKAGE = "cartesian_trajectory_generator" 2 | 3 | from dynamic_reconfigure.parameter_generator_catkin import * 4 | from math import pi 5 | 6 | gen = ParameterGenerator() 7 | group_position=gen.add_group("Position",type="hidden") 8 | group_orientation=gen.add_group("Orientation",type="hidden") 9 | group_preferences=gen.add_group("Preferences",type="hidden") 10 | group_ready=gen.add_group("Send request",type="hidden") 11 | group_position.add("position_x", double_t, 0, "position X", -0.5, -1.8, 1.8) 12 | group_position.add("position_y", double_t, 0, "position Y", -0.5, -1.8, 1.8) 13 | group_position.add("position_z", double_t, 0, "position Z", 0.8, -1.8, 1.8) 14 | 15 | group_orientation.add("roll", double_t, 0, "Euler angle: roll", pi, 0, pi) 16 | group_orientation.add("pitch", double_t, 0, "Euler angle: pitch", 0, -pi, pi) 17 | group_orientation.add("yaw", double_t, 0, "Euler angle: yaw", 0, -pi, pi) 18 | 19 | 20 | 21 | group_ready.add("ready_to_send", bool_t, 0 , "True=Send new request", False) 22 | 23 | exit(gen.generate(PACKAGE, "cartesian_trajectory_generator", "pose_param")) 24 | 25 | -------------------------------------------------------------------------------- /config/trajectory_generator_params.yaml: -------------------------------------------------------------------------------- 1 | pose_topic: ref_pose 2 | new_goal_topic: new_goal 3 | current_goal_topic: current_goal 4 | frame_name: world 5 | ee_link: ee_link 6 | publish_rate: 100 7 | trans_v_max: 0.5 8 | rot_v_max: 0.5 9 | trans_a: 0.2 10 | rot_a: 0.5 11 | trans_goal_threshold_default: 0.04 12 | rot_goal_threshold_default: 0.25 13 | sync: true -------------------------------------------------------------------------------- /include/cartesian_trajectory_generator/cartesian_trajectory_generator_base.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | namespace cartesian_trajectory_generator 10 | { 11 | template 12 | class cartesian_trajectory_generator_base 13 | { 14 | public: 15 | cartesian_trajectory_generator_base() = default; 16 | ~cartesian_trajectory_generator_base() = default; 17 | 18 | Eigen::Vector3d get_position(double time) 19 | { 20 | return start_position_ + trans->get_distance(trans_t_ * time) * trans_vec_; 21 | } 22 | 23 | Eigen::Quaterniond get_orientation(double time) 24 | { 25 | if (d_rot_ == 0.) 26 | { 27 | return end_orientation_; 28 | } 29 | return start_orientation_.slerp((rot->get_distance(rot_t_ * time) / d_rot_), end_orientation_); 30 | } 31 | 32 | double get_trans_distance() 33 | { 34 | return trans->get_distance(); 35 | } 36 | 37 | double get_trans_distance(double time) 38 | { 39 | return trans->get_distance(trans_t_ * time); 40 | } 41 | 42 | double get_rot_distance() 43 | { 44 | return rot->get_distance(); 45 | } 46 | 47 | double get_rot_distance(double time) 48 | { 49 | return rot->get_distance(rot_t_ * time); 50 | } 51 | 52 | double get_trans_vel(double time) 53 | { 54 | return trans->get_velocity(trans_t_ * time); 55 | } 56 | 57 | double get_rot_vel(double time) 58 | { 59 | return rot->get_velocity(rot_t_ * time); 60 | } 61 | 62 | std::shared_ptr get_translation_obj() 63 | { 64 | return trans; 65 | } 66 | 67 | std::shared_ptr get_rotation_obj() 68 | { 69 | return rot; 70 | } 71 | 72 | double get_total_time() 73 | { 74 | return time_; 75 | } 76 | 77 | bool updateGoal(const Eigen::Vector3d& startPosition, const Eigen::Quaterniond& startOrientation, 78 | const Eigen::Vector3d& endPosition, const Eigen::Quaterniond& endOrientation) 79 | { 80 | trans_t_ = rot_t_ = 1.0; 81 | // Translation vector and distance 82 | start_position_ = startPosition; 83 | trans_vec_ = endPosition - startPosition; 84 | double d_trans = (endPosition - startPosition).norm(); 85 | trans_vec_.normalize(); 86 | // Rotation distance 87 | start_orientation_ = startOrientation.normalized(); 88 | end_orientation_ = endOrientation.normalized(); 89 | d_rot_ = start_orientation_.angularDistance(end_orientation_); 90 | // Parameterize velocity functions 91 | trans->set_distance(d_trans); 92 | rot->set_distance(d_rot_); 93 | // Synchronize them if needed 94 | if (synchronized_) 95 | { 96 | if (trans->get_time() > rot->get_time()) 97 | { 98 | rot_t_ = rot->get_time() / trans->get_time(); 99 | } 100 | else if (rot->get_time() > trans->get_time()) 101 | { 102 | trans_t_ = trans->get_time() / rot->get_time(); 103 | } 104 | } 105 | time_ = std::max(trans->get_time(), rot->get_time()); 106 | return true; 107 | } 108 | 109 | void set_synchronized(bool sync) 110 | { 111 | synchronized_ = sync; 112 | } 113 | 114 | private: 115 | std::shared_ptr trans{ new v_trans_t }; 116 | std::shared_ptr rot{ new v_rot_t }; 117 | 118 | bool synchronized_{ false }; 119 | double trans_t_{ 1.0 }; 120 | double rot_t_{ 1.0 }; 121 | Eigen::Vector3d start_position_; 122 | Eigen::Vector3d trans_vec_; 123 | Eigen::Quaterniond start_orientation_; 124 | Eigen::Quaterniond end_orientation_; 125 | double d_rot_{ 0 }; 126 | double time_{ 0 }; 127 | }; 128 | 129 | } // namespace cartesian_trajectory_generator -------------------------------------------------------------------------------- /include/cartesian_trajectory_generator/cartesian_trajectory_generator_ros.h: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace cartesian_trajectory_generator 23 | { 24 | class cartesianTrajectoryGeneratorRos 25 | { 26 | public: 27 | cartesianTrajectoryGeneratorRos(); 28 | 29 | void actionFeedback(); 30 | 31 | void actionGoalCallback(); 32 | 33 | void actionPreemptCallback(); 34 | 35 | void applyOverlay(Eigen::Vector3d& pos, double t_o); 36 | 37 | void markerConfigCallback(cartesian_trajectory_generator::pose_paramConfig &config, uint32_t level); 38 | 39 | bool getCurrentPose(Eigen::Vector3d &startPosition, Eigen::Quaterniond &startOrientation, bool print_exception = true); 40 | 41 | bool goalCallback(const geometry_msgs::PoseStampedConstPtr &msg, bool get_initial_pose = true); 42 | 43 | void goalMsgCallback(const geometry_msgs::PoseStampedConstPtr &msg) 44 | { 45 | this->trans_goal_threshold_ = this->trans_goal_threshold_default_; 46 | this->rot_goal_threshold_ = this->rot_goal_threshold_default_; 47 | goalCallback(msg); 48 | } 49 | 50 | bool overlayCallback(cartesian_trajectory_generator::OverlayMotionRequest &req, 51 | cartesian_trajectory_generator::OverlayMotionResponse &res); 52 | 53 | bool overlayFadeOut(); 54 | 55 | void applyOverlayFadeOut(Eigen::Vector3d &pos); 56 | 57 | void processMarkerFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); 58 | 59 | void publishRefMsg(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot); 60 | 61 | void publishRefTf(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot); 62 | 63 | void run(); 64 | 65 | bool updateGoal(bool get_start_pose = true); 66 | 67 | void updateMarkerPose(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot); 68 | 69 | void updateMarkerPose(const geometry_msgs::Pose &pose) 70 | { 71 | server_->setPose("Goal Pose", pose); 72 | } 73 | 74 | private: 75 | cartesian_trajectory_generator_base 77 | ctg_; 78 | 79 | // ROS Communication 80 | ros::NodeHandle n_; 81 | ros::Publisher pub_pose_; 82 | ros::Subscriber sub_goal_; 83 | ros::Publisher pub_goal_; 84 | ros::ServiceServer srv_overlay_; 85 | std::unique_ptr> as_; 86 | boost::shared_ptr server_; 87 | interactive_markers::MenuHandler menu_handler_; 88 | 89 | Eigen::Vector3d requested_position_; 90 | Eigen::Quaterniond requested_orientation_; 91 | geometry_msgs::PoseStamped requested_pose_; 92 | Eigen::Vector3d start_position_; 93 | Eigen::Quaterniond start_orientation_; 94 | Eigen::Vector3d current_position_; 95 | Eigen::Quaterniond current_orientation_; 96 | ros::Time traj_start_ = ros::Time::now(); 97 | std::shared_ptr overlay_f_; 98 | ros::Time overlay_start_ = ros::Time::now(); 99 | Eigen::Vector3d overlay_fade_{ Eigen::Vector3d::Zero() }; 100 | std::string overlay_frame_id_; 101 | bool first_goal_{ false }; 102 | bool publish_constantly_ { false }; 103 | 104 | geometry_msgs::PoseStamped pose_msg_; 105 | cartesian_trajectory_generator::TrajectoryFeedback action_feedback_; 106 | cartesian_trajectory_generator::TrajectoryResult action_result_; 107 | std::string frame_name_; 108 | std::string ee_link_; 109 | double trans_goal_threshold_{0}; 110 | double rot_goal_threshold_{0}; 111 | double trans_goal_threshold_default_{0}; 112 | double rot_goal_threshold_default_{0}; 113 | double trans_v_max_{0}; 114 | double trajectory_t_{ 0. }; 115 | 116 | ros::Rate rate_ = 1; 117 | 118 | // tf frame 119 | tf::TransformListener tf_listener_; 120 | tf::TransformBroadcaster tf_br_; 121 | tf::Transform tf_br_transform_; 122 | ros::Time tf_last_time_ = ros::Time::now(); 123 | tf::Vector3 tf_pos_; 124 | tf::Quaternion tf_rot_; 125 | 126 | // Dynamic reconfigure 127 | dynamic_reconfigure::Server config_pose_server; 128 | }; 129 | 130 | } // namespace cartesian_trajectory_generator 131 | -------------------------------------------------------------------------------- /include/cartesian_trajectory_generator/overlay_functions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace cartesian_trajectory_generator 8 | { 9 | //* Base class for overlay functions. 10 | /** 11 | * Derived classes need to implement 12 | * - get_velocity(time) 13 | * - get_distance(time) 14 | */ 15 | class overlay_base 16 | { 17 | public: 18 | overlay_base() = default; 19 | 20 | ~overlay_base() = default; 21 | 22 | /*! \brief Get the last time value. */ 23 | double get_last_time() 24 | { 25 | return time_; 26 | } 27 | 28 | /*! \brief */ 29 | virtual Eigen::Vector3d get_translation(double time) = 0; 30 | 31 | /*! \brief */ 32 | virtual Eigen::Quaterniond get_rotation(double time) = 0; 33 | 34 | Eigen::Vector3d get_translation_rotated(double time, const Eigen::Quaterniond& rot) 35 | { 36 | return rot * get_translation(time); 37 | } 38 | 39 | Eigen::Quaterniond get_rotation_rotated(double time, const Eigen::Quaterniond& rot) 40 | { 41 | return rot * get_rotation(time); 42 | } 43 | 44 | protected: 45 | double time_{ 0 }; /*!< Last time. */ 46 | }; 47 | 48 | class archimedes_spiral : public overlay_base 49 | { 50 | public: 51 | Eigen::Vector3d get_translation(double time) 52 | { 53 | if (time > time_) 54 | { 55 | update(time); 56 | } 57 | else if (time < time_) 58 | { 59 | throw std::invalid_argument("Time might not decrease."); 60 | } 61 | add_[0] = r_ * std::cos(theta_); 62 | add_[1] = r_ * std::sin(theta_); 63 | add_[2] = 0.0; 64 | // Rotate vector 65 | return rot_ * add_; 66 | } 67 | 68 | Eigen::Quaterniond get_rotation(double time) 69 | { 70 | return Eigen::Quaterniond::Identity(); 71 | } 72 | 73 | void set_direction(const Eigen::Vector3d& dir) 74 | { 75 | rot_ = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitZ(), dir.normalized()); 76 | } 77 | 78 | void set_max_radius(double r) 79 | { 80 | if (r >= 0) 81 | { 82 | max_r_ = r; 83 | } 84 | else 85 | { 86 | throw std::invalid_argument("Maximum radius must be non-negative."); 87 | } 88 | } 89 | 90 | void set_path_distance(double d) 91 | { 92 | if (d >= 0) 93 | { 94 | d_ = d; 95 | } 96 | else 97 | { 98 | throw std::invalid_argument("Distance must be non-negative."); 99 | } 100 | } 101 | 102 | void set_path_velocity(double v) 103 | { 104 | v_ = v; 105 | } 106 | 107 | void set_allow_decrease(bool decrease) 108 | { 109 | allow_decrease_ = decrease; 110 | } 111 | 112 | private: 113 | void update(double t) 114 | { 115 | double theta_d = v_ * (t - time_) / (r_ + 0.001); 116 | double r_d = theta_d * d_ / (2 * M_PI); 117 | if (v_ < 0) 118 | { 119 | r_d *= -1; 120 | } 121 | theta_ += theta_d; 122 | increase_r_ ? r_ += r_d : r_ -= r_d; 123 | 124 | if (allow_decrease_ && (r_ > max_r_)) 125 | { 126 | increase_r_ = false; 127 | } 128 | if (r_ > max_r_) 129 | { 130 | r_ = max_r_; 131 | } 132 | if (r_ < 0) 133 | { 134 | increase_r_ = true; 135 | r_ = 0.0; 136 | } 137 | time_ = t; 138 | } 139 | 140 | double r_{ 0 }; 141 | double theta_{ 0 }; 142 | double d_{ 0 }; 143 | double v_{ 0 }; 144 | double max_r_{ 0 }; 145 | bool allow_decrease_{ false }; 146 | bool increase_r_{ true }; 147 | Eigen::Quaterniond rot_{ Eigen::Quaterniond::Identity() }; 148 | Eigen::Vector3d add_{ Eigen::Vector3d::Zero() }; 149 | }; 150 | 151 | } // namespace cartesian_trajectory_generator -------------------------------------------------------------------------------- /include/cartesian_trajectory_generator/velocity_functions.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | namespace cartesian_trajectory_generator 7 | { 8 | //* Base class for velocity functions. 9 | /** 10 | * Assumes that there is some maximum velocity. 11 | * Derived classes need to implement 12 | * - parameterize() 13 | * - get_velocity(time) 14 | * - get_distance(time) 15 | * Parameterize is called every time a new distance is set. 16 | */ 17 | class velocity_base 18 | { 19 | public: 20 | velocity_base() = default; 21 | 22 | velocity_base(double v_max) 23 | { 24 | set_v_max(v_max); 25 | } 26 | 27 | ~velocity_base() = default; 28 | 29 | virtual void parameterize() = 0; 30 | 31 | virtual double get_velocity(double time) = 0; 32 | 33 | /*! \brief Get total time needed to cover the distance. */ 34 | double get_time() 35 | { 36 | return time_; 37 | } 38 | 39 | /*! \brief Get the requested distance. */ 40 | double get_distance() 41 | { 42 | return dist_; 43 | } 44 | 45 | /*! \brief Get the covered distance at a given time. */ 46 | virtual double get_distance(double time) = 0; 47 | 48 | /*! \brief Get the maximum velocity. */ 49 | double get_v_max() 50 | { 51 | return v_max_; 52 | } 53 | 54 | /*! \brief Set a new distance. */ 55 | void set_distance(double dist) 56 | { 57 | if (dist >= 0) 58 | { 59 | dist_ = dist; 60 | parameterize(); 61 | } 62 | else 63 | { 64 | throw std::invalid_argument("Distances must not be negative."); 65 | } 66 | } 67 | 68 | /*! \brief Set a maximum velocity. */ 69 | void set_v_max(double v_max) 70 | { 71 | if (v_max > 0) 72 | { 73 | v_max_ = v_max; 74 | } 75 | else 76 | { 77 | throw std::invalid_argument("Maximum velocity must be positive."); 78 | } 79 | } 80 | 81 | protected: 82 | double time_{ 0 }; /*!< Time needed for the defined distance. */ 83 | double dist_{ 0 }; /*!< Stores the specified distance. */ 84 | double v_max_{ 0 }; /*!< Stores the maximum veloicty. */ 85 | }; 86 | 87 | //* Velocity function with constant acceleration. 88 | /** 89 | * It is parameterized with positive values for acceleration and deceleration. 90 | */ 91 | class constant_acceleration : public velocity_base 92 | { 93 | public: 94 | constant_acceleration() = default; 95 | 96 | constant_acceleration(double acc, double dec, double v_max) 97 | { 98 | set_acceleration(acc); 99 | set_deceleration(dec); 100 | set_v_max(v_max); 101 | } 102 | 103 | ~constant_acceleration() = default; 104 | 105 | /*! \brief Get the total distance needed to accelerate. */ 106 | double get_acc_dist() 107 | { 108 | return get_acc_dist(get_acc_time()); 109 | } 110 | 111 | /*! \brief Get the distance needed to accelerate until a given time. */ 112 | double get_acc_dist(double time) 113 | { 114 | return 0.5 * acc_ * std::pow(time, 2.0); 115 | } 116 | 117 | /*! \brief Get the total distance needed to decelerate. */ 118 | double get_dec_dist() 119 | { 120 | return get_dec_dist(get_dec_time(), v_max_); 121 | } 122 | 123 | /*! \brief Get the distance needed to accelerate until a given time. */ 124 | double get_dec_dist(double time, double v_start) 125 | { 126 | return -0.5 * dec_ * std::pow(time, 2.0) + time * v_start; 127 | } 128 | 129 | /*! \brief Get the time needed to accelerate. */ 130 | double get_acc_time() 131 | { 132 | return v_max_ / acc_; 133 | } 134 | 135 | /*! \brief Get the time needed to accelerate. */ 136 | double get_dec_time() 137 | { 138 | return v_max_ / dec_; 139 | } 140 | 141 | /*! \brief Get the total time needed to accelerate and decelerate. */ 142 | double get_total_acc_times() 143 | { 144 | return get_acc_time() + get_dec_time(); 145 | } 146 | 147 | /*! \brief Set acceleration. Value must be positive.*/ 148 | void set_acceleration(double acc) 149 | { 150 | if (acc > 0) 151 | { 152 | acc_ = acc; 153 | } 154 | else 155 | { 156 | throw std::invalid_argument("Acceleration must be positive."); 157 | } 158 | } 159 | 160 | /*! \brief Set deceleration. Value must be positive.*/ 161 | void set_deceleration(double dec) 162 | { 163 | if (dec > 0) 164 | { 165 | dec_ = dec; 166 | } 167 | else 168 | { 169 | throw std::invalid_argument("Deceleration must be positive."); 170 | } 171 | } 172 | 173 | // Bring base class get_distance in the scope to participate in overloading. 174 | using velocity_base::get_distance; 175 | 176 | /*! \brief Get the covered distance up to a specified point in time.*/ 177 | double get_distance(double time) override 178 | { 179 | // The trajectory has ended 180 | if (time > time_) 181 | { 182 | return dist_; 183 | } 184 | // Deceleration phase 185 | else if (time > (t_acc_ + t_const_)) 186 | { 187 | return get_acc_dist(t_acc_) + v_max_ * t_const_ + get_dec_dist(time - t_acc_ - t_const_, get_velocity(t_acc_)); 188 | } 189 | 190 | else if (time > t_acc_) 191 | { 192 | return get_acc_dist(t_acc_) + v_max_ * (time - t_acc_); 193 | } 194 | // Acceleration phase 195 | else if (time >= 0) 196 | { 197 | return get_acc_dist(time); 198 | } 199 | // Negative time means that the trajectory has not started 200 | else 201 | { 202 | return 0; 203 | } 204 | } 205 | 206 | /*! \brief Get the velocity at a specified point in time.*/ 207 | double get_velocity(double time) override 208 | { 209 | // The trajectory has ended 210 | if (time > time_) 211 | { 212 | return 0; 213 | } 214 | // Deceleration phase 215 | else if (time > (t_acc_ + t_const_)) 216 | { 217 | double vel = -dec_ * (time - t_const_) + (acc_ + dec_) * t_acc_; 218 | // Never return negative values. 219 | return (vel >= 0) ? vel : 0; 220 | } 221 | // Constant velocity phase 222 | else if (time > t_acc_) 223 | { 224 | return v_max_; 225 | } 226 | // Acceleration phase 227 | else if (time >= 0) 228 | { 229 | return acc_ * time; 230 | } 231 | // Negative time means that the trajectory has not started 232 | else 233 | { 234 | return 0; 235 | } 236 | } 237 | 238 | private: 239 | /*! \brief Internal function to parameterize when a new distance is given.*/ 240 | void parameterize() override 241 | { 242 | if (acc_ <= 0) 243 | { 244 | throw std::runtime_error("Acceleration must be positive."); 245 | } 246 | if (dec_ <= 0) 247 | { 248 | throw std::runtime_error("Deceleration must be positive."); 249 | } 250 | if (v_max_ <= 0) 251 | { 252 | throw std::runtime_error("Maximum velocity must be positive."); 253 | } 254 | // Case 1: The trajectory has a part with constant velocity 255 | if (dist_ > (get_acc_dist() + get_dec_dist())) 256 | { 257 | t_acc_ = get_acc_time(); 258 | t_const_ = (dist_ - get_acc_dist() - get_dec_dist()) / v_max_; 259 | t_dec_ = get_dec_time(); 260 | } 261 | // Case 2: No constant velocity and the trajectory only accelerates and decelerates 262 | else 263 | { 264 | t_acc_ = std::sqrt(-2 * dist_ / ((acc_ + dec_) * (1 - (acc_ + dec_) / dec_))); 265 | t_const_ = 0; 266 | t_dec_ = t_acc_ * ((acc_ + dec_) / dec_ - 1); 267 | } 268 | time_ = t_acc_ + t_const_ + t_dec_; 269 | } 270 | 271 | double acc_{ 0 }; /*!< Acceleration. Must be positive. */ 272 | double dec_{ 0 }; /*!< Deceleration. Must be positive. */ 273 | double t_acc_{ 0 }; /*!< Time needed to accelerate for the defined distance. */ 274 | double t_const_{ 0 }; /*!< Time of constant velocity for the defined distance. */ 275 | double t_dec_{ 0 }; /*!< Time needed to decelerate for the defined distance. */ 276 | }; 277 | 278 | } // namespace cartesian_trajectory_generator -------------------------------------------------------------------------------- /launch/publisher_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/trajectory_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /msg/OverlayMotionConf.msg: -------------------------------------------------------------------------------- 1 | string motion 2 | float64 radius 3 | float64 path_distance 4 | float64 path_velocity 5 | bool allow_decrease 6 | geometry_msgs/Vector3 dir -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | cartesian_trajectory_generator 4 | 0.0.0 5 | The cartesian_trajectory_generator package 6 | 7 | 8 | 9 | 10 | Matthias Mayr 11 | 12 | 13 | 14 | 15 | 16 | BSD 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Oussama Chouman 29 | Matthias Mayr 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | actionlib_msgs 53 | dynamic_reconfigure 54 | eigen_conversions 55 | geometry_msgs 56 | interactive_markers 57 | roscpp 58 | tf 59 | tf_conversions 60 | visualization_msgs 61 | 62 | message_generation 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /res/interactive_marker.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matthias-mayr/cartesian_trajectory_generator/6b2feda18e1dc44c895e7bd2b903b068f989662b/res/interactive_marker.png -------------------------------------------------------------------------------- /res/tf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 499 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.5886790156364441 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Preferences: 31 | PromptSaveOnExit: true 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.029999999329447746 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Class: rviz/TF 56 | Enabled: true 57 | Frame Timeout: 15 58 | Frames: 59 | All Enabled: true 60 | ee_link: 61 | Value: true 62 | ee_link_ref_pose: 63 | Value: true 64 | world: 65 | Value: true 66 | Marker Scale: 1 67 | Name: TF 68 | Show Arrows: true 69 | Show Axes: true 70 | Show Names: true 71 | Tree: 72 | world: 73 | ee_link: 74 | {} 75 | ee_link_ref_pose: 76 | {} 77 | Update Interval: 0 78 | Value: true 79 | - Class: rviz/InteractiveMarkers 80 | Enable Transparency: true 81 | Enabled: true 82 | Name: InteractiveMarkers 83 | Show Axes: false 84 | Show Descriptions: true 85 | Show Visual Aids: false 86 | Update Topic: /cartesian_trajectory_generator/cartesian_trajectory_generator/update 87 | Value: true 88 | Enabled: true 89 | Global Options: 90 | Background Color: 48; 48; 48 91 | Default Light: true 92 | Fixed Frame: world 93 | Frame Rate: 30 94 | Name: root 95 | Tools: 96 | - Class: rviz/Interact 97 | Hide Inactive Objects: true 98 | - Class: rviz/MoveCamera 99 | - Class: rviz/Select 100 | - Class: rviz/FocusCamera 101 | - Class: rviz/Measure 102 | - Class: rviz/SetInitialPose 103 | Theta std deviation: 0.2617993950843811 104 | Topic: /initialpose 105 | X std deviation: 0.5 106 | Y std deviation: 0.5 107 | - Class: rviz/SetGoal 108 | Topic: /move_base_simple/goal 109 | - Class: rviz/PublishPoint 110 | Single click: true 111 | Topic: /clicked_point 112 | Value: true 113 | Views: 114 | Current: 115 | Class: rviz/Orbit 116 | Distance: 4.644041061401367 117 | Enable Stereo Rendering: 118 | Stereo Eye Separation: 0.05999999865889549 119 | Stereo Focal Distance: 1 120 | Swap Stereo Eyes: false 121 | Value: false 122 | Focal Point: 123 | X: 0.16072607040405273 124 | Y: -0.24295300245285034 125 | Z: 0.043862394988536835 126 | Focal Shape Fixed Size: true 127 | Focal Shape Size: 0.05000000074505806 128 | Invert Z Axis: false 129 | Name: Current View 130 | Near Clip Distance: 0.009999999776482582 131 | Pitch: 0.8603979349136353 132 | Target Frame: 133 | Value: Orbit (rviz) 134 | Yaw: 0.7603978514671326 135 | Saved: ~ 136 | Window Geometry: 137 | "&Displays": 138 | collapsed: false 139 | "&Time": 140 | collapsed: false 141 | "&Views": 142 | collapsed: false 143 | Height: 846 144 | Hide Left Dock: false 145 | Hide Right Dock: false 146 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000294fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007601000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004400000294000000f701000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000011000000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004400000294000000d001000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000004efc0100000002fb0000000800540069006d00650100000000000004b0000002ad01000003fb0000000800540069006d00650100000000000004500000000000000000000002340000029400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 147 | Selection: 148 | collapsed: false 149 | Tool Properties: 150 | collapsed: false 151 | Width: 1200 152 | X: 3344 153 | Y: 541 154 | -------------------------------------------------------------------------------- /src/cartesian_trajectory_generator_ros.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | namespace cartesian_trajectory_generator 5 | { 6 | visualization_msgs::Marker makeBox(visualization_msgs::InteractiveMarker &msg) 7 | { 8 | visualization_msgs::Marker marker; 9 | 10 | marker.type = visualization_msgs::Marker::SPHERE; 11 | marker.scale.x = msg.scale * 0.45; 12 | marker.scale.y = msg.scale * 0.45; 13 | marker.scale.z = msg.scale * 0.45; 14 | marker.color.r = 0.5; 15 | marker.color.g = 0.8; 16 | marker.color.b = 0.8; 17 | marker.color.a = 0.7; 18 | 19 | return marker; 20 | } 21 | 22 | visualization_msgs::InteractiveMarkerControl &makeBoxControl(visualization_msgs::InteractiveMarker &msg) 23 | { 24 | visualization_msgs::InteractiveMarkerControl control; 25 | control.always_visible = true; 26 | control.markers.push_back(makeBox(msg)); 27 | msg.controls.push_back(control); 28 | 29 | return msg.controls.back(); 30 | } 31 | 32 | void addMarkerControls(visualization_msgs::InteractiveMarker &int_marker) 33 | { 34 | visualization_msgs::InteractiveMarkerControl control; 35 | tf::Quaternion orien(1.0, 0.0, 0.0, 1.0); 36 | orien.normalize(); 37 | tf::quaternionTFToMsg(orien, control.orientation); 38 | control.name = "rotate_x"; 39 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 40 | int_marker.controls.push_back(control); 41 | control.name = "move_x"; 42 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 43 | int_marker.controls.push_back(control); 44 | 45 | orien = tf::Quaternion(0.0, 1.0, 0.0, 1.0); 46 | orien.normalize(); 47 | tf::quaternionTFToMsg(orien, control.orientation); 48 | control.name = "rotate_z"; 49 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 50 | int_marker.controls.push_back(control); 51 | control.name = "move_z"; 52 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 53 | int_marker.controls.push_back(control); 54 | 55 | orien = tf::Quaternion(0.0, 0.0, 1.0, 1.0); 56 | orien.normalize(); 57 | tf::quaternionTFToMsg(orien, control.orientation); 58 | control.name = "rotate_y"; 59 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS; 60 | int_marker.controls.push_back(control); 61 | control.name = "move_y"; 62 | control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS; 63 | int_marker.controls.push_back(control); 64 | } 65 | 66 | cartesianTrajectoryGeneratorRos::cartesianTrajectoryGeneratorRos() 67 | { 68 | double publish_rate{ 0 }; 69 | std::string pose_topic; 70 | std::string new_goal_topic; 71 | std::string current_goal_topic; 72 | double rot_v_max_{ 0 }; 73 | double trans_a_{ 0 }; 74 | double rot_a_{ 0 }; 75 | double trans_d_{ 0 }; 76 | double rot_d_{ 0 }; 77 | bool synced{ false }; 78 | if (!(n_.getParam("pose_topic", pose_topic) && n_.getParam("new_goal_topic", new_goal_topic) && 79 | n_.getParam("current_goal_topic", current_goal_topic) && n_.getParam("frame_name", frame_name_) && 80 | n_.getParam("ee_link", ee_link_) && n_.getParam("publish_rate", publish_rate) && 81 | n_.getParam("trans_v_max", trans_v_max_) && n_.getParam("rot_v_max", rot_v_max_) && 82 | n_.getParam("trans_a", trans_a_) && n_.getParam("rot_a", rot_a_))) 83 | { 84 | ROS_ERROR_STREAM("Failed to load required parameters. Are they load to the parameter server in namespace '" << n_.getNamespace() << "'?"); 85 | ros::shutdown(); 86 | } 87 | n_.param("trans_d", trans_d_, trans_a_); 88 | n_.param("rot_d", rot_d_, trans_a_); 89 | n_.param("sync", synced, false); 90 | n_.param("publish_constantly", publish_constantly_, false); 91 | n_.param("trans_goal_threshold_default", trans_goal_threshold_default_, 0.04); 92 | n_.param("rot_goal_threshold_default", rot_goal_threshold_default_, 0.25); 93 | if (n_.hasParam("trans_goal_threshold") || n_.hasParam("rot_goal_threshold")) 94 | { 95 | ROS_WARN("Deprecation: The parameters 'trans_goal_threshold' & 'rot_goal_threshold' were renamed to 'trans_goal_threshold_default' & 'rot_goal_threshold_default'. Please update your YAML file."); 96 | n_.param("trans_goal_threshold", trans_goal_threshold_default_); 97 | n_.param("rot_goal_threshold", rot_goal_threshold_default_); 98 | } 99 | 100 | rate_ = ros::Rate(publish_rate); 101 | pub_pose_ = n_.advertise(pose_topic, 1); 102 | pub_goal_ = n_.advertise(current_goal_topic, 1); 103 | sub_goal_ = n_.subscribe(new_goal_topic, 1, &cartesianTrajectoryGeneratorRos::goalMsgCallback, this); 104 | srv_overlay_ = n_.advertiseService("overlay_motion", &cartesianTrajectoryGeneratorRos::overlayCallback, this); 105 | 106 | pose_msg_.header.frame_id = frame_name_; 107 | requested_pose_.header.frame_id = frame_name_; 108 | // Set base parameters 109 | auto t = ctg_.get_translation_obj(); 110 | t->set_acceleration(trans_a_); 111 | t->set_deceleration(trans_d_); 112 | t->set_v_max(trans_v_max_); 113 | auto r = ctg_.get_rotation_obj(); 114 | r->set_acceleration(rot_a_); 115 | r->set_deceleration(rot_d_); 116 | r->set_v_max(rot_v_max_); 117 | ctg_.set_synchronized(synced); 118 | 119 | config_pose_server.setCallback(boost::bind(&cartesianTrajectoryGeneratorRos::markerConfigCallback, this, _1, _2)); 120 | traj_start_ = ros::Time::now(); 121 | 122 | visualization_msgs::InteractiveMarker int_marker; 123 | server_.reset(new interactive_markers::InteractiveMarkerServer("cartesian_trajectory_generator", "", false)); 124 | int_marker.header.frame_id = frame_name_; 125 | int_marker.scale = 0.5; 126 | int_marker.name = "Goal Pose"; 127 | int_marker.description = "6-DOF Goal Pose"; 128 | makeBoxControl(int_marker); 129 | int_marker.controls[0].interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_ROTATE_3D; 130 | addMarkerControls(int_marker); 131 | server_->insert(int_marker); 132 | server_->setCallback(int_marker.name, boost::bind(&cartesianTrajectoryGeneratorRos::processMarkerFeedback, this, _1)); 133 | menu_handler_.insert("Send Pose", boost::bind(&cartesianTrajectoryGeneratorRos::processMarkerFeedback, this, _1)); 134 | menu_handler_.insert("Reset Marker Pose", 135 | boost::bind(&cartesianTrajectoryGeneratorRos::processMarkerFeedback, this, _1)); 136 | menu_handler_.apply(*server_, int_marker.name); 137 | 138 | // Action Server Setup 139 | as_ = std::unique_ptr>( 140 | new actionlib::SimpleActionServer( 141 | n_, std::string("goal_action"), false)); 142 | as_->registerGoalCallback(boost::bind(&cartesianTrajectoryGeneratorRos::actionGoalCallback, this)); 143 | as_->registerPreemptCallback(boost::bind(&cartesianTrajectoryGeneratorRos::actionPreemptCallback, this)); 144 | as_->start(); 145 | } 146 | 147 | void cartesianTrajectoryGeneratorRos::actionFeedback() 148 | { 149 | if (as_->isActive()) 150 | { 151 | // Time percentage 152 | if (ctg_.get_total_time() != 0.0) 153 | { 154 | action_feedback_.time_percentage = std::min(1.0, trajectory_t_ / ctg_.get_total_time()); 155 | } 156 | else 157 | { 158 | action_feedback_.time_percentage = 0.0; 159 | } 160 | // Path percentage 161 | double d_trans, d_rot = 1.0; 162 | if (ctg_.get_trans_distance() != 0.0) 163 | { 164 | d_trans = ctg_.get_trans_distance(trajectory_t_) / ctg_.get_trans_distance(); 165 | } 166 | if (ctg_.get_rot_distance() != 0.0) 167 | { 168 | d_rot = ctg_.get_rot_distance(trajectory_t_) / ctg_.get_rot_distance(); 169 | } 170 | action_feedback_.path_percentage = std::min(d_trans, d_rot); 171 | // Errors 172 | bool error_below_thresholds{false}; 173 | if (getCurrentPose(this->current_position_, this->current_orientation_, true)) 174 | { 175 | action_feedback_.trans_goal_error = (this->current_position_ - this->requested_position_).norm(); 176 | action_feedback_.rot_goal_error = this->current_orientation_.angularDistance(this->requested_orientation_); 177 | if (action_feedback_.trans_goal_error < this->trans_goal_threshold_ && action_feedback_.rot_goal_error < this->rot_goal_threshold_) 178 | { 179 | error_below_thresholds = true; 180 | } 181 | } 182 | 183 | as_->publishFeedback(action_feedback_); 184 | if (action_feedback_.time_percentage == 1.0 && error_below_thresholds) 185 | { 186 | ROS_INFO("Set goal succeeded."); 187 | action_result_.error_code = action_result_.SUCCESSFUL; 188 | as_->setSucceeded(action_result_); 189 | } 190 | } 191 | } 192 | 193 | void cartesianTrajectoryGeneratorRos::actionGoalCallback() 194 | { 195 | boost::shared_ptr action; 196 | action = as_->acceptNewGoal(); 197 | // Get thresholds from action 198 | this->trans_goal_threshold_ = action->trans_goal_tolerance; 199 | if (this->trans_goal_threshold_ == 0.0) 200 | { 201 | this->trans_goal_threshold_ = this->trans_goal_threshold_default_; 202 | } 203 | this->rot_goal_threshold_ = action->rot_goal_tolerance; 204 | if (this->rot_goal_threshold_ == 0.0) 205 | { 206 | this->rot_goal_threshold_ = this->rot_goal_threshold_default_; 207 | } 208 | // Get start pose from action if it is supplied 209 | bool get_initial_pose = true; 210 | if (action->start.pose != geometry_msgs::Pose()) 211 | { 212 | get_initial_pose = false; 213 | geometry_msgs::PoseStamped start_pose = action->start; 214 | try 215 | { 216 | tf_listener_.transformPose(this->frame_name_, action->start, start_pose); 217 | } 218 | catch(tf::TransformException ex) 219 | { 220 | ROS_ERROR("%s", ex.what()); 221 | } 222 | 223 | tf::pointMsgToEigen(start_pose.pose.position, this->start_position_); 224 | tf::quaternionMsgToEigen(start_pose.pose.orientation, this->start_orientation_); 225 | } 226 | if (!goalCallback(boost::make_shared(action->goal), get_initial_pose)) 227 | { 228 | action_result_.error_code = action_result_.TF_FAILED; 229 | as_->setAborted(action_result_); 230 | return; 231 | } 232 | ROS_INFO("Accepted new goal."); 233 | } 234 | 235 | void cartesianTrajectoryGeneratorRos::actionPreemptCallback() 236 | { 237 | ROS_INFO("Actionserver got preempted."); 238 | // Abort by setting a new goal to the current position 239 | if (getCurrentPose(this->requested_position_, this->requested_orientation_, true)) { 240 | updateGoal(true); 241 | } else { 242 | ROS_WARN("Could not update goal pose. Emergency abort by setting early start time."); 243 | this->traj_start_ = ros::Time::now() - ros::Duration(60 * 60); 244 | } 245 | as_->setPreempted(); 246 | } 247 | 248 | void cartesianTrajectoryGeneratorRos::markerConfigCallback(cartesian_trajectory_generator::pose_paramConfig &config, 249 | uint32_t level) 250 | { 251 | if (config.ready_to_send) 252 | { 253 | config.ready_to_send = false; 254 | requested_position_[0] = config.position_x; 255 | requested_position_[1] = config.position_y; 256 | requested_position_[2] = config.position_z; 257 | requested_orientation_ = Eigen::AngleAxisd(config.roll, Eigen::Vector3d::UnitX()) * 258 | Eigen::AngleAxisd(config.pitch, Eigen::Vector3d::UnitY()) * 259 | Eigen::AngleAxisd(config.yaw, Eigen::Vector3d::UnitZ()); 260 | 261 | updateGoal(); 262 | ROS_INFO("Request from dynamic reconfig-server recieved"); 263 | } 264 | } 265 | 266 | void cartesianTrajectoryGeneratorRos::applyOverlay(Eigen::Vector3d &pos, double t_o) 267 | { 268 | if (!overlay_f_) 269 | { 270 | return; 271 | } 272 | 273 | Eigen::Quaterniond rot = Eigen::Quaterniond::Identity(); 274 | if (overlay_frame_id_ != frame_name_) 275 | { 276 | try 277 | { 278 | tf::StampedTransform transform; 279 | tf_listener_.lookupTransform(frame_name_, overlay_frame_id_, ros::Time(0), transform); 280 | tf::quaternionTFToEigen(transform.getRotation(), rot); 281 | } 282 | catch (tf::TransformException ex) 283 | { 284 | ROS_ERROR_THROTTLE(1, "%s", ex.what()); 285 | return; 286 | } 287 | } 288 | pos += overlay_f_->get_translation_rotated(t_o, rot); 289 | } 290 | 291 | bool cartesianTrajectoryGeneratorRos::getCurrentPose(Eigen::Vector3d &startPosition, 292 | Eigen::Quaterniond &startOrientation, bool print_exception) 293 | { 294 | tf::StampedTransform transform; 295 | try 296 | { 297 | tf_listener_.waitForTransform(frame_name_, ee_link_, ros::Time(0), ros::Duration(3.0)); 298 | tf_listener_.lookupTransform(frame_name_, ee_link_, ros::Time(0), transform); 299 | } 300 | catch (tf::TransformException &ex) 301 | { 302 | if (print_exception) 303 | { 304 | ROS_ERROR("%s", ex.what()); 305 | } 306 | return false; 307 | } 308 | tf::vectorTFToEigen(transform.getOrigin(), startPosition); 309 | tf::quaternionTFToEigen(transform.getRotation(), startOrientation); 310 | return true; 311 | } 312 | 313 | bool cartesianTrajectoryGeneratorRos::goalCallback(const geometry_msgs::PoseStampedConstPtr &msg, bool get_initial_pose) 314 | { 315 | if (msg->header.frame_id == frame_name_) 316 | { 317 | tf::pointMsgToEigen(msg->pose.position, requested_position_); 318 | tf::quaternionMsgToEigen(msg->pose.orientation, requested_orientation_); 319 | } 320 | else 321 | { 322 | try 323 | { 324 | geometry_msgs::PoseStamped t_msg; 325 | tf_listener_.transformPose(frame_name_, ros::Time(0), *msg, msg->header.frame_id, t_msg); 326 | tf::pointMsgToEigen(t_msg.pose.position, requested_position_); 327 | tf::quaternionMsgToEigen(t_msg.pose.orientation, requested_orientation_); 328 | } 329 | catch (tf::TransformException ex) 330 | { 331 | ROS_ERROR("%s Did not update goal.", ex.what()); 332 | return false; 333 | } 334 | } 335 | updateGoal(get_initial_pose); 336 | return true; 337 | } 338 | 339 | bool cartesianTrajectoryGeneratorRos::overlayCallback(cartesian_trajectory_generator::OverlayMotionRequest &req, 340 | cartesian_trajectory_generator::OverlayMotionResponse &res) 341 | { 342 | if (!first_goal_) 343 | { 344 | if (getCurrentPose(requested_position_, requested_orientation_)) { 345 | updateGoal(); 346 | } else { 347 | ROS_WARN("Could not update goal pose. Will not apply overlay."); 348 | return false; 349 | } 350 | } 351 | if (overlay_f_) 352 | { 353 | overlay_fade_ = overlay_f_->get_translation((ros::Time::now() - overlay_start_).toSec()); 354 | } 355 | if (req.motion == cartesian_trajectory_generator::OverlayMotionRequest::ARCHIMEDES) 356 | { 357 | auto o = std::make_shared(); 358 | overlay_f_ = o; 359 | o->set_allow_decrease(req.allow_decrease); 360 | Eigen::Vector3d vec; 361 | tf::vectorMsgToEigen(req.dir, vec); 362 | o->set_direction(vec); 363 | o->set_max_radius(req.radius); 364 | o->set_path_velocity(req.path_velocity); 365 | o->set_path_distance(req.path_distance); 366 | overlay_start_ = ros::Time::now(); 367 | if (!req.header.frame_id.empty()) 368 | { 369 | overlay_frame_id_ = req.header.frame_id; 370 | } 371 | else 372 | { 373 | overlay_frame_id_ = ee_link_; 374 | } 375 | } 376 | else 377 | { 378 | overlay_f_.reset(); 379 | } 380 | return true; 381 | } 382 | 383 | bool cartesianTrajectoryGeneratorRos::overlayFadeOut() 384 | { 385 | double norm = overlay_fade_.norm(); 386 | return norm > 0.0; 387 | } 388 | 389 | void cartesianTrajectoryGeneratorRos::applyOverlayFadeOut(Eigen::Vector3d &pos) 390 | { 391 | if (overlayFadeOut()) 392 | { 393 | double norm = overlay_fade_.norm(); 394 | double diff = rate_.expectedCycleTime().toSec() * 0.25 * trans_v_max_; 395 | if (diff > norm) 396 | { 397 | overlay_fade_ = Eigen::Vector3d::Zero(); 398 | } 399 | else 400 | { 401 | overlay_fade_ = overlay_fade_ * (norm - diff) / norm; 402 | } 403 | pos += overlay_fade_; 404 | } 405 | } 406 | 407 | void cartesianTrajectoryGeneratorRos::processMarkerFeedback( 408 | const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) 409 | { 410 | if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT && 411 | feedback->menu_entry_id == 1) 412 | { 413 | ROS_INFO_STREAM("New marker pose received"); 414 | geometry_msgs::PoseStamped msg; 415 | msg.header.frame_id = frame_name_; 416 | msg.pose = feedback->pose; 417 | geometry_msgs::PoseStampedConstPtr msg_ptr(new geometry_msgs::PoseStamped(msg)); 418 | goalCallback(msg_ptr); 419 | } 420 | else if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT && 421 | feedback->menu_entry_id == 2) 422 | { 423 | if (getCurrentPose(this->current_position_, this->current_orientation_)) 424 | { 425 | updateMarkerPose(this->current_position_, this->current_orientation_); 426 | } 427 | } 428 | 429 | server_->applyChanges(); 430 | } 431 | 432 | void cartesianTrajectoryGeneratorRos::publishRefTf(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot) 433 | { 434 | if (ros::Time::now() > this->tf_last_time_) 435 | { 436 | tf::vectorEigenToTF(pos, tf_pos_); 437 | tf_br_transform_.setOrigin(tf_pos_); 438 | tf::quaternionEigenToTF(rot, tf_rot_); 439 | tf_br_transform_.setRotation(tf_rot_); 440 | tf_br_.sendTransform(tf::StampedTransform(tf_br_transform_, ros::Time::now(), frame_name_, ee_link_ + "_ref_pose")); 441 | this->tf_last_time_ = ros::Time::now(); 442 | } 443 | } 444 | 445 | void cartesianTrajectoryGeneratorRos::publishRefMsg(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot) 446 | { 447 | pose_msg_.header.stamp = ros::Time::now(); 448 | tf::pointEigenToMsg(pos, pose_msg_.pose.position); 449 | tf::quaternionEigenToMsg(rot, pose_msg_.pose.orientation); 450 | pub_pose_.publish(pose_msg_); 451 | } 452 | 453 | void cartesianTrajectoryGeneratorRos::run() 454 | { 455 | while (!getCurrentPose(requested_position_, requested_orientation_, false)) 456 | { 457 | ROS_INFO_STREAM("Waiting for inital transform from " << frame_name_ << " to " << ee_link_); 458 | ros::Duration(1.0).sleep(); 459 | } 460 | updateMarkerPose(requested_position_, requested_orientation_); 461 | ctg_.updateGoal(requested_position_, requested_orientation_, requested_position_, requested_orientation_); 462 | traj_start_ = ros::Time::now(); 463 | 464 | ROS_INFO_STREAM("Setup complete."); 465 | double t_o{ 0. }; 466 | Eigen::Vector3d pos{ requested_position_ }; 467 | Eigen::Quaterniond rot{ requested_orientation_ }; 468 | 469 | while (n_.ok()) 470 | { 471 | trajectory_t_ = (ros::Time::now() - traj_start_).toSec(); 472 | if (publish_constantly_ || trajectory_t_ < ctg_.get_total_time() || overlay_f_ || overlayFadeOut()) 473 | { 474 | t_o = (ros::Time::now() - overlay_start_).toSec(); 475 | pos = ctg_.get_position(trajectory_t_); 476 | rot = ctg_.get_orientation(trajectory_t_); 477 | 478 | applyOverlay(pos, t_o); 479 | applyOverlayFadeOut(pos); 480 | if (first_goal_) 481 | { 482 | publishRefMsg(pos, rot); 483 | } 484 | } 485 | actionFeedback(); 486 | publishRefTf(pos, rot); 487 | server_->applyChanges(); 488 | ros::spinOnce(); 489 | rate_.sleep(); 490 | } 491 | } 492 | 493 | bool cartesianTrajectoryGeneratorRos::updateGoal(bool get_start_pose) 494 | { 495 | if (get_start_pose) 496 | { 497 | if (!getCurrentPose(start_position_, start_orientation_)) 498 | { 499 | ROS_ERROR("Could not look up transform. Not setting new goal."); 500 | return false; 501 | } 502 | } 503 | first_goal_ = true; 504 | requested_orientation_.normalize(); 505 | // publishing latest request once 506 | requested_pose_.header.stamp = ros::Time::now(); 507 | 508 | tf::pointEigenToMsg(requested_position_, requested_pose_.pose.position); 509 | tf::quaternionEigenToMsg(requested_orientation_, requested_pose_.pose.orientation); 510 | 511 | 512 | pub_goal_.publish(requested_pose_); 513 | updateMarkerPose(requested_pose_.pose); 514 | 515 | ROS_INFO("Starting position:(x: %2lf,y: %2lf,z: %2lf) \t Orientation:(x: %3lf,y: %3lf,z: %3lf, w: %3lf)", start_position_[0], 516 | start_position_[1], start_position_[2], start_orientation_.coeffs()[0], start_orientation_.coeffs()[1], 517 | start_orientation_.coeffs()[2], start_orientation_.coeffs()[3]); 518 | ctg_.updateGoal(start_position_, start_orientation_, requested_position_, requested_orientation_); 519 | traj_start_ = ros::Time::now(); 520 | return true; 521 | } 522 | 523 | void cartesianTrajectoryGeneratorRos::updateMarkerPose(const Eigen::Vector3d &pos, const Eigen::Quaterniond &rot) 524 | { 525 | geometry_msgs::Pose pose; 526 | tf::pointEigenToMsg(pos, pose.position); 527 | tf::quaternionEigenToMsg(rot, pose.orientation); 528 | updateMarkerPose(pose); 529 | } 530 | 531 | } // namespace cartesian_trajectory_generator 532 | -------------------------------------------------------------------------------- /src/cartesian_trajectory_generator_ros_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "cartesian_trajectory_generator"); 6 | 7 | cartesian_trajectory_generator::cartesianTrajectoryGeneratorRos gen; 8 | gen.run(); 9 | return 0; 10 | } 11 | -------------------------------------------------------------------------------- /srv/OverlayMotion.srv: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | int8 motion 4 | int8 NONE = 0 5 | int8 ARCHIMEDES = 1 6 | 7 | float64 radius 8 | float64 path_distance 9 | float64 path_velocity 10 | bool allow_decrease 11 | geometry_msgs/Vector3 dir 12 | --- 13 | --------------------------------------------------------------------------------