diff --git a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h index 9f2e4f4..3cb4a18 100644 --- a/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h +++ b/src/roboglue_ros/include/roboglue_ros/roboglue_utils.h @@ -98,6 +98,7 @@ typedef struct { robot_state::RobotStatePtr kine_state; std::vector joint_names; std::map joint_homes; + double joint_jump_tresh; } robotData; typedef struct { @@ -114,4 +115,4 @@ std::map floatMap2doubleMap(std::map); double e3dist(geometry_msgs::Twist, geometry_msgs::Twist); double rad2deg(double ang); double deg2rad(double ang); -bool checkJointJump(moveit_msgs::RobotTrajectoryPtr, double); +bool checkJointJump(moveit_msgs::RobotTrajectory*, double); diff --git a/src/roboglue_ros/launch/roboglue_ros.launch b/src/roboglue_ros/launch/roboglue_ros.launch index c4a69b8..e3c465b 100644 --- a/src/roboglue_ros/launch/roboglue_ros.launch +++ b/src/roboglue_ros/launch/roboglue_ros.launch @@ -29,6 +29,7 @@ + diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch index 224d156..ddedb96 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver.launch @@ -33,6 +33,7 @@ + diff --git a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch index 850d4a5..d7b450a 100644 --- a/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch +++ b/src/roboglue_ros/launch/roboglue_ros_urdriver_norviz.launch @@ -30,6 +30,7 @@ + diff --git a/src/roboglue_ros/src/roboglue_recorder.cpp b/src/roboglue_ros/src/roboglue_recorder.cpp index 4738a38..60b71b6 100644 --- a/src/roboglue_ros/src/roboglue_recorder.cpp +++ b/src/roboglue_ros/src/roboglue_recorder.cpp @@ -33,6 +33,7 @@ che non si incappi in configurazioni instabili. 20190708 - Implementazione funzioni di visualizzazione del percorso e pulizia dello schermo prima dell'esecuzione del file. +20191029 - Spostata la documentazione si Git */ ////////// ROS INCLUDES ///////////// #include "ros/ros.h" @@ -260,6 +261,7 @@ int main(int argc, char **argv) { // load robot model information nh.getParam(param_ns+"robot_model_name", robot_data.robot_model_name); nh.getParam(param_ns+"move_group_name", robot_data.move_group_name); + nh.getParam(param_ns+"joint_jump_tresh", robot_data.joint_jump_tresh); /// set console log level /// ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Warn); @@ -365,17 +367,18 @@ int main(int argc, char **argv) { move_group.setStartStateToCurrentState(); move_group.computeCartesianPath(pss,ee_step,jmp_thr,traj, false, &ec); ROS_DEBUG("Trajectory Compute ExitCode: %d",ec.val); -// BUG: il controllo del salto dei giunti crasha il nodo. -// if (checkJointJump(boost::shared_ptr(&traj), 90.0)){ -// ROS_WARN("Joint Jump Detected!!!"); -// } - jointPlan.trajectory_ = traj; - plotTrajectory(&rviz_data, &robot_data, &traj); - // async execute planned trajectory - moveit::planning_interface::MoveItErrorCode retval = move_group.execute(jointPlan); - ROS_INFO("Finished executing first [%u] datapoints, execute: %d", i, retval.val); + // check for joint jump and if not detected execute motion + if (checkJointJump(&traj, robot_data.joint_jump_tresh)){ + ROS_WARN("Joint Jump Detected!!! \n ABORTING EXECUTION "); + } else { + jointPlan.trajectory_ = traj; + plotTrajectory(&rviz_data, &robot_data, &traj); + // async execute planned trajectory + moveit::planning_interface::MoveItErrorCode retval = move_group.execute(jointPlan); + ROS_INFO("Finished executing first [%u] datapoints, execute: %d", i, retval.val); + } } else if (!robot_data.planning_mode.compare("joint")){ - // TODO SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO + // TODO: SCRIVERE LA LOGICA PER IL PERCORSO JOINT QUANDO // QUANDO I PROBLEMI DI STABILITÀ DEL MOVIMENTO SONO RISOLTI } else { ROS_ERROR("Invalid Plannning Mode!"); diff --git a/src/roboglue_ros/src/roboglue_utils.cpp b/src/roboglue_ros/src/roboglue_utils.cpp index 153a05f..ebad69d 100644 --- a/src/roboglue_ros/src/roboglue_utils.cpp +++ b/src/roboglue_ros/src/roboglue_utils.cpp @@ -94,7 +94,7 @@ double deg2rad(double ang){ return ang*M_PI/180; } -bool checkJointJump(moveit_msgs::RobotTrajectoryPtr traj, double thresh){ +bool checkJointJump(moveit_msgs::RobotTrajectory *traj, double thresh){ // identify max joint jump for each joint typedef struct{ double first, second;