From a4e77ac4abff5250f801170e9825d66bef5aa946 Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Tue, 21 Jul 2020 01:17:58 +0200 Subject: [PATCH] set initial speed at the right time, use full robot speed --- src/TimedPlanner.cpp | 25 +++++++++++++------------ 1 file changed, 13 insertions(+), 12 deletions(-) diff --git a/src/TimedPlanner.cpp b/src/TimedPlanner.cpp index e53c46a..9a94fbd 100644 --- a/src/TimedPlanner.cpp +++ b/src/TimedPlanner.cpp @@ -115,7 +115,7 @@ void TimedPlanner::doMotion() { do { for (auto &waypoint : currentTrajectory.waypoints) { moveit::planning_interface::MoveGroupInterface::Plan plan; - if (TrajectoryUtil::computePathToPose(*group, plan, waypoint.pose, waypoint.mode, 0.8, 0.8)) { + if (TrajectoryUtil::computePathToPose(*group, plan, waypoint.pose, waypoint.mode, 1, 1)) { for (auto trajectory : split(*group, plan)) { if (nextTrajectory.is_initialized()) { ROS_WARN_STREAM("terminating current trajectory"); @@ -222,17 +222,6 @@ int main(int argc, char **argv) { moveit::planning_interface::MoveGroupInterface group("panda_arm"); moveit::planning_interface::PlanningSceneInterface planning_scene_interface; - // init motionSpeedFactor - double factor = -1; - if (node_handle.getParam("robot_speed_factor", factor)) { - ros::Publisher pub = node_handle.advertise<std_msgs::Float64>("max_velocity", 1000); - std_msgs::Float64 velocity; - velocity.data = factor; - pub.publish(velocity); - } else { - ROS_WARN_STREAM("No default robot_speed_factor provided!"); - } - // add the ground plate std::vector<moveit_msgs::CollisionObject> collision_objects; constructPlate(collision_objects, 5.0, 5.0); @@ -245,6 +234,18 @@ int main(int argc, char **argv) { ros::Subscriber velocitySubscriber = node_handle.subscribe("max_velocity", 1000, &TimedPlanner::newMotionSpeedFactorCallback, &planner); + + // init motionSpeedFactor + double factor = -1; + if (node_handle.getParam("robot_speed_factor", factor)) { + ros::Publisher pub = node_handle.advertise<std_msgs::Float64>("max_velocity", 1000); + std_msgs::Float64 velocity; + velocity.data = factor; + pub.publish(velocity); + } else { + ROS_WARN_STREAM("No default robot_speed_factor provided!"); + } + while (ros::ok()) { // execute a trajectory -- GitLab