From cf6962f073269e0a4b803d5c30cf471da2905a3c Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 7 Mar 2018 11:00:47 +0100 Subject: [PATCH 1/4] Resolved dependencies --- godel/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/godel/package.xml b/godel/package.xml index 367e1eb8..28a0c2fa 100644 --- a/godel/package.xml +++ b/godel/package.xml @@ -10,7 +10,7 @@ catkin godel_plugins - godel_path_planning + godel_process_planning godel_process_path_generation godel_msgs godel_surface_detection From b714ab16e00e4094522fff3b97ba8b985b40b239 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Wed, 7 Mar 2018 11:07:33 +0100 Subject: [PATCH 2/4] static_cast from MoveItErrorCode to bool --- godel_surface_detection/src/scan/robot_scan.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/godel_surface_detection/src/scan/robot_scan.cpp b/godel_surface_detection/src/scan/robot_scan.cpp index 87f889eb..3862c40a 100644 --- a/godel_surface_detection/src/scan/robot_scan.cpp +++ b/godel_surface_detection/src/scan/robot_scan.cpp @@ -172,7 +172,7 @@ void RobotScan::publish_scan_poses(std::string topic) bool RobotScan::move_to_pose(geometry_msgs::Pose& target_pose) { move_group_ptr_->setPoseTarget(target_pose, params_.tcp_frame); - return move_group_ptr_->move(); + return static_cast(move_group_ptr_->move()); } int RobotScan::scan(bool move_only) @@ -223,7 +223,7 @@ int RobotScan::scan(bool move_only) // move_group_ptr_->setPoseTarget(trajectory_poses[i], params_.tcp_frame); moveit::planning_interface::MoveGroupInterface::Plan my_plan; - bool success = move_group_ptr_->plan(my_plan); + bool success = static_cast(move_group_ptr_->plan(my_plan)); if (!success) { From 3b7373d7615af3c6bf4d2bafbc45d330ce5ada59 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 30 Apr 2018 22:10:56 +0200 Subject: [PATCH 3/4] Fix for trajectory timestamp Errors: "Execution client unavailable or unable to execute approach trajectory." "Dropping all trajectory point(s) out of 30, as they occur before the current time" --- godel_process_execution/src/blend_process_service.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/godel_process_execution/src/blend_process_service.cpp b/godel_process_execution/src/blend_process_service.cpp index 95c4f586..5be619da 100644 --- a/godel_process_execution/src/blend_process_service.cpp +++ b/godel_process_execution/src/blend_process_service.cpp @@ -47,6 +47,10 @@ void godel_process_execution::BlendProcessService::executionCallback( bool godel_process_execution::BlendProcessService::executeProcess( const godel_msgs::ProcessExecutionGoalConstPtr &goal) { + //goal->trajectory_approach.header.stamp = ros::Time::now(); + //goal->trajectory_depart.header.stamp = ros::Time::now(); + //goal->trajectory_process.header.stamp = ros::Time::now(); + godel_msgs::TrajectoryExecution srv_approach; srv_approach.request.wait_for_execution = true; srv_approach.request.trajectory = goal->trajectory_approach; @@ -59,18 +63,21 @@ bool godel_process_execution::BlendProcessService::executeProcess( srv_depart.request.wait_for_execution = true; srv_depart.request.trajectory = goal->trajectory_depart; + srv_approach.request.trajectory.header.stamp = ros::Time::now(); if (!real_client_.call(srv_approach)) { ROS_ERROR("Execution client unavailable or unable to execute approach trajectory."); return false; } + srv_process.request.trajectory.header.stamp = ros::Time::now(); if (!real_client_.call(srv_process)) { ROS_ERROR("Execution client unavailable or unable to execute process trajectory."); return false; } + srv_depart.request.trajectory.header.stamp = ros::Time::now(); if (!real_client_.call(srv_depart)) { ROS_ERROR("Execution client unavailable or unable to execute departure trajectory."); From 7e06720ba8b98d22d6d2d7cd66dfd83d7e315bc2 Mon Sep 17 00:00:00 2001 From: Harsh Deshpande Date: Mon, 30 Apr 2018 22:17:33 +0200 Subject: [PATCH 4/4] Removed comments --- godel_process_execution/src/blend_process_service.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/godel_process_execution/src/blend_process_service.cpp b/godel_process_execution/src/blend_process_service.cpp index 5be619da..ee1b1671 100644 --- a/godel_process_execution/src/blend_process_service.cpp +++ b/godel_process_execution/src/blend_process_service.cpp @@ -47,10 +47,6 @@ void godel_process_execution::BlendProcessService::executionCallback( bool godel_process_execution::BlendProcessService::executeProcess( const godel_msgs::ProcessExecutionGoalConstPtr &goal) { - //goal->trajectory_approach.header.stamp = ros::Time::now(); - //goal->trajectory_depart.header.stamp = ros::Time::now(); - //goal->trajectory_process.header.stamp = ros::Time::now(); - godel_msgs::TrajectoryExecution srv_approach; srv_approach.request.wait_for_execution = true; srv_approach.request.trajectory = goal->trajectory_approach;