sample apps

d74e778e · Sebastian Ebert · dea2694b · d74e778e · d74e778e · d74e778e
Commit d74e778e authored 5 years ago by Sebastian Ebert
--- a/.gitignore
+++ b/.gitignore
+.idea/
+cmake-build-debug/
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
+cmake_minimum_required(VERSION 2.8.3)
+project(sample_applications)
+
+add_compile_options(-std=c++14)
+set(GCC_COVERAGE_COMPILE_FLAGS "-fpermissive")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GCC_COVERAGE_COMPILE_FLAGS} ${GAZEBO_CXX_FLAGS}")
+
+# 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 controller_manager
+        effort_controllers
+        gazebo_ros
+        genmsg
+        joint_state_controller
+        joint_state_publisher
+        joint_trajectory_controller
+        robot_state_publisher
+        roscpp
+        std_msgs
+        xacro
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface)
+
+# System dependencies are found with CMake's conventions
+find_package(PkgConfig REQUIRED)
+
+# ################################################################################################################################
+# catkin specific configuration ##
+# ################################################################################################################################
+# The catkin_package macro generates cmake config files for your package Declare things to be passed to dependent projects
+catkin_package(CATKIN_DEPENDS
+        moveit_core
+        moveit_visual_tools
+        moveit_ros_planning_interface
+        controller_interface
+        hardware_interface
+        pluginlib
+        DEPENDS
+        # system_lib
+        )
+
+# ################################################################################################################################
+# Build ##
+# ################################################################################################################################
+
+# Specify additional locations of header files Your package locations should be listed before other locations
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_executable(SampleConstraintPlanner src/SampleConstraintPlanner.cpp)
+add_executable(SampleTimedCartesianPlanner src/SampleTimedCartesianPlanner.cpp)
+add_executable(SampleSimpleMotion src/SampleSimpleMotion.cpp)
+
+# Specify libraries to link a library or executable target against
+target_link_libraries(SampleConstraintPlanner ${catkin_LIBRARIES})
+target_link_libraries(SampleTimedCartesianPlanner ${catkin_LIBRARIES})
+target_link_libraries(SampleSimpleMotion ${catkin_LIBRARIES})
\ No newline at end of file
--- a/README.md
+++ b/README.md
-# Sample applications for ROS combined with (simulated) Franka Emika Panda robots
\ No newline at end of file
+### Sample applications for ROS combined with a (simulated) Franka Emika Panda robots
+
+Contains different examples for the planning and execution of robotic motions with MoveIt in ROS.
+
+##### The different examples:
+
+- Planning and simulation based on rviz: `roslaunch panda_simulation simulation.launch`
+- Execution of a simple motion: `roslaunch sample_applications sample_simple_simulation.launch`
+- Execution of a simple motion costraint by a blocking object: `roslaunch sample_applications sample_constraint_simulation.launch`
+- Execution of a velocity constraint cartesian trajectory: `roslaunch sample_applications simulation.launch`
\ No newline at end of file
--- a/launch/sample_constraint_simulation.launch
+++ b/launch/sample_constraint_simulation.launch
+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+    <!-- launch robot control node for moveit motion planning -->
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node pkg="sample_applications" type="SampleConstraintPlanner" name="SampleConstraintPlanner" />
+
+</launch>
--- a/launch/sample_simple_simulation.launch
+++ b/launch/sample_simple_simulation.launch
+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+    <!-- launch robot control node for moveit motion planning -->
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node pkg="sample_applications" type="SampleSimpleMotion" name="SampleSimpleMotion" />
+
+</launch>
--- a/launch/sample_timed_cartesian_simulation.launch
+++ b/launch/sample_timed_cartesian_simulation.launch
+<launch>
+    <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+    <!-- GAZEBO arguments -->
+    <arg name="paused" default="false" />
+    <arg name="use_sim_time" default="true" />
+    <arg name="gui" default="true" />
+    <arg name="headless" default="false" />
+    <arg name="debug" default="false" />
+    <arg name="load_gripper" default="true" />
+
+    <!--launch GAZEBO with own world configuration -->
+    <include file="$(find gazebo_ros)/launch/empty_world.launch">
+        <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> -->
+        <arg name="debug" value="$(arg debug)" />
+        <arg name="gui" value="$(arg gui)" />
+        <arg name="paused" value="$(arg paused)" />
+        <arg name="use_sim_time" value="$(arg use_sim_time)" />
+        <arg name="headless" value="$(arg headless)" />
+    </include>
+
+    <node name="rqt_console" pkg="rqt_console" type="rqt_console" />
+    <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> -->
+
+    <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" />
+
+    <!-- Load joint controller configurations from YAML file to parameter server -->
+    <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" />
+
+
+    <!-- load the controllers -->
+    <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" />
+    <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" />
+
+
+    <!-- convert joint states to TF transforms for rviz, etc -->
+    <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
+
+    <include file="$(find panda_moveit_config)/launch/planning_context.launch">
+        <arg name="load_robot_description" value="true" />
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <include file="$(find panda_moveit_config)/launch/move_group.launch">
+        <arg name="load_gripper" value="$(arg load_gripper)" />
+    </include>
+    <group if="$(arg gui)">
+        <include file="$(find panda_moveit_config)/launch/moveit_rviz.launch" />
+    </group>
+
+    <node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
+
+    <!-- launch robot control node for moveit motion planning -->
+    <node pkg="panda_simulation" type="robot_control_node" name="robot_control_node" output="screen" />
+
+    <!-- load (not start!) custom joint position controller -->
+    <node pkg="controller_manager" type="spawner" name="joint_position_launcher" args="--stopped joint_position_controller" />
+
+    <!-- run custom node for automatic intialization -->
+    <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" />
+
+    <node pkg="sample_applications" type="SampleTimedCartesianPlanner" name="SampleTimedCartesianPlanner" />
+
+</launch>
--- a/package.xml
+++ b/package.xml
+<?xml version="1.0"?>
+<package format="2">
+  <name>sample_applications</name>
+  <version>0.0.0</version>
+  <description>The sample_applications package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="smt@todo.todo">Chair of Software Technology</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>TODO</license>
+
+
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <depend>controller_manager</depend>
+  <depend>effort_controllers</depend>
+  <depend>gazebo_ros</depend>
+  <depend>gazebo_ros_control</depend>
+  <depend>joint_state_controller</depend>
+  <depend>joint_state_publisher</depend>
+  <depend>joint_trajectory_controller</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>roscpp</depend>
+  <depend>std_msgs</depend>
+  <depend>tf</depend>
+  <depend>xacro</depend>
+  <depend>libjsoncpp-dev</depend>
+  <depend>moveit_simple_controller_manager</depend>
+
+  <!-- CUSTOM -->
+  <depend>moveit_core</depend>
+  <depend>moveit_ros_planning_interface</depend>
+  <depend>moveit_ros_perception</depend>
+  <depend>moveit_visual_tools</depend>
+  <depend>controller_interface</depend>
+  <depend>hardware_interface</depend>
+
+  <exec_depend>pluginlib</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
--- a/src/SampleConstraintPlanner.cpp
+++ b/src/SampleConstraintPlanner.cpp
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * simple demo of constraint aware planning
+ */
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "CONSTRAINT PLANNER");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    // wait for robot init of robot_state_initializer
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
+    ros::Duration(5.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+    static const std::string PLANNING_GROUP = "panda_arm";
+    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    const robot_state::JointModelGroup* joint_model_group =
+            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
+
+    // Visualization Setup
+    namespace rvt = rviz_visual_tools;
+    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+    visual_tools.deleteAllMarkers();
+    visual_tools.loadRemoteControl();
+
+    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+    text_pose.translation().z() = 1.75;
+    visual_tools.publishText(text_pose, "Sample constraint planner node", rvt::WHITE, rvt::XLARGE);
+
+    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.trigger();
+
+    // Getting Basic Information
+    ROS_INFO_NAMED("constraint_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
+    ROS_INFO_NAMED("constraint_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
+    ROS_INFO_NAMED("constraint_planner", "Available Planning Groups:");
+    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
+              std::ostream_iterator<std::string>(std::cout, ", "));
+
+    // Define a collision object ROS message.
+    moveit_msgs::CollisionObject collision_object;
+    collision_object.header.frame_id = move_group.getPlanningFrame();
+
+    // The id of the object is used to identify it.
+    collision_object.id = "box1";
+
+    // Define a box to add to the world.
+    shape_msgs::SolidPrimitive primitive;
+    primitive.type = primitive.BOX;
+    primitive.dimensions.resize(3);
+    primitive.dimensions[0] = 0.4;
+    primitive.dimensions[1] = 0.1;
+    primitive.dimensions[2] = 0.4;
+
+    // Define a pose for the box (specified relative to frame_id)
+    geometry_msgs::Pose box_pose;
+    box_pose.orientation.w = 1.0;
+    box_pose.position.x = 0.4;
+    box_pose.position.y = -0.2;
+    box_pose.position.z = 1.0;
+
+    collision_object.primitives.push_back(primitive);
+    collision_object.primitive_poses.push_back(box_pose);
+    collision_object.operation = collision_object.ADD;
+
+    std::vector<moveit_msgs::CollisionObject> collision_objects;
+    collision_objects.push_back(collision_object);
+
+    // Now, let's add the collision object into the world
+    ROS_INFO_NAMED("constraint_planner", "Add an object into the world");
+    planning_scene_interface.addCollisionObjects(collision_objects);
+
+    // Show text in RViz of status
+    visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
+    visual_tools.trigger();
+    visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");
+
+    // Now when we plan a trajectory it will avoid the obstacle
+    move_group.setStartState(*move_group.getCurrentState());
+    geometry_msgs::Pose another_pose;
+    another_pose.orientation.w = 1.0;
+    another_pose.position.x = 0.4;
+    another_pose.position.y = -0.4;
+    another_pose.position.z = 0.9;
+    move_group.setPoseTarget(another_pose);
+    //move_group.setMaxVelocityScalingFactor(0.5);
+
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    ROS_INFO_NAMED("constraint_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
+
+    visual_tools.prompt("Press 'next' to move the simulated robot.");
+    visual_tools.trigger();
+
+    // Move the simulated robot in gazebo
+    move_group.move();
+
+    ros::shutdown();
+    return 0;
+}
\ No newline at end of file
--- a/src/SampleSimpleMotion.cpp
+++ b/src/SampleSimpleMotion.cpp
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * simple demo of constraint aware planning
+ */
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "SIMPLE PLANNER");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    // wait for robot init of robot_state_initializer
+    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
+    ros::Duration(5.0).sleep();
+    ROS_INFO_NAMED("simple_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+    static const std::string PLANNING_GROUP = "panda_arm";
+    moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
+    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
+    const robot_state::JointModelGroup* joint_model_group =
+            move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);
+
+    // Visualization Setup
+    namespace rvt = rviz_visual_tools;
+    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+    visual_tools.deleteAllMarkers();
+    visual_tools.loadRemoteControl();
+
+    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+    text_pose.translation().z() = 1.75;
+    visual_tools.publishText(text_pose, "simple_planner node", rvt::WHITE, rvt::XLARGE);
+
+    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.trigger();
+
+    // Getting Basic Information
+    ROS_INFO_NAMED("simple_planner", "Planning frame: %s", move_group.getPlanningFrame().c_str());
+    ROS_INFO_NAMED("simple_planner", "End effector link: %s", move_group.getEndEffectorLink().c_str());
+    ROS_INFO_NAMED("simple_planner", "Available Planning Groups:");
+    std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
+              std::ostream_iterator<std::string>(std::cout, ", "));
+
+    move_group.setStartState(*move_group.getCurrentState());
+    geometry_msgs::Pose another_pose;
+    another_pose.orientation.w = 1.0;
+    another_pose.position.x = 0.4;
+    another_pose.position.y = -0.4;
+    another_pose.position.z = 0.9;
+    move_group.setPoseTarget(another_pose);
+
+    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
+    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
+    ROS_INFO_NAMED("simple_planner", "Visualizing constraint plan %s", success ? "" : "FAILED");
+
+    visual_tools.prompt("Press 'next' to move the simulated robot.");
+    visual_tools.trigger();
+
+    // Move the simulated robot in gazebo
+    move_group.move();
+
+    ros::shutdown();
+    return 0;
+}
\ No newline at end of file
--- a/src/SampleTimedCartesianPlanner.cpp
+++ b/src/SampleTimedCartesianPlanner.cpp
+#include <moveit/move_group_interface/move_group_interface.h>
+
+#include <moveit_msgs/DisplayRobotState.h>
+#include <moveit_msgs/DisplayTrajectory.h>
+
+#include <moveit_msgs/AttachedCollisionObject.h>
+#include <moveit_msgs/CollisionObject.h>
+
+#include <moveit_visual_tools/moveit_visual_tools.h>
+#include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
+
+/**
+ * allows time/velocity-constraint planning for cartesian paths
+ */
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "sample_timed_cartesian");
+    ros::NodeHandle node_handle;
+    ros::AsyncSpinner spinner(1);
+    spinner.start();
+
+    // wait for robot init of robot_state_initializer
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAITING FOR ROBOT INIT <<<<<<<<<<<<<<<<");
+    ros::Duration(5.0).sleep();
+    ROS_INFO_NAMED("constraint_planner", ">>>>>>>>>>>>>>>>> WAKING UP AFTER INIT <<<<<<<<<<<<<<<<");
+
+    // Visualization Setup
+    namespace rvt = rviz_visual_tools;
+    moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
+    visual_tools.deleteAllMarkers();
+    visual_tools.loadRemoteControl();
+
+    Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+    text_pose.translation().z() = 1.75;
+    visual_tools.publishText(text_pose, "Sample timed cartesian planner node", rvt::WHITE, rvt::XLARGE);
+
+    // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
+    visual_tools.trigger();
+
+    moveit::planning_interface::MoveGroupInterface group("panda_arm");
+
+    ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
+    moveit_msgs::DisplayTrajectory display_trajectory;
+
+    moveit::planning_interface::MoveGroupInterface::Plan plan;
+    group.setStartStateToCurrentState();
+
+    std::vector<geometry_msgs::Pose> waypoints;
+
+    // initial pose of the robot
+    geometry_msgs::Pose start_pose = group.getCurrentPose().pose;
+
+    // waypoints of the target trajectory
+    geometry_msgs::Pose target_pose_1 = group.getCurrentPose().pose;
+    geometry_msgs::Pose target_pose_2 = group.getCurrentPose().pose;
+    geometry_msgs::Pose target_pose_3 = group.getCurrentPose().pose;
+
+    target_pose_1.position.z = start_pose.position.z - 0.4;
+    target_pose_1.position.y = start_pose.position.y;
+    target_pose_1.position.x = start_pose.position.x + 0.5;
+    waypoints.push_back(target_pose_1);
+
+    target_pose_2.position.z = 0.6;
+    target_pose_2.position.y = -0.6;
+    target_pose_2.position.x = 0;
+    waypoints.push_back(target_pose_2);
+
+    target_pose_3.position.z = 0.6;
+    target_pose_3.position.y = -0.032;
+    target_pose_3.position.x = -0.6;
+    waypoints.push_back(target_pose_3);
+
+    moveit_msgs::RobotTrajectory trajectory_msg;
+    group.setPlanningTime(10.0);
+
+    double fraction = group.computeCartesianPath(waypoints,0.01,0.0,trajectory_msg, false);
+
+    // The trajectory needs to be modified so it will include velocities as well.
+    // Create a RobotTrajectory object
+    robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "panda_arm");
+
+    // Get a RobotTrajectory from trajectory
+    rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
+
+    // Create a IterativeParabolicTimeParameterization object
+    trajectory_processing::IterativeParabolicTimeParameterization iptp;
+    // Compute computeTimeStamps
+    bool success = iptp.computeTimeStamps(rt);
+    ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
+    rt.getRobotTrajectoryMsg(trajectory_msg);
+
+    // adjust velocities, accelerations and time_from_start
+    for(int i = 0; i < trajectory_msg.joint_trajectory.points.size(); i++)
+    {
+        trajectory_msg.joint_trajectory.points[i].time_from_start.operator*=(2);
+        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).velocities.size(); j++)
+        {
+            if(trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) != 0.0){
+                trajectory_msg.joint_trajectory.points.at(i).velocities.at(j) /= 2;
+            }
+        }
+
+        for(int j = 0; j < trajectory_msg.joint_trajectory.points.at(i).accelerations.size(); j++)
+        {
+            trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j) =
+                    sqrt(std::abs(trajectory_msg.joint_trajectory.points.at(i).accelerations.at(j)));
+        }
+    }
+
+    plan.trajectory_ = trajectory_msg;
+    ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);
+
+    group.execute(plan);
+
+    ros::shutdown();
+    return 0;
+}
\ No newline at end of file