diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h deleted file mode 100644 index 220902b224..0000000000 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_arm_data.h +++ /dev/null @@ -1,142 +0,0 @@ -/******************************************************************************* - * Title : jog_arm_data.h - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -#pragma once - -// System -#include -#include - -// Eigen -#include - -// ROS -#include -#include -#include -#include - -// moveit_jog_arm -#include "status_codes.h" - -namespace moveit_jog_arm -{ -// Variables to share between threads, and their mutexes -// Inherit from a mutex so the struct can be locked/unlocked easily -struct JogArmShared : public std::mutex -{ - geometry_msgs::TwistStamped command_deltas; - - control_msgs::JointJog joint_command_deltas; - - sensor_msgs::JointState joints; - - double collision_velocity_scale = 1; - - // Flag a valid incoming Cartesian command having nonzero velocities - bool have_nonzero_cartesian_cmd = false; - - // Flag a valid incoming joint angle command having nonzero velocities - bool have_nonzero_joint_cmd = false; - - // Indicates that we have not received a new command in some time - bool command_is_stale = false; - - // The new command which is calculated - trajectory_msgs::JointTrajectory outgoing_command; - - // Timestamp of incoming commands - ros::Time latest_nonzero_cmd_stamp = ros::Time(0.); - - // Indicates no collision, etc, so outgoing commands can be sent - bool ok_to_publish = false; - - // The transform from the MoveIt planning frame to robot_link_command_frame - Eigen::Isometry3d tf_moveit_to_cmd_frame; - - // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] - std::atomic_bool drift_dimensions[6] = { ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), - ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false), ATOMIC_VAR_INIT(false) }; - - // Status of the jogger. 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - std::atomic status; - - // Pause/unpause jog threads - threads are not paused by default - std::atomic paused{ false }; - - // Stop jog loop threads - threads are not stopped by default - std::atomic stop_requested{ false }; - - // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] - std::atomic_bool control_dimensions[6] = { ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), - ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true), ATOMIC_VAR_INIT(true) }; -}; - -// ROS params to be read. See the yaml file in /config for a description of each. -struct JogArmParameters -{ - std::string move_group_name; - std::string joint_topic; - std::string cartesian_command_in_topic; - std::string robot_link_command_frame; - std::string command_out_topic; - std::string planning_frame; - std::string status_topic; - std::string joint_command_in_topic; - std::string command_in_type; - std::string command_out_type; - double linear_scale; - double rotational_scale; - double joint_scale; - double lower_singularity_threshold; - double hard_stop_singularity_threshold; - double scene_collision_proximity_threshold; - double self_collision_proximity_threshold; - double low_pass_filter_coeff; - double publish_period; - double incoming_command_timeout; - double joint_limit_margin; - double collision_check_rate; - int num_outgoing_halt_msgs_to_publish; - bool use_gazebo; - bool check_collisions; - bool publish_joint_positions; - bool publish_joint_velocities; - bool publish_joint_accelerations; -}; -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h deleted file mode 100644 index 049f7af452..0000000000 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_calcs.h +++ /dev/null @@ -1,187 +0,0 @@ -/******************************************************************************* - * Title : jog_calcs.h - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -#pragma once - -// System -#include - -// ROS -#include -#include -#include - -// moveit_jog_arm -#include "jog_arm_data.h" -#include "low_pass_filter.h" -#include "status_codes.h" - -namespace moveit_jog_arm -{ -class JogCalcs -{ -public: - JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr); - - void startMainLoop(JogArmShared& shared_variables); - - /** \brief Check if the robot state is being updated and the end effector transform is known - * @return true if initialized properly - */ - bool isInitialized(); - -protected: - ros::NodeHandle nh_; - - // Flag that robot state is up to date, end effector transform is known - std::atomic is_initialized_; - - /** \brief Do jogging calculations for Cartesian twist commands. */ - bool cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables); - - /** \brief Do jogging calculations for direct commands to a joint. */ - bool jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables); - - /** \brief Update the stashed status so it can be retrieved asynchronously */ - void updateCachedStatus(JogArmShared& shared_variables); - - /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ - bool updateJoints(JogArmShared& shared_variables); - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - */ - Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; - - /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. - * Also, multiply by timestep to calculate a position change. - */ - Eigen::VectorXd scaleJointCommand(const control_msgs::JointJog& command) const; - - bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; - - /** \brief Suddenly halt for a joint limit or other critical issue. - * Is handled differently for position vs. velocity control. - */ - void suddenHalt(trajectory_msgs::JointTrajectory& joint_traj); - void suddenHalt(Eigen::ArrayXd& delta_theta); - - /** \brief Publish the status of the jogger to a ROS topic */ - void publishStatus() const; - - /** \brief Scale the delta theta to match joint velocity/acceleration limits */ - void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); - - /** \brief Avoid overshooting joint limits */ - bool enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj); - - /** \brief Possibly calculate a velocity scaling factor, due to proximity of - * singularity and direction of motion - */ - double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& jacobian, const Eigen::MatrixXd& pseudo_inverse); - - /** - * Slow motion down if close to singularity or collision. - * @param shared_variables data shared between threads, tells how close we are to collision - * @param delta_theta motion command, used in calculating new_joint_tray - * @param singularity_scale tells how close we are to a singularity - */ - void applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, double singularity_scale); - - /** \brief Compose the outgoing JointTrajectory message */ - trajectory_msgs::JointTrajectory composeJointTrajMessage(sensor_msgs::JointState& joint_state) const; - - /** \brief Smooth position commands with a lowpass filter */ - void lowPassFilterPositions(sensor_msgs::JointState& joint_state); - - /** \brief Convert an incremental position command to joint velocity message */ - void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); - - /** \brief Convert joint deltas to an outgoing JointTrajectory command. - * This happens for joint commands and Cartesian commands. - */ - bool convertDeltasToOutgoingCmd(); - - /** \brief Gazebo simulations have very strict message timestamp requirements. - * Satisfy Gazebo by stuffing multiple messages into one. - */ - void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const; - - /** - * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy - * - * @param matrix The Jacobian matrix. - * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() - * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. - */ - void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); - - const moveit::core::JointModelGroup* joint_model_group_; - - moveit::core::RobotStatePtr kinematic_state_; - - // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. - // internal_joint_state_ is used in jog calculations. It shouldn't be relied on to be accurate. - // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints jog_arm acts on. - sensor_msgs::JointState incoming_joint_state_, internal_joint_state_, original_joint_state_; - std::map joint_state_name_map_; - trajectory_msgs::JointTrajectory outgoing_command_; - - std::vector position_filters_; - - ros::Publisher status_pub_; - - StatusCode status_ = NO_WARNING; - - JogArmParameters parameters_; - - // Use ArrayXd type to enable more coefficient-wise operations - Eigen::ArrayXd delta_theta_; - Eigen::ArrayXd prev_joint_velocity_; - - Eigen::Isometry3d tf_moveit_to_cmd_frame_; - - const int gazebo_redundant_message_count_ = 30; - - uint num_joints_; - - ros::Rate default_sleep_rate_; -}; -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h b/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h deleted file mode 100644 index 9dee171b76..0000000000 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_interface_base.h +++ /dev/null @@ -1,113 +0,0 @@ -/******************************************************************************* - * Title : jog_interface_base.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -#include "collision_check_thread.h" -#include -#include "jog_arm_data.h" -#include "jog_calcs.h" -#include "low_pass_filter.h" -#include -#include -#include -#include -#include -#include - -namespace moveit_jog_arm -{ -/** - * Class JogInterfaceBase - Base class for C++ interface and ROS node. - * Handles ROS subs & pubs and creates the worker threads. - */ -class JogInterfaceBase -{ -public: - JogInterfaceBase(); - - /** \brief Update the joints of the robot */ - void jointsCB(const sensor_msgs::JointStateConstPtr& msg); - - /** - * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. - * This can help avoid singularities. - * - * @param request the service request - * @param response the service response - * @return true if the adjustment was made - */ - bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res); - - /** \brief Start the main calculation thread */ - // Service callback for changing jogging dimensions - bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res); - - // Jogging calculation thread - bool startJogCalcThread(); - - /** \brief Stop the main calculation thread */ - bool stopJogCalcThread(); - - /** \brief Start collision checking */ - bool startCollisionCheckThread(); - - /** \brief Stop collision checking */ - bool stopCollisionCheckThread(); - -protected: - bool readParameters(ros::NodeHandle& n); - - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - - // Store the parameters that were read from ROS server - JogArmParameters ros_parameters_; - - // Share data between threads - JogArmShared shared_variables_; - - // Jog calcs - std::unique_ptr jog_calcs_; - std::unique_ptr jog_calc_thread_; - - // Collision checks - std::unique_ptr collision_checker_; - std::unique_ptr collision_check_thread_; -}; -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch b/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch deleted file mode 100644 index bc347ec658..0000000000 --- a/moveit_experimental/moveit_jog_arm/launch/cpp_interface_example.launch +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp b/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp deleted file mode 100644 index 88f19713d6..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/collision_check_thread.cpp +++ /dev/null @@ -1,159 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Title : collision_check_thread.cpp - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include - -static const std::string LOGNAME = "collision_check_thread"; -static const double MIN_RECOMMENDED_COLLISION_RATE = 10; - -namespace moveit_jog_arm -{ -// Constructor for the class that handles collision checking -CollisionCheckThread::CollisionCheckThread( - const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) - : parameters_(parameters), planning_scene_monitor_(planning_scene_monitor) -{ - if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Collision check rate is low, increase it in yaml file if CPU allows"); -} - -planning_scene_monitor::LockedPlanningSceneRO CollisionCheckThread::getLockedPlanningSceneRO() const -{ - return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); -} - -void CollisionCheckThread::startMainLoop(JogArmShared& shared_variables) -{ - // Init collision request - collision_detection::CollisionRequest collision_request; - collision_request.group_name = parameters_.move_group_name; - collision_request.distance = true; // enable distance-based collision checking - collision_detection::CollisionResult collision_result; - - // Copy the planning scene's version of current state into new memory - moveit::core::RobotState current_state(getLockedPlanningSceneRO()->getCurrentState()); - - const double self_velocity_scale_coefficient = -log(0.001) / parameters_.self_collision_proximity_threshold; - const double scene_velocity_scale_coefficient = -log(0.001) / parameters_.scene_collision_proximity_threshold; - ros::Rate collision_rate(parameters_.collision_check_rate); - - double self_collision_distance = 0; - double scene_collision_distance = 0; - bool collision_detected; - - // Scale robot velocity according to collision proximity and user-defined thresholds. - // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. - // Proximity decreasing --> decelerate - double velocity_scale = 1; - - collision_detection::AllowedCollisionMatrix acm = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); - ///////////////////////////////////////////////// - // Spin while checking collisions - ///////////////////////////////////////////////// - while (ros::ok() && !shared_variables.stop_requested) - { - if (!shared_variables.paused) - { - shared_variables.lock(); - sensor_msgs::JointState jts = shared_variables.joints; - shared_variables.unlock(); - - for (std::size_t i = 0; i < jts.position.size(); ++i) - current_state.setJointPositions(jts.name[i], &jts.position[i]); - - current_state.updateCollisionBodyTransforms(); - collision_detected = false; - - // Do a thread-safe distance-based collision detection - { // Lock PlanningScene - auto scene_ro = getLockedPlanningSceneRO(); - - collision_result.clear(); - scene_ro->getCollisionWorld()->checkRobotCollision(collision_request, collision_result, - *scene_ro->getCollisionRobot(), current_state, acm); - - scene_collision_distance = collision_result.distance; - collision_detected |= collision_result.collision; - - collision_result.clear(); - scene_ro->getCollisionRobotUnpadded()->checkSelfCollision(collision_request, collision_result, current_state, - acm); - } // Unlock PlanningScene - - self_collision_distance = collision_result.distance; - collision_detected |= collision_result.collision; - - velocity_scale = 1; - // If we're definitely in collision, stop immediately - if (collision_detected) - { - velocity_scale = 0; - } - - // If we are far from a collision, velocity_scale should be 1. - // If we are very close to a collision, velocity_scale should be ~zero. - // When scene_collision_proximity_threshold is breached, start decelerating exponentially. - if (scene_collision_distance < parameters_.scene_collision_proximity_threshold) - { - // velocity_scale = e ^ k * (collision_distance - threshold) - // k = - ln(0.001) / collision_proximity_threshold - // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. - // velocity_scale should equal 0.001 when collision_distance is at zero. - velocity_scale = - std::min(velocity_scale, exp(scene_velocity_scale_coefficient * - (scene_collision_distance - parameters_.scene_collision_proximity_threshold))); - } - - if (self_collision_distance < parameters_.self_collision_proximity_threshold) - { - velocity_scale = - std::min(velocity_scale, exp(self_velocity_scale_coefficient * - (self_collision_distance - parameters_.self_collision_proximity_threshold))); - } - - shared_variables.lock(); - shared_variables.collision_velocity_scale = velocity_scale; - shared_variables.unlock(); - } - - collision_rate.sleep(); - } -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp b/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp deleted file mode 100644 index f08a9473bb..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_calcs.cpp +++ /dev/null @@ -1,849 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Title : jog_calcs.cpp - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include - -static const std::string LOGNAME = "jog_calcs"; - -namespace moveit_jog_arm -{ -// Constructor for the class that handles jogging calculations -JogCalcs::JogCalcs(const JogArmParameters& parameters, const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr) - : parameters_(parameters), default_sleep_rate_(1000) -{ - // Publish jogger status - status_pub_ = nh_.advertise(parameters_.status_topic, 1); - - // MoveIt Setup - while (ros::ok() && !model_loader_ptr) - { - ROS_WARN_THROTTLE_NAMED(5, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); - default_sleep_rate_.sleep(); - } - const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); - kinematic_state_ = std::make_shared(kinematic_model); - kinematic_state_->setToDefaultValues(); - - joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); - prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); -} - -void JogCalcs::startMainLoop(JogArmShared& shared_variables) -{ - // Reset flags - is_initialized_ = false; - - // Wait for initial messages - ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Waiting for first joint msg."); - ros::topic::waitForMessage(parameters_.joint_topic); - ROS_INFO_NAMED(LOGNAME, "jog_calcs_thread: Received first joint msg."); - - internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); - num_joints_ = internal_joint_state_.name.size(); - internal_joint_state_.position.resize(num_joints_); - internal_joint_state_.velocity.resize(num_joints_); - // A map for the indices of incoming joint commands - for (std::size_t i = 0; i < internal_joint_state_.name.size(); ++i) - { - joint_state_name_map_[internal_joint_state_.name[i]] = i; - } - - // Low-pass filters for the joint positions & velocities - for (size_t i = 0; i < num_joints_; ++i) - { - position_filters_.emplace_back(parameters_.low_pass_filter_coeff); - } - - // Initialize the position filters to initial robot joints - while (!updateJoints(shared_variables) && ros::ok()) - { - if (shared_variables.stop_requested) - return; - - shared_variables.lock(); - incoming_joint_state_ = shared_variables.joints; - shared_variables.unlock(); - default_sleep_rate_.sleep(); - } - - is_initialized_ = true; - - // Track the number of cycles during which motion has not occurred. - // Will avoid re-publishing zero velocities endlessly. - int zero_velocity_count = 0; - - ros::Rate loop_rate(1. / parameters_.publish_period); - - // Flag for staying inactive while there are no incoming commands - bool wait_for_jog_commands = true; - - // Incoming command messages - geometry_msgs::TwistStamped cartesian_deltas; - control_msgs::JointJog joint_deltas; - - // Do jogging calcs - while (ros::ok() && !shared_variables.stop_requested) - { - // Always update the joints and end-effector transform for 2 reasons: - // 1) in case the getCommandFrameTransform() method is being used - // 2) so the low-pass filters are up to date and don't cause a jump - while (!updateJoints(shared_variables) && ros::ok()) - { - default_sleep_rate_.sleep(); - } - - // Get the transform from MoveIt planning frame to jogging command frame - // We solve (planning_frame -> base -> robot_link_command_frame) - // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) - kinematic_state_->setVariableValues(incoming_joint_state_); - tf_moveit_to_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); - shared_variables.lock(); - shared_variables.tf_moveit_to_cmd_frame = tf_moveit_to_cmd_frame_; - shared_variables.unlock(); - - // If paused or while waiting for initial jog commands, just keep the low-pass filters up to date with current - // joints so a jump doesn't occur when restarting - if (wait_for_jog_commands || shared_variables.paused) - { - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); - - shared_variables.lock(); - // Check if there are any new commands with valid timestamp - wait_for_jog_commands = shared_variables.command_deltas.header.stamp == ros::Time(0.) && - shared_variables.joint_command_deltas.header.stamp == ros::Time(0.); - shared_variables.unlock(); - } - // If not waiting for initial command, and not paused. - // Do jogging calculations only if the robot should move, for efficiency - else - { - // Halt if the command is stale or inputs are all zero, or commands were zero - shared_variables.lock(); - bool have_nonzero_cartesian_cmd = shared_variables.have_nonzero_cartesian_cmd; - bool have_nonzero_joint_cmd = shared_variables.have_nonzero_joint_cmd; - bool stale_command = shared_variables.command_is_stale; - shared_variables.unlock(); - - bool valid_nonzero_command = false; - if (!stale_command) - { - // Prioritize cartesian jogging above joint jogging - if (have_nonzero_cartesian_cmd) - { - shared_variables.lock(); - cartesian_deltas = shared_variables.command_deltas; - shared_variables.unlock(); - - if (!cartesianJogCalcs(cartesian_deltas, shared_variables)) - continue; - } - else if (have_nonzero_joint_cmd) - { - shared_variables.lock(); - joint_deltas = shared_variables.joint_command_deltas; - shared_variables.unlock(); - - if (!jointJogCalcs(joint_deltas, shared_variables)) - continue; - } - - valid_nonzero_command = have_nonzero_cartesian_cmd || have_nonzero_joint_cmd; - } - - // If we should halt - if (!valid_nonzero_command) - { - // Keep the joint position filters up-to-date with current joints - for (std::size_t i = 0; i < num_joints_; ++i) - position_filters_[i].reset(original_joint_state_.position[i]); - - suddenHalt(outgoing_command_); - have_nonzero_cartesian_cmd = false; - have_nonzero_joint_cmd = false; - // Reset the valid command flag so jogging stops until a new command arrives - shared_variables.lock(); - shared_variables.have_nonzero_cartesian_cmd = false; - shared_variables.have_nonzero_joint_cmd = false; - shared_variables.unlock(); - } - - // Send the newest target joints - shared_variables.lock(); - // If everything normal, share the new traj to be published - if (valid_nonzero_command) - { - shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - // Skip the jogging publication if all inputs have been zero for several cycles in a row. - // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. - else if ((parameters_.num_outgoing_halt_msgs_to_publish != 0) && - (zero_velocity_count > parameters_.num_outgoing_halt_msgs_to_publish)) - { - shared_variables.ok_to_publish = false; - } - // The command is invalid but we are publishing num_outgoing_halt_msgs_to_publish - else - { - shared_variables.outgoing_command = outgoing_command_; - shared_variables.ok_to_publish = true; - } - shared_variables.unlock(); - - // Store last zero-velocity message flag to prevent superfluous warnings. - // Cartesian and joint commands must both be zero. - if (!have_nonzero_cartesian_cmd && !have_nonzero_joint_cmd) - { - // Avoid overflow - if (zero_velocity_count < std::numeric_limits::max()) - ++zero_velocity_count; - } - else - zero_velocity_count = 0; - } - - loop_rate.sleep(); - } -} - -bool JogCalcs::isInitialized() -{ - return is_initialized_; -} - -// Perform the jogging calculations -bool JogCalcs::cartesianJogCalcs(geometry_msgs::TwistStamped& cmd, JogArmShared& shared_variables) -{ - // Check for nan's in the incoming command - if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || - std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); - return false; - } - - // If incoming commands should be in the range [-1:1], check for |delta|>1 - if (parameters_.command_in_type == "unitless") - { - if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || - (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "Component of incoming command is >1. Skipping this datapoint."); - return false; - } - } - - // Set uncontrolled dimensions to 0 in command frame - if (!shared_variables.control_dimensions[0]) - cmd.twist.linear.x = 0; - if (!shared_variables.control_dimensions[1]) - cmd.twist.linear.y = 0; - if (!shared_variables.control_dimensions[2]) - cmd.twist.linear.z = 0; - if (!shared_variables.control_dimensions[3]) - cmd.twist.angular.x = 0; - if (!shared_variables.control_dimensions[4]) - cmd.twist.angular.y = 0; - if (!shared_variables.control_dimensions[5]) - cmd.twist.angular.z = 0; - - // Transform the command to the MoveGroup planning frame - if (cmd.header.frame_id != parameters_.planning_frame) - { - Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); - Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); - - // We solve (planning_frame -> base -> cmd.header.frame_id) - // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) - const auto tf_planning_to_cmd_frame = - kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * - kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); - - translation_vector = tf_planning_to_cmd_frame.linear() * translation_vector; - angular_vector = tf_planning_to_cmd_frame.linear() * angular_vector; - - // Put these components back into a TwistStamped - cmd.header.frame_id = parameters_.planning_frame; - cmd.twist.linear.x = translation_vector(0); - cmd.twist.linear.y = translation_vector(1); - cmd.twist.linear.z = translation_vector(2); - cmd.twist.angular.x = angular_vector(0); - cmd.twist.angular.y = angular_vector(1); - cmd.twist.angular.z = angular_vector(2); - } - - Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); - - // Convert from cartesian commands to joint commands - Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_); - - // May allow some dimensions to drift, based on shared_variables.drift_dimensions - // i.e. take advantage of task redundancy. - // Remove the Jacobian rows corresponding to True in the vector shared_variables.drift_dimensions - // Work backwards through the 6-vector so indices don't get out of order - for (auto dimension = jacobian.rows(); dimension >= 0; --dimension) - { - if (shared_variables.drift_dimensions[dimension] && jacobian.rows() > 1) - { - removeDimension(jacobian, delta_x, dimension); - } - } - - Eigen::JacobiSVD svd = - Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); - Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); - Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); - - delta_theta_ = pseudo_inverse * delta_x; - - enforceSRDFAccelVelLimits(delta_theta_); - - // If close to a collision or a singularity, decelerate - applyVelocityScaling(shared_variables, delta_theta_, - velocityScalingFactorForSingularity(delta_x, svd, jacobian, pseudo_inverse)); - if (status_ == HALT_FOR_COLLISION) - { - ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); - suddenHalt(delta_theta_); - } - - prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - - publishStatus(); - // Cache the status so it can be retrieved asynchronously - updateCachedStatus(shared_variables); - - return convertDeltasToOutgoingCmd(); -} - -bool JogCalcs::jointJogCalcs(const control_msgs::JointJog& cmd, JogArmShared& shared_variables) -{ - // Check for nan's or |delta|>1 in the incoming command - for (double velocity : cmd.velocities) - { - if (std::isnan(velocity)) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, "nan in incoming command. Skipping this datapoint."); - return false; - } - } - - // Apply user-defined scaling - delta_theta_ = scaleJointCommand(cmd); - - enforceSRDFAccelVelLimits(delta_theta_); - - kinematic_state_->setVariableValues(internal_joint_state_); - - prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; - - publishStatus(); - // Cache the status so it can be retrieved asynchronously - updateCachedStatus(shared_variables); - - return convertDeltasToOutgoingCmd(); -} - -void JogCalcs::updateCachedStatus(JogArmShared& shared_variables) -{ - shared_variables.status = status_; - status_ = NO_WARNING; -} - -bool JogCalcs::convertDeltasToOutgoingCmd() -{ - internal_joint_state_ = original_joint_state_; - if (!addJointIncrements(internal_joint_state_, delta_theta_)) - return false; - - lowPassFilterPositions(internal_joint_state_); - - // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked - calculateJointVelocities(internal_joint_state_, delta_theta_); - - outgoing_command_ = composeJointTrajMessage(internal_joint_state_); - - if (!enforceSRDFPositionLimits(outgoing_command_)) - { - suddenHalt(outgoing_command_); - status_ = JOINT_BOUND; - } - - // done with calculations - if (parameters_.use_gazebo) - { - insertRedundantPointsIntoTrajectory(outgoing_command_, gazebo_redundant_message_count_); - } - - return true; -} - -// Spam several redundant points into the trajectory. The first few may be skipped if the -// time stamp is in the past when it reaches the client. Needed for gazebo simulation. -// Start from 2 because the first point's timestamp is already 1*parameters_.publish_period -void JogCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& trajectory, int count) const -{ - auto point = trajectory.points[0]; - // Start from 2 because we already have the first point. End at count+1 so (total #) == count - for (int i = 2; i < count + 1; ++i) - { - point.time_from_start = ros::Duration(i * parameters_.publish_period); - trajectory.points.push_back(point); - } -} - -void JogCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) -{ - for (size_t i = 0; i < position_filters_.size(); ++i) - { - joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); - } -} - -void JogCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) -{ - for (int i = 0; i < delta_theta.size(); ++i) - { - joint_state.velocity[i] = delta_theta[i] / parameters_.publish_period; - } -} - -trajectory_msgs::JointTrajectory JogCalcs::composeJointTrajMessage(sensor_msgs::JointState& joint_state) const -{ - trajectory_msgs::JointTrajectory new_joint_traj; - new_joint_traj.header.frame_id = parameters_.planning_frame; - new_joint_traj.header.stamp = ros::Time::now(); - new_joint_traj.joint_names = joint_state.name; - - trajectory_msgs::JointTrajectoryPoint point; - point.time_from_start = ros::Duration(parameters_.publish_period); - if (parameters_.publish_joint_positions) - point.positions = joint_state.position; - if (parameters_.publish_joint_velocities) - point.velocities = joint_state.velocity; - if (parameters_.publish_joint_accelerations) - { - // I do not know of a robot that takes acceleration commands. - // However, some controllers check that this data is non-empty. - // Send all zeros, for now. - std::vector acceleration(num_joints_); - point.accelerations = acceleration; - } - new_joint_traj.points.push_back(point); - - return new_joint_traj; -} - -// Apply velocity scaling for proximity of collisions and singularities. -// Scale for collisions is read from a shared variable. -void JogCalcs::applyVelocityScaling(JogArmShared& shared_variables, Eigen::ArrayXd& delta_theta, - double singularity_scale) -{ - shared_variables.lock(); - double collision_scale = shared_variables.collision_velocity_scale; - shared_variables.unlock(); - - if (collision_scale > 0 && collision_scale < 1) - { - status_ = DECELERATE_FOR_COLLISION; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); - } - else if (collision_scale == 0) - { - status_ = HALT_FOR_COLLISION; - } - - delta_theta = collision_scale * singularity_scale * delta_theta; -} - -// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion -double JogCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, - const Eigen::JacobiSVD& svd, - const Eigen::MatrixXd& jacobian, - const Eigen::MatrixXd& pseudo_inverse) -{ - double velocity_scale = 1; - std::size_t num_dimensions = jacobian.rows(); - - // Find the direction away from nearest singularity. - // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. - // The sign can flip at any time, so we have to do some extra checking. - // Look ahead to see if the Jacobian's condition will decrease. - Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); - - double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); - - // This singular vector tends to flip direction unpredictably. See R. Bro, - // "Resolving the Sign Ambiguity in the Singular Value Decomposition". - // Look ahead to see if the Jacobian's condition will decrease in this - // direction. Start with a scaled version of the singular vector - Eigen::VectorXd delta_x(num_dimensions); - double scale = 100; - delta_x = vector_toward_singularity / scale; - - // Calculate a small change in joints - Eigen::VectorXd new_theta; - kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); - new_theta += pseudo_inverse * delta_x; - kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); - - Eigen::JacobiSVD new_svd(jacobian); - double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); - // If new_condition < ini_condition, the singular vector does point towards a - // singularity. Otherwise, flip its direction. - if (ini_condition >= new_condition) - { - vector_toward_singularity *= -1; - } - - // If this dot product is positive, we're moving toward singularity ==> decelerate - double dot = vector_toward_singularity.dot(commanded_velocity); - if (dot > 0) - { - // Ramp velocity down linearly when the Jacobian condition is between lower_singularity_threshold and - // hard_stop_singularity_threshold, and we're moving towards the singularity - if ((ini_condition > parameters_.lower_singularity_threshold) && - (ini_condition < parameters_.hard_stop_singularity_threshold)) - { - velocity_scale = 1. - - (ini_condition - parameters_.lower_singularity_threshold) / - (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); - status_ = DECELERATE_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); - } - - // Very close to singularity, so halt. - else if (ini_condition > parameters_.hard_stop_singularity_threshold) - { - velocity_scale = 0; - status_ = HALT_FOR_SINGULARITY; - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, JOG_ARM_STATUS_CODE_MAP.at(status_)); - } - } - - return velocity_scale; -} - -void JogCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) -{ - Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; - const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; - - std::size_t joint_delta_index = 0; - for (auto joint : joint_model_group_->getActiveJointModels()) - { - // Some joints do not have bounds defined - const auto bounds = joint->getVariableBounds(joint->getName()); - if (bounds.acceleration_bounded_) - { - bool clip_acceleration = false; - double acceleration_limit = 0.0; - if (acceleration(joint_delta_index) < bounds.min_acceleration_) - { - clip_acceleration = true; - acceleration_limit = bounds.min_acceleration_; - } - else if (acceleration(joint_delta_index) > bounds.max_acceleration_) - { - clip_acceleration = true; - acceleration_limit = bounds.max_acceleration_; - } - - // Apply acceleration bounds - if (clip_acceleration) - { - // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t - // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t - const double relative_change = - ((acceleration_limit * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * - parameters_.publish_period) / - delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); - } - } - - if (bounds.velocity_bounded_) - { - velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_.publish_period; - - bool clip_velocity = false; - double velocity_limit = 0.0; - if (velocity(joint_delta_index) < bounds.min_velocity_) - { - clip_velocity = true; - velocity_limit = bounds.min_velocity_; - } - else if (velocity(joint_delta_index) > bounds.max_velocity_) - { - clip_velocity = true; - velocity_limit = bounds.max_velocity_; - } - - // Apply velocity bounds - if (clip_velocity) - { - // delta_theta = joint_velocity * delta_t - const double relative_change = (velocity_limit * parameters_.publish_period) / delta_theta(joint_delta_index); - // Avoid nan - if (fabs(relative_change) < 1) - { - delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); - velocity(joint_delta_index) = relative_change * velocity(joint_delta_index); - } - } - ++joint_delta_index; - } - } -} - -bool JogCalcs::enforceSRDFPositionLimits(trajectory_msgs::JointTrajectory& new_joint_traj) -{ - bool halting = false; - - for (auto joint : joint_model_group_->getActiveJointModels()) - { - // Halt if we're past a joint margin and joint velocity is moving even farther past - double joint_angle = 0; - for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c) - { - if (original_joint_state_.name[c] == joint->getName()) - { - joint_angle = original_joint_state_.position.at(c); - break; - } - } - if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) - { - const std::vector limits = joint->getVariableBoundsMsg(); - - // Joint limits are not defined for some joints. Skip them. - if (!limits.empty()) - { - if ((kinematic_state_->getJointVelocities(joint)[0] < 0 && - (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || - (kinematic_state_->getJointVelocities(joint)[0] > 0 && - (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) - { - ROS_WARN_STREAM_THROTTLE_NAMED(2, LOGNAME, ros::this_node::getName() << " " << joint->getName() - << " close to a " - " position limit. Halting."); - halting = true; - } - } - } - } - return !halting; -} - -void JogCalcs::publishStatus() const -{ - std_msgs::Int8 status_msg; - status_msg.data = status_; - status_pub_.publish(status_msg); -} - -// Suddenly halt for a joint limit or other critical issue. -// Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(Eigen::ArrayXd& delta_theta) -{ - delta_theta = Eigen::ArrayXd::Zero(delta_theta.rows()); -} - -// Suddenly halt for a joint limit or other critical issue. -// Is handled differently for position vs. velocity control. -void JogCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_traj) -{ - if (joint_traj.points.empty()) - { - joint_traj.points.push_back(trajectory_msgs::JointTrajectoryPoint()); - joint_traj.points[0].positions.resize(num_joints_); - joint_traj.points[0].velocities.resize(num_joints_); - } - - for (std::size_t i = 0; i < num_joints_; ++i) - { - // For position-controlled robots, can reset the joints to a known, good state - if (parameters_.publish_joint_positions) - joint_traj.points[0].positions[i] = original_joint_state_.position[i]; - - // For velocity-controlled robots, stop - if (parameters_.publish_joint_velocities) - joint_traj.points[0].velocities[i] = 0; - } -} - -// Parse the incoming joint msg for the joints of our MoveGroup -bool JogCalcs::updateJoints(JogArmShared& shared_variables) -{ - shared_variables.lock(); - incoming_joint_state_ = shared_variables.joints; - shared_variables.unlock(); - - // Check that the msg contains enough joints - if (incoming_joint_state_.name.size() < num_joints_) - return false; - - // Store joints in a member variable - for (std::size_t m = 0; m < incoming_joint_state_.name.size(); ++m) - { - std::size_t c; - try - { - c = joint_state_name_map_.at(incoming_joint_state_.name[m]); - } - catch (const std::out_of_range& e) - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); - continue; - } - - internal_joint_state_.position[c] = incoming_joint_state_.position[m]; - } - - // Cache the original joints in case they need to be reset - original_joint_state_ = internal_joint_state_; - - return true; -} - -// Scale the incoming jog command -Eigen::VectorXd JogCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const -{ - Eigen::VectorXd result(6); - - // Apply user-defined scaling if inputs are unitless [-1:1] - if (parameters_.command_in_type == "unitless") - { - result[0] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.x; - result[1] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.y; - result[2] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.z; - result[3] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.x; - result[4] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.y; - result[5] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.z; - } - // Otherwise, commands are in m/s and rad/s - else if (parameters_.command_in_type == "speed_units") - { - result[0] = command.twist.linear.x * parameters_.publish_period; - result[1] = command.twist.linear.y * parameters_.publish_period; - result[2] = command.twist.linear.z * parameters_.publish_period; - result[3] = command.twist.angular.x * parameters_.publish_period; - result[4] = command.twist.angular.y * parameters_.publish_period; - result[5] = command.twist.angular.z * parameters_.publish_period; - } - else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type"); - - return result; -} - -Eigen::VectorXd JogCalcs::scaleJointCommand(const control_msgs::JointJog& command) const -{ - Eigen::VectorXd result(num_joints_); - - for (std::size_t i = 0; i < num_joints_; ++i) - { - result[i] = 0.0; - } - - std::size_t c; - for (std::size_t m = 0; m < command.joint_names.size(); ++m) - { - try - { - c = joint_state_name_map_.at(command.joint_names[m]); - } - catch (const std::out_of_range& e) - { - ROS_WARN_STREAM_THROTTLE_NAMED(5, LOGNAME, "Ignoring joint " << incoming_joint_state_.name[m]); - continue; - } - // Apply user-defined scaling if inputs are unitless [-1:1] - if (parameters_.command_in_type == "unitless") - result[c] = command.velocities[m] * parameters_.joint_scale * parameters_.publish_period; - // Otherwise, commands are in m/s and rad/s - else if (parameters_.command_in_type == "speed_units") - result[c] = command.velocities[m] * parameters_.publish_period; - else - ROS_ERROR_STREAM_NAMED(LOGNAME, "Unexpected command_in_type, check yaml file."); - } - - return result; -} - -// Add the deltas to each joint -bool JogCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const -{ - for (std::size_t i = 0, size = static_cast(increments.size()); i < size; ++i) - { - try - { - output.position[i] += increments[static_cast(i)]; - } - catch (const std::out_of_range& e) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, ros::this_node::getName() << " Lengths of output and " - "increments do not match."); - return false; - } - } - - return true; -} - -void JogCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) -{ - unsigned int num_rows = jacobian.rows() - 1; - unsigned int num_cols = jacobian.cols(); - - if (row_to_remove < num_rows) - { - jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) = - jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols); - delta_x.segment(row_to_remove, num_rows - row_to_remove) = - delta_x.segment(row_to_remove + 1, num_rows - row_to_remove); - } - jacobian.conservativeResize(num_rows, num_cols); - delta_x.conservativeResize(num_rows); -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp deleted file mode 100644 index 47e23f5eb0..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_cpp_interface.cpp +++ /dev/null @@ -1,232 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Title : jog_cpp_interface.cpp - * Project : moveit_jog_arm - * Created : 11/20/2019 - * Author : Andy Zelenak - */ - -#include "moveit_jog_arm/jog_cpp_interface.h" - -static const std::string LOGNAME = "jog_cpp_interface"; - -namespace moveit_jog_arm -{ -JogCppInterface::JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) -{ - planning_scene_monitor_ = planning_scene_monitor; - - // Read ROS parameters, typically from YAML file - if (!readParameters(nh_)) - exit(EXIT_FAILURE); -} - -JogCppInterface::~JogCppInterface() -{ - stopMainLoop(); -} - -void JogCppInterface::startMainLoop() -{ - // Reset loop termination flag - shared_variables_.stop_requested = false; - shared_variables_.paused = false; - - // Crunch the numbers in this thread - startJogCalcThread(); - - // Check collisions in this thread - if (ros_parameters_.check_collisions) - startCollisionCheckThread(); - - // ROS subscriptions. Share the data with the worker threads - ros::Subscriber joints_sub = - nh_.subscribe(ros_parameters_.joint_topic, 1, &JogInterfaceBase::jointsCB, dynamic_cast(this)); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - ros::Publisher outgoing_cmd_pub; - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub = nh_.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub = nh_.advertise(ros_parameters_.command_out_topic, 1); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); - - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); - - ros::Rate main_rate(1. / ros_parameters_.publish_period); - - while (ros::ok() && !shared_variables_.stop_requested) - { - ros::spinOnce(); - - if (!shared_variables_.paused) - { - shared_variables_.lock(); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check if incoming commands are stale - shared_variables_.command_is_stale = (ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= - ros::Duration(ros_parameters_.incoming_command_timeout); - - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); - } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - - shared_variables_.unlock(); - } - - main_rate.sleep(); - } - - stopJogCalcThread(); - stopCollisionCheckThread(); -} - -void JogCppInterface::stopMainLoop() -{ - shared_variables_.stop_requested = true; -} - -void JogCppInterface::setPaused(bool paused) -{ - shared_variables_.paused = paused; -} - -void JogCppInterface::provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command) -{ - shared_variables_.lock(); - - shared_variables_.command_deltas.twist = velocity_command.twist; - shared_variables_.command_deltas.header = velocity_command.header; - - // Input frame determined by YAML file if not passed with message - if (shared_variables_.command_deltas.header.frame_id.empty()) - { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; - } - - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || - shared_variables_.command_deltas.twist.linear.y != 0.0 || - shared_variables_.command_deltas.twist.linear.z != 0.0 || - shared_variables_.command_deltas.twist.angular.x != 0.0 || - shared_variables_.command_deltas.twist.angular.y != 0.0 || - shared_variables_.command_deltas.twist.angular.z != 0.0; - - if (shared_variables_.have_nonzero_cartesian_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = velocity_command.header.stamp; - } - shared_variables_.unlock(); -}; - -void JogCppInterface::provideJointCommand(const control_msgs::JointJog& joint_command) -{ - shared_variables_.lock(); - shared_variables_.joint_command_deltas = joint_command; - - // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication - bool all_zeros = true; - for (double delta : shared_variables_.joint_command_deltas.velocities) - { - all_zeros &= (delta == 0.0); - }; - shared_variables_.have_nonzero_joint_cmd = !all_zeros; - - if (shared_variables_.have_nonzero_joint_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = joint_command.header.stamp; - } - shared_variables_.unlock(); -} - -sensor_msgs::JointState JogCppInterface::getJointState() -{ - shared_variables_.lock(); - sensor_msgs::JointState current_joints = shared_variables_.joints; - shared_variables_.unlock(); - - return current_joints; -} - -bool JogCppInterface::getCommandFrameTransform(Eigen::Isometry3d& transform) -{ - if (!jog_calcs_ || !jog_calcs_->isInitialized()) - return false; - - shared_variables_.lock(); - transform = shared_variables_.tf_moveit_to_cmd_frame; - shared_variables_.unlock(); - - // All zeros means the transform wasn't initialized, so return false - return !transform.matrix().isZero(0); -} - -StatusCode JogCppInterface::getJoggerStatus() -{ - return shared_variables_.status; -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp b/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp deleted file mode 100644 index 3cd342a100..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_interface_base.cpp +++ /dev/null @@ -1,325 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Title : jog_interface_base.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -#include "moveit_jog_arm/jog_interface_base.h" - -static const std::string LOGNAME = "jog_interface_base"; - -namespace moveit_jog_arm -{ -JogInterfaceBase::JogInterfaceBase() -{ -} - -// Read ROS parameters, typically from YAML file -bool JogInterfaceBase::readParameters(ros::NodeHandle& n) -{ - std::size_t error = 0; - - // Specified in the launch file. All other parameters will be read from this namespace. - std::string parameter_ns; - ros::param::get("~parameter_ns", parameter_ns); - if (parameter_ns.empty()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); - ROS_ERROR_STREAM_NAMED(LOGNAME, ""); - return false; - } - - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_period", ros_parameters_.publish_period); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/collision_check_rate", ros_parameters_.collision_check_rate); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/num_outgoing_halt_msgs_to_publish", - ros_parameters_.num_outgoing_halt_msgs_to_publish); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/linear", ros_parameters_.linear_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/rotational", ros_parameters_.rotational_scale); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/scale/joint", ros_parameters_.joint_scale); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/low_pass_filter_coeff", ros_parameters_.low_pass_filter_coeff); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_topic", ros_parameters_.joint_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_in_type", ros_parameters_.command_in_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/cartesian_command_in_topic", - ros_parameters_.cartesian_command_in_topic); - error += - !rosparam_shortcuts::get("", n, parameter_ns + "/joint_command_in_topic", ros_parameters_.joint_command_in_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/robot_link_command_frame", - ros_parameters_.robot_link_command_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/incoming_command_timeout", - ros_parameters_.incoming_command_timeout); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/lower_singularity_threshold", - ros_parameters_.lower_singularity_threshold); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/hard_stop_singularity_threshold", - ros_parameters_.hard_stop_singularity_threshold); - // parameter was removed, replaced with separate self- and scene-collision proximity thresholds; the logic handling - // the different possible sets of defined parameters is somewhat complicated at this point - // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling - bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( - "", n, parameter_ns + "/self_collision_proximity_threshold", ros_parameters_.self_collision_proximity_threshold); - bool have_scene_collision_proximity_threshold = - rosparam_shortcuts::get("", n, parameter_ns + "/scene_collision_proximity_threshold", - ros_parameters_.scene_collision_proximity_threshold); - double collision_proximity_threshold; - if (n.hasParam(parameter_ns + "/collision_proximity_threshold") && - rosparam_shortcuts::get("", n, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) - { - ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" - "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " - "parameters. Please update the jogging yaml file."); - if (!have_self_collision_proximity_threshold) - { - ros_parameters_.self_collision_proximity_threshold = collision_proximity_threshold; - have_self_collision_proximity_threshold = true; - } - if (!have_scene_collision_proximity_threshold) - { - ros_parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; - have_scene_collision_proximity_threshold = true; - } - } - error += !have_self_collision_proximity_threshold; - error += !have_scene_collision_proximity_threshold; - error += !rosparam_shortcuts::get("", n, parameter_ns + "/move_group_name", ros_parameters_.move_group_name); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/planning_frame", ros_parameters_.planning_frame); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/use_gazebo", ros_parameters_.use_gazebo); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/check_collisions", ros_parameters_.check_collisions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/joint_limit_margin", ros_parameters_.joint_limit_margin); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_topic", ros_parameters_.command_out_topic); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/command_out_type", ros_parameters_.command_out_type); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_positions", - ros_parameters_.publish_joint_positions); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_velocities", - ros_parameters_.publish_joint_velocities); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/publish_joint_accelerations", - ros_parameters_.publish_joint_accelerations); - - // This parameter name was changed recently. - // Try retrieving from the correct name. If it fails, then try the deprecated name. - // TODO(andyz): remove this deprecation warning in ROS Noetic - if (!rosparam_shortcuts::get("", n, parameter_ns + "/status_topic", ros_parameters_.status_topic)) - { - ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " - "the jogging yaml file."); - error += !rosparam_shortcuts::get("", n, parameter_ns + "/warning_topic", ros_parameters_.status_topic); - } - - rosparam_shortcuts::shutdownIfError(parameter_ns, error); - - // Input checking - if (ros_parameters_.publish_period <= 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'publish_period' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.num_outgoing_halt_msgs_to_publish < 0) - { - ROS_WARN_NAMED(LOGNAME, - "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.hard_stop_singularity_threshold < ros_parameters_.lower_singularity_threshold) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'hard_stop_singularity_threshold' " - "should be greater than 'lower_singularity_threshold.' " - "Check yaml file."); - return false; - } - if ((ros_parameters_.hard_stop_singularity_threshold < 0.) || (ros_parameters_.lower_singularity_threshold < 0.)) - { - ROS_WARN_NAMED(LOGNAME, "Parameters 'hard_stop_singularity_threshold' " - "and 'lower_singularity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.self_collision_proximity_threshold < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.scene_collision_proximity_threshold < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.scene_collision_proximity_threshold < ros_parameters_.self_collision_proximity_threshold) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " - "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); - } - if (ros_parameters_.low_pass_filter_coeff < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.joint_limit_margin < 0.) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " - "greater than zero. Check yaml file."); - return false; - } - if (ros_parameters_.command_in_type != "unitless" && ros_parameters_.command_in_type != "speed_units") - { - ROS_WARN_NAMED(LOGNAME, "command_in_type should be 'unitless' or " - "'speed_units'. Check yaml file."); - return false; - } - if (ros_parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && - ros_parameters_.command_out_type != "std_msgs/Float64MultiArray") - { - ROS_WARN_NAMED(LOGNAME, "Parameter command_out_type should be " - "'trajectory_msgs/JointTrajectory' or " - "'std_msgs/Float64MultiArray'. Check yaml file."); - return false; - } - if (!ros_parameters_.publish_joint_positions && !ros_parameters_.publish_joint_velocities && - !ros_parameters_.publish_joint_accelerations) - { - ROS_WARN_NAMED(LOGNAME, "At least one of publish_joint_positions / " - "publish_joint_velocities / " - "publish_joint_accelerations must be true. Check " - "yaml file."); - return false; - } - if ((ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") && ros_parameters_.publish_joint_positions && - ros_parameters_.publish_joint_velocities) - { - ROS_WARN_NAMED(LOGNAME, "When publishing a std_msgs/Float64MultiArray, " - "you must select positions OR velocities."); - return false; - } - if (ros_parameters_.collision_check_rate < 0) - { - ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " - "greater than zero. Check yaml file."); - return false; - } - - return true; -} - -// Listen to joint angles. Store them in a shared variable. -void JogInterfaceBase::jointsCB(const sensor_msgs::JointStateConstPtr& msg) -{ - shared_variables_.lock(); - shared_variables_.joints = *msg; - shared_variables_.unlock(); -} - -bool JogInterfaceBase::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, - moveit_msgs::ChangeDriftDimensions::Response& res) -{ - // These are std::atomic's, they are threadsafe without a mutex lock - shared_variables_.drift_dimensions[0] = req.drift_x_translation; - shared_variables_.drift_dimensions[1] = req.drift_y_translation; - shared_variables_.drift_dimensions[2] = req.drift_z_translation; - shared_variables_.drift_dimensions[3] = req.drift_x_rotation; - shared_variables_.drift_dimensions[4] = req.drift_y_rotation; - shared_variables_.drift_dimensions[5] = req.drift_z_rotation; - - res.success = true; - return true; -} - -bool JogInterfaceBase::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, - moveit_msgs::ChangeControlDimensions::Response& res) -{ - shared_variables_.control_dimensions[0] = req.control_x_translation; - shared_variables_.control_dimensions[1] = req.control_y_translation; - shared_variables_.control_dimensions[2] = req.control_z_translation; - shared_variables_.control_dimensions[3] = req.control_x_rotation; - shared_variables_.control_dimensions[4] = req.control_y_rotation; - shared_variables_.control_dimensions[5] = req.control_z_rotation; - - res.success = true; - return true; -} - -// A separate thread for the heavy jogging calculations. -bool JogInterfaceBase::startJogCalcThread() -{ - if (!jog_calcs_) - jog_calcs_.reset(new JogCalcs(ros_parameters_, planning_scene_monitor_->getRobotModelLoader())); - - jog_calc_thread_.reset(new std::thread([&]() { jog_calcs_->startMainLoop(shared_variables_); })); - - return true; -} - -bool JogInterfaceBase::stopJogCalcThread() -{ - if (jog_calc_thread_) - { - if (jog_calc_thread_->joinable()) - jog_calc_thread_->join(); - - jog_calc_thread_.reset(); - } - - return true; -} - -// A separate thread for collision checking. -bool JogInterfaceBase::startCollisionCheckThread() -{ - if (!collision_checker_) - collision_checker_.reset(new CollisionCheckThread(ros_parameters_, planning_scene_monitor_)); - - collision_check_thread_.reset(new std::thread([&]() { collision_checker_->startMainLoop(shared_variables_); })); - - return true; -} - -bool JogInterfaceBase::stopCollisionCheckThread() -{ - if (collision_check_thread_) - { - if (collision_check_thread_->joinable()) - collision_check_thread_->join(); - - collision_check_thread_.reset(); - } - - return true; -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp b/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp deleted file mode 100644 index 567e0b707a..0000000000 --- a/moveit_experimental/moveit_jog_arm/src/jog_ros_interface.cpp +++ /dev/null @@ -1,223 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2019, Los Alamos National Security, LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -*******************************************************************************/ - -/* Title : jog_ros_interface.cpp - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - */ - -// Server node for arm jogging with MoveIt. - -#include - -static const std::string LOGNAME = "jog_ros_interface"; - -namespace moveit_jog_arm -{ -///////////////////////////////////////////////////////////////////////////////// -// JogROSInterface handles ROS subscriptions and instantiates the worker threads. -// One worker thread does the jogging calculations. -// Another worker thread does collision checking. -///////////////////////////////////////////////////////////////////////////////// - -// Constructor for the main ROS interface node -JogROSInterface::JogROSInterface() -{ - ros::NodeHandle nh; - - // Read ROS parameters, typically from YAML file - if (!readParameters(nh)) - exit(EXIT_FAILURE); - - // Load the planning scene monitor - planning_scene_monitor_ = std::make_shared("robot_description"); - if (!planning_scene_monitor_->getPlanningScene()) - { - ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); - exit(EXIT_FAILURE); - } - - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startWorldGeometryMonitor( - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, - planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, - false /* skip octomap monitor */); - planning_scene_monitor_->startStateMonitor(); - - // Crunch the numbers in this thread - startJogCalcThread(); - - // Check collisions in this thread - if (ros_parameters_.check_collisions) - startCollisionCheckThread(); - - // ROS subscriptions. Share the data with the worker threads - ros::Subscriber cmd_sub = - nh.subscribe(ros_parameters_.cartesian_command_in_topic, 1, &JogROSInterface::deltaCartesianCmdCB, this); - ros::Subscriber joint_jog_cmd_sub = - nh.subscribe(ros_parameters_.joint_command_in_topic, 1, &JogROSInterface::deltaJointCmdCB, this); - ros::Subscriber joints_sub = - nh.subscribe(ros_parameters_.joint_topic, 1, &JogInterfaceBase::jointsCB, dynamic_cast(this)); - - // ROS Server for allowing drift in some dimensions - ros::ServiceServer drift_dimensions_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", - &JogInterfaceBase::changeDriftDimensions, dynamic_cast(this)); - // ROS Server for changing the control dimensions - ros::ServiceServer dims_server = - nh.advertiseService(nh.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", - &JogInterfaceBase::changeControlDimensions, dynamic_cast(this)); - - // Publish freshly-calculated joints to the robot. - // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - ros::Publisher outgoing_cmd_pub; - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - outgoing_cmd_pub = nh.advertise(ros_parameters_.command_out_topic, 1); - - // Wait for incoming topics to appear - ROS_DEBUG_NAMED(LOGNAME, "Waiting for JointState topic"); - ros::topic::waitForMessage(ros_parameters_.joint_topic); - - // Wait for low pass filters to stabilize - ROS_INFO_STREAM_NAMED(LOGNAME, "Waiting for low-pass filters to stabilize."); - ros::Duration(10 * ros_parameters_.publish_period).sleep(); - - ros::Rate main_rate(1. / ros_parameters_.publish_period); - - while (ros::ok()) - { - ros::spinOnce(); - - shared_variables_.lock(); - trajectory_msgs::JointTrajectory outgoing_command = shared_variables_.outgoing_command; - - // Check for stale cmds - shared_variables_.command_is_stale = ((ros::Time::now() - shared_variables_.latest_nonzero_cmd_stamp) >= - ros::Duration(ros_parameters_.incoming_command_timeout)); - - // Publish the most recent trajectory, unless the jogging calculation thread tells not to - if (shared_variables_.ok_to_publish) - { - // Put the outgoing msg in the right format - // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). - if (ros_parameters_.command_out_type == "trajectory_msgs/JointTrajectory") - { - outgoing_command.header.stamp = ros::Time::now(); - outgoing_cmd_pub.publish(outgoing_command); - } - else if (ros_parameters_.command_out_type == "std_msgs/Float64MultiArray") - { - std_msgs::Float64MultiArray joints; - if (ros_parameters_.publish_joint_positions) - joints.data = outgoing_command.points[0].positions; - else if (ros_parameters_.publish_joint_velocities) - joints.data = outgoing_command.points[0].velocities; - outgoing_cmd_pub.publish(joints); - } - } - else if (shared_variables_.command_is_stale) - { - ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " - "Try a larger 'incoming_command_timeout' parameter?"); - } - else - { - ROS_DEBUG_STREAM_THROTTLE_NAMED(10, LOGNAME, "All-zero command. Doing nothing."); - } - - shared_variables_.unlock(); - - main_rate.sleep(); - } - - // Stop JogArm threads - shared_variables_.stop_requested = true; - stopJogCalcThread(); - stopCollisionCheckThread(); -} - -// Listen to cartesian delta commands. Store them in a shared variable. -void JogROSInterface::deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg) -{ - shared_variables_.lock(); - - // Copy everything but the frame name. The frame name is set by yaml file at startup. - // (so it isn't copied over and over) - shared_variables_.command_deltas.twist = msg->twist; - shared_variables_.command_deltas.header = msg->header; - - // Input frame determined by YAML file if not passed with message - if (shared_variables_.command_deltas.header.frame_id.empty()) - { - shared_variables_.command_deltas.header.frame_id = ros_parameters_.robot_link_command_frame; - } - - // Check if input is all zeros. Flag it if so to skip calculations/publication after num_outgoing_halt_msgs_to_publish - shared_variables_.have_nonzero_cartesian_cmd = shared_variables_.command_deltas.twist.linear.x != 0.0 || - shared_variables_.command_deltas.twist.linear.y != 0.0 || - shared_variables_.command_deltas.twist.linear.z != 0.0 || - shared_variables_.command_deltas.twist.angular.x != 0.0 || - shared_variables_.command_deltas.twist.angular.y != 0.0 || - shared_variables_.command_deltas.twist.angular.z != 0.0; - - if (shared_variables_.have_nonzero_cartesian_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; - } - shared_variables_.unlock(); -} - -// Listen to joint delta commands. Store them in a shared variable. -void JogROSInterface::deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg) -{ - shared_variables_.lock(); - shared_variables_.joint_command_deltas = *msg; - - // Check if joint inputs is all zeros. Flag it if so to skip calculations/publication - bool all_zeros = true; - for (double delta : shared_variables_.joint_command_deltas.velocities) - { - all_zeros &= (delta == 0.0); - }; - shared_variables_.have_nonzero_joint_cmd = !all_zeros; - - if (shared_variables_.have_nonzero_joint_cmd) - { - shared_variables_.latest_nonzero_cmd_stamp = msg->header.stamp; - } - shared_variables_.unlock(); -} -} // namespace moveit_jog_arm diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp deleted file mode 100644 index 2b18f7e083..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.cpp +++ /dev/null @@ -1,101 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, PickNik LLC - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of PickNik LLC nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Dave Coleman - Desc: Stub test for the C++ interface to JogArm -*/ - -// C++ -#include - -// ROS -#include - -// Testing -#include - -// Main class -#include - -static const std::string LOGNAME = "jog_cpp_interface_test"; - -namespace moveit_jog_arm -{ -class TestJogCppInterface : public ::testing::Test -{ -public: - void SetUp() override - { - nh_.reset(new ros::NodeHandle("~")); - - // Load the planning scene monitor - planning_scene_monitor_ = std::make_shared("robot_description"); - planning_scene_monitor_->startSceneMonitor(); - planning_scene_monitor_->startStateMonitor(); - } - void TearDown() override - { - } - -protected: - std::unique_ptr nh_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; -}; // class TestJogCppInterface - -TEST_F(TestJogCppInterface, InitTest) -{ - moveit_jog_arm::JogCppInterface jog_cpp_interface(planning_scene_monitor_); - ros::Duration(1).sleep(); // Give the started thread some time to run -} - -// TODO(davetcoleman): due to many blocking checks for ROS messages, and -// an abundance of threads, unit tests are not currently feasible for the -// cpp interface. This should be addressed in future re-factors - -} // namespace moveit_jog_arm - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "jog_cpp_interface_test_test"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - int result = RUN_ALL_TESTS(); - - spinner.stop(); - ros::shutdown(); - return result; -} diff --git a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test b/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test deleted file mode 100644 index 428d8482e1..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/jog_cpp_interface_test.test +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test deleted file mode 100644 index 33669dc061..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_drift_dimensions_service_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test deleted file mode 100644 index 0ff461bb8c..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_halt_msg_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test deleted file mode 100644 index 4761fb2ffb..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_msg_reception_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test b/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test deleted file mode 100644 index 6a35a19e08..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/launch/jog_arm_vel_accel_limits_test.test +++ /dev/null @@ -1,15 +0,0 @@ - - - - - - - - - - - - - - - diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py b/moveit_experimental/moveit_jog_arm/test/python_tests/util.py deleted file mode 100644 index 0ee323b8c7..0000000000 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/util.py +++ /dev/null @@ -1,10 +0,0 @@ -import rospy - -def wait_for_jogger_initialization(service_name): - try: - rospy.wait_for_service(service_name, timeout=15) - except rospy.ROSException as exc: - rospy.logerr("The jogger service " + service_name + " is not available: " + str(exc)) - return False - - return True diff --git a/moveit_experimental/moveit_jog_arm/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt similarity index 72% rename from moveit_experimental/moveit_jog_arm/CMakeLists.txt rename to moveit_ros/moveit_servo/CMakeLists.txt index d8e01a7ec2..67830ea2d6 100644 --- a/moveit_experimental/moveit_jog_arm/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -1,16 +1,18 @@ cmake_minimum_required(VERSION 3.1.3) -project(moveit_jog_arm) +project(moveit_servo) set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) -set(LIBRARY_NAME jog_arm_cpp_api) +set(LIBRARY_NAME moveit_servo_cpp_api) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) set(CMAKE_BUILD_TYPE Release) endif() +find_package(Boost REQUIRED) + find_package(catkin REQUIRED COMPONENTS control_msgs geometry_msgs @@ -45,6 +47,7 @@ include_directories( include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${Boost_INCLUDE_DIR} ) ######################################### @@ -52,16 +55,17 @@ include_directories( ######################################### add_library(${LIBRARY_NAME} SHARED - src/collision_check_thread.cpp - src/jog_calcs.cpp - src/jog_cpp_interface.cpp - src/jog_interface_base.cpp + src/collision_check.cpp + src/servo_calcs.cpp + src/servo.cpp + src/joint_state_subscriber.cpp src/low_pass_filter.cpp ) add_dependencies(${LIBRARY_NAME} ${catkin_EXPORTED_TARGETS}) target_link_libraries(${LIBRARY_NAME} ${catkin_LIBRARIES} ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} ) # An example of using the C++ library @@ -72,6 +76,7 @@ add_dependencies(cpp_interface_example ${catkin_EXPORTED_TARGETS}) target_link_libraries(cpp_interface_example ${catkin_LIBRARIES} ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} ${LIBRARY_NAME} ) @@ -79,16 +84,16 @@ target_link_libraries(cpp_interface_example ## ROS message-based node ## ############################ -add_executable(jog_server - src/collision_check_thread.cpp - src/jog_calcs.cpp - src/jog_interface_base.cpp - src/jog_ros_interface.cpp - src/jog_server.cpp - src/low_pass_filter.cpp +add_executable(servo_server + src/servo_server.cpp +) +add_dependencies(servo_server ${catkin_EXPORTED_TARGETS}) +target_link_libraries(servo_server + ${LIBRARY_NAME} + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ${Boost_LIBRARIES} ) -add_dependencies(jog_server ${catkin_EXPORTED_TARGETS}) -target_link_libraries(jog_server ${catkin_LIBRARIES} ${Eigen_LIBRARIES}) ################################################ ## An example of converting joystick commands ## @@ -107,7 +112,7 @@ target_link_libraries(spacenav_to_twist ${catkin_LIBRARIES}) install( TARGETS ${LIBRARY_NAME} - jog_server + servo_server spacenav_to_twist ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} @@ -127,19 +132,20 @@ if(CATKIN_ENABLE_TESTING) find_package(ros_pytest REQUIRED) # Python integration tests - add_rostest(test/launch/jog_arm_halt_msg_test.test) - add_rostest(test/launch/jog_arm_msg_reception_test.test) - add_rostest(test/launch/jog_arm_vel_accel_limits_test.test) - add_rostest(test/launch/jog_arm_drift_dimensions_service_test.test) - - # jog_cpp_interface - add_rostest_gtest(jog_cpp_interface_test - test/jog_cpp_interface_test.test - test/jog_cpp_interface_test.cpp + add_rostest(test/launch/servo_halt_msg_test.test) + add_rostest(test/launch/servo_msg_reception_test.test) + add_rostest(test/launch/servo_vel_accel_limits_test.test) + add_rostest(test/launch/servo_drift_dimensions_service_test.test) + + # servo_cpp_interface + add_rostest_gtest(servo_cpp_interface_test + test/servo_cpp_interface_test.test + test/servo_cpp_interface_test.cpp ) - target_link_libraries(jog_cpp_interface_test + target_link_libraries(servo_cpp_interface_test ${LIBRARY_NAME} ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} ${Boost_LIBRARIES} ) endif() diff --git a/moveit_experimental/moveit_jog_arm/README.md b/moveit_ros/moveit_servo/README.md similarity index 82% rename from moveit_experimental/moveit_jog_arm/README.md rename to moveit_ros/moveit_servo/README.md index 93c7c036d9..e889244cb8 100644 --- a/moveit_experimental/moveit_jog_arm/README.md +++ b/moveit_ros/moveit_servo/README.md @@ -1,4 +1,4 @@ -## Jog Arm +## Moveit Servo #### Quick Start Guide for UR5 example @@ -30,14 +30,14 @@ stop_controllers: strictness: 2" ``` -Launch the jog node. This example uses commands from a SpaceNavigator joystick-like device: +Launch the servo node. This example uses commands from a SpaceNavigator joystick-like device: - roslaunch moveit_jog_arm spacenav_cpp.launch + roslaunch moveit_servo spacenav_cpp.launch If you dont have a SpaceNavigator, send commands like this: ```sh -rostopic pub -r 100 /jog_server/delta_jog_cmds geometry_msgs/TwistStamped "header: auto +rostopic pub -r 100 /servo_server/delta_twist_cmds geometry_msgs/TwistStamped "header: auto twist: linear: x: 0.0 @@ -53,6 +53,6 @@ If you see a warning about "close to singularity", try changing the direction of #### Running Tests -Run tests from the jog\_arm folder: +Run tests from the moveit\_servo folder: catkin run_tests --no-deps --this \ No newline at end of file diff --git a/moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml similarity index 93% rename from moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml rename to moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml index 2308e0c5a0..cbc6cfa1a4 100644 --- a/moveit_experimental/moveit_jog_arm/config/spacenav_via_teleop_tools.yaml +++ b/moveit_ros/moveit_servo/config/spacenav_via_teleop_tools.yaml @@ -2,7 +2,7 @@ teleop: cartesian_twist_command: type: topic message_type: geometry_msgs/TwistStamped - topic_name: jog_server/delta_jog_cmds + topic_name: servo_server/delta_twist_cmds axis_mappings: - axis: 0 diff --git a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml similarity index 71% rename from moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml rename to moveit_ros/moveit_servo/config/ur_simulated_config.yaml index 8a42b955dd..ed3917f236 100644 --- a/moveit_experimental/moveit_jog_arm/config/ur_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/ur_simulated_config.yaml @@ -1,5 +1,5 @@ ############################################### -# Modify all parameters related to jogging here +# Modify all parameters related to servoing here ############################################### use_gazebo: true # Whether the robot is started in a Gazebo simulation environment @@ -33,7 +33,7 @@ move_group_name: manipulator # Often 'manipulator' or 'arm' planning_frame: world # The MoveIt planning frame. Often 'base_link' or 'world' ## Stopping behaviour -incoming_command_timeout: 0.1 # Stop jogging if X seconds elapse without a new command +incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 4 @@ -44,14 +44,22 @@ hard_stop_singularity_threshold: 30 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: joint_states -status_topic: jog_server/status # Publish status to this topic +status_topic: servo_server/status # Publish status to this topic command_out_topic: joint_group_position_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? -collision_check_rate: 10 # [Hz] Collision-checking can easily bog down a CPU if done too often. +collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] scene_collision_proximity_threshold: 0.05 # Start decelerating when a scene collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h new file mode 100644 index 0000000000..6b9610d6ce --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_check.h @@ -0,0 +1,129 @@ +/******************************************************************************* + * Title : collision_check.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include + +namespace moveit_servo +{ +enum CollisionCheckType +{ + K_THRESHOLD_DISTANCE = 1, + K_STOP_DISTANCE = 2 +}; + +class CollisionCheck +{ +public: + /** \brief Constructor + * \param parameters: common settings of moveit_servo + * \param planning_scene_monitor: PSM should have scene monitor and state monitor + * already started when passed into this class + */ + CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber); + + /** \brief start and stop the Timer */ + void start(); + void stop(); + + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ + void setPaused(bool paused); + +private: + void run(const ros::TimerEvent& timer_event); + planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; + void worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg); + + ros::NodeHandle nh_; + + // Parameters from yaml + const ServoParameters& parameters_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Subscriber to joint states + const std::shared_ptr joint_state_subscriber_; + + // Robot state and collision matrix from planning scene + std::unique_ptr current_state_; + collision_detection::AllowedCollisionMatrix acm_; + + // Scale robot velocity according to collision proximity and user-defined thresholds. + // I scaled exponentially (cubic power) so velocity drops off quickly after the threshold. + // Proximity decreasing --> decelerate + CollisionCheckType collision_check_type_; + double velocity_scale_ = 1; + double self_collision_distance_ = 0; + double scene_collision_distance_ = 0; + bool collision_detected_ = false; + bool paused_ = false; + + // Variables for stop-distance-based collision checking + double current_collision_distance_ = 0; + double derivative_of_collision_distance_ = 0; + double prev_collision_distance_ = 0; + double est_time_to_collision_ = 0; + double safety_factor_ = 1000; + double worst_case_stop_time_ = std::numeric_limits::max(); + + const double self_velocity_scale_coefficient_; + const double scene_velocity_scale_coefficient_; + + // collision request + collision_detection::CollisionRequest collision_request_; + collision_detection::CollisionResult collision_result_; + + // ROS + ros::Timer timer_; + ros::Duration period_; + ros::Subscriber joint_state_sub_; + ros::Publisher collision_velocity_scale_pub_; + ros::Subscriber worst_case_stop_time_sub_; +}; +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h similarity index 69% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h rename to moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h index 7afe5029f5..8300134bab 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_ros_interface.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/joint_state_subscriber.h @@ -1,8 +1,8 @@ /******************************************************************************* - * Title : jog_ros_interface.h - * Project : moveit_jog_arm - * Created : 3/9/2017 - * Author : Brian O'Neil, Blake Anderson, Andy Zelenak + * Title : joint_state_subscriber.h + * Project : moveit_servo + * Created : 06/11/2020 + * Author : Tyler Weaver * * BSD 3-Clause License * @@ -36,25 +36,33 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -// Server node for arm jogging with MoveIt. - #pragma once -#include "jog_interface_base.h" +#include + +#include +#include + +#include -namespace moveit_jog_arm +namespace moveit_servo { -/** - * Class JogROSInterface - Instantiated in main(). Handles ROS subs & pubs and creates the worker threads. - */ -class JogROSInterface : protected JogInterfaceBase +class JointStateSubscriber { public: - JogROSInterface(); + /** \brief Constructor */ + JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name); + + /** \brief Get the latest joint state message */ + sensor_msgs::JointStateConstPtr getLatest() const; private: - // ROS subscriber callbacks - void deltaCartesianCmdCB(const geometry_msgs::TwistStampedConstPtr& msg); - void deltaJointCmdCB(const control_msgs::JointJogConstPtr& msg); + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + + ros::Subscriber joint_state_sub_; + + // Latest joint state, updated by ROS callback + mutable std::mutex joint_state_mutex_; + sensor_msgs::JointStateConstPtr latest_joint_state_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h similarity index 94% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h rename to moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h index 6d5d6ba2b0..9108626617 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/low_pass_filter.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/low_pass_filter.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : low_pass_filter.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Andy Zelenak * @@ -40,7 +40,7 @@ #include -namespace moveit_jog_arm +namespace moveit_servo { /** * Class LowPassFilter - Filter a signal to soften jerks. @@ -51,7 +51,7 @@ namespace moveit_jog_arm class LowPassFilter { public: - // Larger filter_coeff-> more smoothing of jog commands, but more lag. + // Larger filter_coeff-> more smoothing of servo commands, but more lag. // Rough plot, with cutoff frequency on the y-axis: // https://www.wolframalpha.com/input/?i=plot+arccot(c) explicit LowPassFilter(double low_pass_filter_coeff); @@ -66,4 +66,4 @@ class LowPassFilter const double scale_term_; const double feedback_term_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h similarity index 76% rename from moveit_experimental/moveit_jog_arm/src/jog_server.cpp rename to moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h index f258211f45..73fd73df8d 100644 --- a/moveit_experimental/moveit_jog_arm/src/jog_server.cpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/make_shared_from_pool.h @@ -1,4 +1,9 @@ /******************************************************************************* + * Title : make_shared_from_pool.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Tyler Weaver + * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -31,21 +36,21 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -/* Title : jog_server.cpp - * Project : moveit_jog_arm - * Created : 12/31/2018 - * Author : Andy Zelenak - */ - -#include +#pragma once -static const std::string LOGNAME = "jog_server"; +#include -int main(int argc, char** argv) +namespace moveit { - ros::init(argc, argv, LOGNAME); - - moveit_jog_arm::JogROSInterface ros_interface; - - return 0; +namespace util +{ +// Useful template for creating messages from a message pool +template +boost::shared_ptr make_shared_from_pool() +{ + using allocator_t = boost::fast_pool_allocator>; + return boost::allocate_shared(allocator_t()); } + +} // namespace util +} // namespace moveit diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h b/moveit_ros/moveit_servo/include/moveit_servo/servo.h similarity index 60% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h rename to moveit_ros/moveit_servo/include/moveit_servo/servo.h index a6d35e077a..6a3a29d4bd 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/jog_cpp_interface.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.h @@ -1,8 +1,8 @@ /******************************************************************************* - * Title : jog_cpp_interface.h - * Project : moveit_jog_arm - * Created : 11/20/2019 - * Author : Andy Zelenak + * Title : servo.h + * Project : moveit_servo + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * * BSD 3-Clause License * @@ -38,46 +38,34 @@ #pragma once -#include -#include "jog_interface_base.h" -#include +#include -namespace moveit_jog_arm +#include +#include +#include +#include + +namespace moveit_servo { /** -* Class JogCppInterface - This class should be instantiated in a new thread -* See cpp_interface_example.cpp -*/ -class JogCppInterface : public JogInterfaceBase + * Class Servo - Jacobian based robot control with collision avoidance. + */ +class Servo { public: - JogCppInterface(const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); - ~JogCppInterface(); + ~Servo(); - void startMainLoop(); + /** \brief start servo node */ + void start(); - void stopMainLoop(); + /** \brief stop servo node */ + void stop(); - /** \brief Pause or unpause processing jog commands while keeping the main loop alive */ + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ void setPaused(bool paused); - /** \brief Provide a Cartesian velocity command to the jogger. - * The units are determined by settings in the yaml file. - */ - void provideTwistStampedCommand(const geometry_msgs::TwistStamped& velocity_command); - - /** \brief Send joint position(s) commands */ - void provideJointCommand(const control_msgs::JointJog& joint_command); - - /** - * Returns the most recent JointState that the jogger has received. - * May eliminate the need to create your own joint_state subscriber. - * - * @return the most recent joints known to the jogger - */ - sensor_msgs::JointState getJointState(); - /** * Get the MoveIt planning link transform. * The transform from the MoveIt planning frame to robot_link_command_frame @@ -87,14 +75,29 @@ class JogCppInterface : public JogInterfaceBase */ bool getCommandFrameTransform(Eigen::Isometry3d& transform); - /** - * Get the status of the jogger. - * - * @return 0 for no warning. The meaning of nonzero values can be seen in status_codes.h - */ - StatusCode getJoggerStatus(); + /** \brief Get the parameters used by servo node. */ + const ServoParameters& getParameters() const; + + /** \brief Get the latest joint state. */ + sensor_msgs::JointStateConstPtr getLatestJointState() const; private: + bool readParameters(); + ros::NodeHandle nh_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Store the parameters that were read from ROS server + ServoParameters parameters_; + + std::shared_ptr joint_state_subscriber_; + std::unique_ptr servo_calcs_; + std::unique_ptr collision_checker_; }; -} // namespace moveit_jog_arm + +// ServoPtr using alias +using ServoPtr = std::shared_ptr; + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h new file mode 100644 index 0000000000..a8f676033a --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h @@ -0,0 +1,284 @@ +/******************************************************************************* + * Title : servo_calcs.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#pragma once + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// moveit_servo +#include +#include +#include +#include + +namespace moveit_servo +{ +class ServoCalcs +{ +public: + ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber); + + /** \brief Start and stop the timer where we do work and publish outputs */ + void start(); + void stop(); + + /** + * Get the MoveIt planning link transform. + * The transform from the MoveIt planning frame to robot_link_command_frame + * + * @param transform the transform that will be calculated + * @return true if a valid transform was available + */ + bool getCommandFrameTransform(Eigen::Isometry3d& transform); + + /** \brief Pause or unpause processing servo commands while keeping the timers alive */ + void setPaused(bool paused); + +private: + /** \brief Timer method */ + void run(const ros::TimerEvent& timer_event); + + /** \brief Do servoing calculations for Cartesian twist commands. */ + bool cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Do servoing calculations for direct commands to a joint. */ + bool jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Parse the incoming joint msg for the joints of our MoveGroup */ + bool updateJoints(); + + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ + Eigen::VectorXd scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const; + + /** \brief If incoming velocity commands are from a unitless joystick, scale them to physical units. + * Also, multiply by timestep to calculate a position change. + */ + Eigen::VectorXd scaleJointCommand(const control_msgs::JointJog& command) const; + + bool addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const; + + /** \brief Suddenly halt for a joint limit or other critical issue. + * Is handled differently for position vs. velocity control. + */ + void suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Scale the delta theta to match joint velocity/acceleration limits */ + void enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta); + + /** \brief Avoid overshooting joint limits */ + bool enforceSRDFPositionLimits(); + + /** \brief Possibly calculate a velocity scaling factor, due to proximity of + * singularity and direction of motion + */ + double velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse); + + /** + * Slow motion down if close to singularity or collision. + * @param delta_theta motion command, used in calculating new_joint_tray + * @param singularity_scale tells how close we are to a singularity + */ + void applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale); + + /** \brief Compose the outgoing JointTrajectory message */ + void composeJointTrajMessage(const sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const; + + /** \brief Smooth position commands with a lowpass filter */ + void lowPassFilterPositions(sensor_msgs::JointState& joint_state); + + /** \brief Set the filters to the specified values */ + void resetLowPassFilters(const sensor_msgs::JointState& joint_state); + + /** \brief Convert an incremental position command to joint velocity message */ + void calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta); + + /** \brief Convert joint deltas to an outgoing JointTrajectory command. + * This happens for joint commands and Cartesian commands. + */ + bool convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory); + + /** \brief Gazebo simulations have very strict message timestamp requirements. + * Satisfy Gazebo by stuffing multiple messages into one. + */ + void insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, int count) const; + + /** + * Remove the Jacobian row and the delta-x element of one Cartesian dimension, to take advantage of task redundancy + * + * @param matrix The Jacobian matrix. + * @param delta_x Vector of Cartesian delta commands, should be the same size as matrix.rows() + * @param row_to_remove Dimension that will be allowed to drift, e.g. row_to_remove = 2 allows z-translation drift. + */ + void removeDimension(Eigen::MatrixXd& matrix, Eigen::VectorXd& delta_x, unsigned int row_to_remove); + + /* \brief Callback for joint subsription */ + void jointStateCB(const sensor_msgs::JointStateConstPtr& msg); + + /* \brief Command callbacks */ + void twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg); + void jointCmdCB(const control_msgs::JointJogConstPtr& msg); + void collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg); + + /** + * Allow drift in certain dimensions. For example, may allow the wrist to rotate freely. + * This can help avoid singularities. + * + * @param request the service request + * @param response the service response + * @return true if the adjustment was made + */ + bool changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res); + + /** \brief Service callback for changing servoing dimensions (e.g. ignore rotation about X) */ + bool changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res); + + ros::NodeHandle nh_; + + // Parameters from yaml + const ServoParameters& parameters_; + + // Pointer to the collision environment + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + + // Subscriber to the latest joint states + const std::shared_ptr joint_state_subscriber_; + + // Track the number of cycles during which motion has not occurred. + // Will avoid re-publishing zero velocities endlessly. + int zero_velocity_count_ = 0; + + // Flag for staying inactive while there are no incoming commands + bool wait_for_servo_commands_ = true; + + // Flag saying if the filters were updated during the timer callback + bool updated_filters_ = false; + + // Nonzero status flags + bool have_nonzero_twist_stamped_ = false; + bool have_nonzero_joint_command_ = false; + bool have_nonzero_command_ = false; + + // Incoming command messages + geometry_msgs::TwistStamped twist_stamped_cmd_; + control_msgs::JointJog joint_servo_cmd_; + + const moveit::core::JointModelGroup* joint_model_group_; + + moveit::core::RobotStatePtr kinematic_state_; + + // incoming_joint_state_ is the incoming message. It may contain passive joints or other joints we don't care about. + // (mutex protected below) + // internal_joint_state_ is used in servo calculations. It shouldn't be relied on to be accurate. + // original_joint_state_ is the same as incoming_joint_state_ except it only contains the joints the servo node acts + // on. + sensor_msgs::JointState internal_joint_state_, original_joint_state_; + std::map joint_state_name_map_; + + std::vector position_filters_; + + trajectory_msgs::JointTrajectoryConstPtr last_sent_command_; + + // ROS + ros::Timer timer_; + ros::Duration period_; + ros::Subscriber joint_state_sub_; + ros::Subscriber twist_stamped_sub_; + ros::Subscriber joint_cmd_sub_; + ros::Subscriber collision_velocity_scale_sub_; + ros::Publisher status_pub_; + ros::Publisher worst_case_stop_time_pub_; + ros::Publisher outgoing_cmd_pub_; + ros::ServiceServer drift_dimensions_server_; + ros::ServiceServer control_dimensions_server_; + + // Status + StatusCode status_ = StatusCode::NO_WARNING; + bool stop_requested_ = false; + bool paused_ = false; + bool twist_command_is_stale_ = false; + bool joint_command_is_stale_ = false; + bool ok_to_publish_ = false; + double collision_velocity_scale_ = 1.0; + + // Use ArrayXd type to enable more coefficient-wise operations + Eigen::ArrayXd delta_theta_; + Eigen::ArrayXd prev_joint_velocity_; + + const int gazebo_redundant_message_count_ = 30; + + uint num_joints_; + + // True -> allow drift in this dimension. In the command frame. [x, y, z, roll, pitch, yaw] + std::array drift_dimensions_ = { { false, false, false, false, false, false } }; + + // The dimesions to control. In the command frame. [x, y, z, roll, pitch, yaw] + std::array control_dimensions_ = { { true, true, true, true, true, true } }; + + // Amount we sleep when waiting + ros::Rate default_sleep_rate_ = 100; + + // latest_state_mutex_ is used to protect the state below it + mutable std::mutex latest_state_mutex_; + Eigen::Isometry3d tf_moveit_to_robot_cmd_frame_; + geometry_msgs::TwistStampedConstPtr latest_twist_stamped_; + control_msgs::JointJogConstPtr latest_joint_cmd_; + ros::Time latest_twist_command_stamp_ = ros::Time(0.); + ros::Time latest_joint_command_stamp_ = ros::Time(0.); + bool latest_nonzero_twist_stamped_ = false; + bool latest_nonzero_joint_cmd_ = false; +}; +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h new file mode 100644 index 0000000000..2fcdea7c5c --- /dev/null +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo_parameters.h @@ -0,0 +1,83 @@ +/******************************************************************************* + * Title : servo_parameters.h + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + * + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +#pragma once + +namespace moveit_servo +{ +// Size of queues used in ros pub/sub/service +constexpr size_t ROS_QUEUE_SIZE = 2; + +// ROS params to be read. See the yaml file in /config for a description of each. +struct ServoParameters +{ + std::string move_group_name; + std::string joint_topic; + std::string cartesian_command_in_topic; + std::string robot_link_command_frame; + std::string command_out_topic; + std::string planning_frame; + std::string status_topic; + std::string joint_command_in_topic; + std::string command_in_type; + std::string command_out_type; + double linear_scale; + double rotational_scale; + double joint_scale; + double lower_singularity_threshold; + double hard_stop_singularity_threshold; + double low_pass_filter_coeff; + double publish_period; + double incoming_command_timeout; + double joint_limit_margin; + int num_outgoing_halt_msgs_to_publish; + bool use_gazebo; + bool publish_joint_positions; + bool publish_joint_velocities; + bool publish_joint_accelerations; + // Collision checking + bool check_collisions; + std::string collision_check_type; + double collision_check_rate; + double scene_collision_proximity_threshold; + double self_collision_proximity_threshold; + double collision_distance_safety_factor; + double min_allowable_collision_distance; +}; + +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h similarity index 73% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h rename to moveit_ros/moveit_servo/include/moveit_servo/status_codes.h index 85660cd50b..f6e9ddcb54 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/status_codes.h +++ b/moveit_ros/moveit_servo/include/moveit_servo/status_codes.h @@ -1,6 +1,6 @@ /******************************************************************************* * Title : status_codes.h - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 2/25/2019 * Author : Andy Zelenak * @@ -41,10 +41,11 @@ #include #include -namespace moveit_jog_arm +namespace moveit_servo { enum StatusCode : int8_t { + INVALID = -1, NO_WARNING = 0, DECELERATE_FOR_SINGULARITY = 1, HALT_FOR_SINGULARITY = 2, @@ -53,11 +54,12 @@ enum StatusCode : int8_t JOINT_BOUND = 5 }; -const std::unordered_map - JOG_ARM_STATUS_CODE_MAP({ { NO_WARNING, "No warnings" }, - { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, - { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, - { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, - { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, - { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); -} // namespace moveit_jog_arm +const std::unordered_map + SERVO_STATUS_CODE_MAP({ { INVALID, "Invalid" }, + { NO_WARNING, "No warnings" }, + { DECELERATE_FOR_SINGULARITY, "Close to a singularity, decelerating" }, + { HALT_FOR_SINGULARITY, "Very close to a singularity, emergency stop" }, + { DECELERATE_FOR_COLLISION, "Close to a collision, decelerating" }, + { HALT_FOR_COLLISION, "Collision detected, emergency stop" }, + { JOINT_BOUND, "Close to a joint bound (position or velocity), halting" } }); +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/launch/cpp_interface_example.launch b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch new file mode 100644 index 0000000000..c2ac9b8748 --- /dev/null +++ b/moveit_ros/moveit_servo/launch/cpp_interface_example.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch similarity index 63% rename from moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch rename to moveit_ros/moveit_servo/launch/spacenav_cpp.launch index 9aa9f6b0c0..41d81862db 100644 --- a/moveit_experimental/moveit_jog_arm/launch/spacenav_cpp.launch +++ b/moveit_ros/moveit_servo/launch/spacenav_cpp.launch @@ -1,5 +1,5 @@ - - - - - + + + + - + diff --git a/moveit_experimental/moveit_jog_arm/package.xml b/moveit_ros/moveit_servo/package.xml similarity index 83% rename from moveit_experimental/moveit_jog_arm/package.xml rename to moveit_ros/moveit_servo/package.xml index 931be71bac..0e6b0f433b 100644 --- a/moveit_experimental/moveit_jog_arm/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -1,9 +1,9 @@ - moveit_jog_arm - 1.0.1 + moveit_servo + 1.1.0 - Provides real-time manipulator Cartesian jogging. + Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak @@ -16,6 +16,7 @@ Andy Zelenak Blake Anderson Alexander Rössler + Tyler Weaver catkin diff --git a/moveit_ros/moveit_servo/src/collision_check.cpp b/moveit_ros/moveit_servo/src/collision_check.cpp new file mode 100644 index 0000000000..14a7d51426 --- /dev/null +++ b/moveit_ros/moveit_servo/src/collision_check.cpp @@ -0,0 +1,222 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Title : collision_check.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include + +#include +#include + +static const char LOGNAME[] = "collision_check"; +static const double MIN_RECOMMENDED_COLLISION_RATE = 10; +constexpr double EPSILON = 1e-6; // For very small numeric comparisons +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops + +namespace moveit_servo +{ +// Constructor for the class that handles collision checking +CollisionCheck::CollisionCheck(ros::NodeHandle& nh, const moveit_servo::ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber) + : nh_(nh) + , parameters_(parameters) + , planning_scene_monitor_(planning_scene_monitor) + , joint_state_subscriber_(joint_state_subscriber) + , self_velocity_scale_coefficient_(-log(0.001) / parameters.self_collision_proximity_threshold) + , scene_velocity_scale_coefficient_(-log(0.001) / parameters.scene_collision_proximity_threshold) + , period_(1. / parameters_.collision_check_rate) +{ + // Init collision request + collision_request_.group_name = parameters_.move_group_name; + collision_request_.distance = true; // enable distance-based collision checking + + if (parameters_.collision_check_rate < MIN_RECOMMENDED_COLLISION_RATE) + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Collision check rate is low, increase it in yaml file if CPU allows"); + + collision_check_type_ = + (parameters_.collision_check_type == "threshold_distance" ? K_THRESHOLD_DISTANCE : K_STOP_DISTANCE); + safety_factor_ = parameters_.collision_distance_safety_factor; + + // Internal namespace + ros::NodeHandle internal_nh("~internal"); + collision_velocity_scale_pub_ = internal_nh.advertise("collision_velocity_scale", ROS_QUEUE_SIZE); + worst_case_stop_time_sub_ = + internal_nh.subscribe("worst_case_stop_time", ROS_QUEUE_SIZE, &CollisionCheck::worstCaseStopTimeCB, this); + + current_state_ = std::make_unique(getLockedPlanningSceneRO()->getCurrentState()); + acm_ = getLockedPlanningSceneRO()->getAllowedCollisionMatrix(); +} + +planning_scene_monitor::LockedPlanningSceneRO CollisionCheck::getLockedPlanningSceneRO() const +{ + return planning_scene_monitor::LockedPlanningSceneRO(planning_scene_monitor_); +} + +void CollisionCheck::start() +{ + timer_ = nh_.createTimer(period_, &CollisionCheck::run, this); +} + +void CollisionCheck::stop() +{ + timer_.stop(); +} + +void CollisionCheck::run(const ros::TimerEvent& timer_event) +{ + // Log warning when the last loop duration was longer than the period + if (timer_event.profile.last_duration.toSec() > period_.toSec()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + + if (paused_) + { + return; + } + + // Copy the latest joint state + auto latest_joint_state = joint_state_subscriber_->getLatest(); + for (std::size_t i = 0; i < latest_joint_state->position.size(); ++i) + current_state_->setJointPositions(latest_joint_state->name[i], &latest_joint_state->position[i]); + + current_state_->updateCollisionBodyTransforms(); + collision_detected_ = false; + + // Do a thread-safe distance-based collision detection + { // Lock PlanningScene + auto scene_ro = getLockedPlanningSceneRO(); + + collision_result_.clear(); + scene_ro->getCollisionWorld()->checkRobotCollision(collision_request_, collision_result_, + *scene_ro->getCollisionRobot(), *current_state_, acm_); + + scene_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + + collision_result_.clear(); + scene_ro->getCollisionRobotUnpadded()->checkSelfCollision(collision_request_, collision_result_, *current_state_, + acm_); + } // Unlock PlanningScene + + self_collision_distance_ = collision_result_.distance; + collision_detected_ |= collision_result_.collision; + + velocity_scale_ = 1; + // If we're definitely in collision, stop immediately + if (collision_detected_) + { + velocity_scale_ = 0; + } + // If threshold distances were specified + else if (collision_check_type_ == K_THRESHOLD_DISTANCE) + { + // If we are far from a collision, velocity_scale should be 1. + // If we are very close to a collision, velocity_scale should be ~zero. + // When scene_collision_proximity_threshold is breached, start decelerating exponentially. + if (scene_collision_distance_ < parameters_.scene_collision_proximity_threshold) + { + // velocity_scale = e ^ k * (collision_distance - threshold) + // k = - ln(0.001) / collision_proximity_threshold + // velocity_scale should equal one when collision_distance is at collision_proximity_threshold. + // velocity_scale should equal 0.001 when collision_distance is at zero. + velocity_scale_ = + std::min(velocity_scale_, exp(scene_velocity_scale_coefficient_ * + (scene_collision_distance_ - parameters_.scene_collision_proximity_threshold))); + } + + if (self_collision_distance_ < parameters_.self_collision_proximity_threshold) + { + velocity_scale_ = + std::min(velocity_scale_, exp(self_velocity_scale_coefficient_ * + (self_collision_distance_ - parameters_.self_collision_proximity_threshold))); + } + } + // Else, we stop based on worst-case stopping distance + else + { + // Retrieve the worst-case time to stop, based on current joint velocities + + // Calculate rate of change of distance to nearest collision + current_collision_distance_ = std::min(scene_collision_distance_, self_collision_distance_); + derivative_of_collision_distance_ = (current_collision_distance_ - prev_collision_distance_) / period_.toSec(); + + if (current_collision_distance_ < parameters_.min_allowable_collision_distance && + derivative_of_collision_distance_ <= 0) + { + velocity_scale_ = 0; + } + // Only bother doing calculations if we are moving toward the nearest collision + else if (derivative_of_collision_distance_ < -EPSILON) + { + // At the rate the collision distance is decreasing, how long until we collide? + est_time_to_collision_ = fabs(current_collision_distance_ / derivative_of_collision_distance_); + + // halt if we can't stop fast enough (including the safety factor) + if (est_time_to_collision_ < (safety_factor_ * worst_case_stop_time_)) + { + velocity_scale_ = 0; + } + } + + // Update for the next iteration + prev_collision_distance_ = current_collision_distance_; + } + + // publish message + { + auto msg = moveit::util::make_shared_from_pool(); + msg->data = velocity_scale_; + collision_velocity_scale_pub_.publish(msg); + } +} + +void CollisionCheck::worstCaseStopTimeCB(const std_msgs::Float64ConstPtr& msg) +{ + worst_case_stop_time_ = msg->data; +} + +void CollisionCheck::setPaused(bool paused) +{ + paused_ = paused; +} + +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp similarity index 55% rename from moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp rename to moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp index 987bf449cb..6db053e4be 100644 --- a/moveit_experimental/moveit_jog_arm/src/cpp_interface_example/cpp_interface_example.cpp +++ b/moveit_ros/moveit_servo/src/cpp_interface_example/cpp_interface_example.cpp @@ -1,6 +1,6 @@ /******************************************************************************* * Title : cpp_interface_example.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 11/20/2019 * Author : Andy Zelenak * @@ -36,19 +36,48 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#include "moveit_jog_arm/jog_cpp_interface.h" -#include "moveit_jog_arm/status_codes.h" +#include + +#include +#include +#include static const std::string LOGNAME = "cpp_interface_example"; +// Class for monitoring status of moveit_servo +class StatusMonitor +{ +public: + StatusMonitor(ros::NodeHandle& nh, const std::string& topic) + { + sub_ = nh.subscribe(topic, 1, &StatusMonitor::statusCB, this); + } + +private: + void statusCB(const std_msgs::Int8ConstPtr& msg) + { + moveit_servo::StatusCode latest_status = static_cast(msg->data); + if (latest_status != status_) + { + status_ = latest_status; + const auto& status_str = moveit_servo::SERVO_STATUS_CODE_MAP.at(status_); + ROS_INFO_STREAM_NAMED(LOGNAME, "Servo status: " << status_str); + } + } + moveit_servo::StatusCode status_ = moveit_servo::StatusCode::INVALID; + ros::Subscriber sub_; +}; + /** - * Instantiate the C++ jogging interface. + * Instantiate the C++ servo node interface. * Send some Cartesian commands, then some joint commands. - * Then retrieve the current joint state from the jogger. */ int main(int argc, char** argv) { ros::init(argc, argv, LOGNAME); + ros::NodeHandle nh; + ros::AsyncSpinner spinner(8); + spinner.start(); // Load the planning scene monitor planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor; @@ -66,15 +95,17 @@ int main(int argc, char** argv) false /* skip octomap monitor */); planning_scene_monitor->startStateMonitor(); - // Run the jogging C++ interface in a new thread to ensure a constant outgoing message rate. - moveit_jog_arm::JogCppInterface jog_interface(planning_scene_monitor); - std::thread jogging_thread([&]() { jog_interface.startMainLoop(); }); + // Run the servo node C++ interface in a new timer to ensure a constant outgoing message rate. + moveit_servo::Servo servo(nh, planning_scene_monitor); + servo.start(); + + // Subscribe to servo status (and log it when it changes) + StatusMonitor status_monitor(nh, servo.getParameters().status_topic); - // Make a Cartesian velocity message - geometry_msgs::TwistStamped velocity_msg; - velocity_msg.header.frame_id = "base_link"; - velocity_msg.twist.linear.y = 0.01; - velocity_msg.twist.linear.z = -0.01; + // Create publishers to send servo commands + auto twist_stamped_pub = + nh.advertise(servo.getParameters().cartesian_command_in_topic, 1); + auto joint_servo_pub = nh.advertise(servo.getParameters().joint_command_in_topic, 1); ros::Rate cmd_rate(100); uint num_commands = 0; @@ -82,42 +113,43 @@ int main(int argc, char** argv) // Send a few Cartesian velocity commands while (ros::ok() && num_commands < 200) { - ++num_commands; - velocity_msg.header.stamp = ros::Time::now(); - jog_interface.provideTwistStampedCommand(velocity_msg); + // Make a Cartesian velocity message + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not copied we should not modify it after we send it. + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "base_link"; + msg->twist.linear.y = 0.01; + msg->twist.linear.z = -0.01; + + // Send the message + twist_stamped_pub.publish(msg); cmd_rate.sleep(); + ++num_commands; } - // Leave plenty of time for the jogger to halt its previous motion. + // Leave plenty of time for the servo node to halt its previous motion. // For a faster response, adjust the incoming_command_timeout yaml parameter ros::Duration(2).sleep(); - // Make a joint command - control_msgs::JointJog base_joint_command; - base_joint_command.joint_names.push_back("elbow_joint"); - base_joint_command.velocities.push_back(0.2); - base_joint_command.header.stamp = ros::Time::now(); - // Send a few joint commands num_commands = 0; while (ros::ok() && num_commands < 200) { - ++num_commands; - base_joint_command.header.stamp = ros::Time::now(); - jog_interface.provideJointCommand(base_joint_command); + // Make a joint command + // Messages are sent to servo node as boost::shared_ptr to enable zero-copy message_passing. + // Because this message is not copied we should not modify it after we send it. + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->joint_names.push_back("elbow_joint"); + msg->velocities.push_back(0.2); + + // Send the message + joint_servo_pub.publish(msg); cmd_rate.sleep(); + ++num_commands; } - // Retrieve the current joint state from the jogger - sensor_msgs::JointState current_joint_state = jog_interface.getJointState(); - ROS_INFO_STREAM_NAMED(LOGNAME, "Current joint state:"); - ROS_INFO_STREAM_NAMED(LOGNAME, current_joint_state); - - // Retrieve the current status of the jogger - moveit_jog_arm::StatusCode status = jog_interface.getJoggerStatus(); - ROS_INFO_STREAM_NAMED(LOGNAME, "Jogger status:\n" << status); - - jog_interface.stopMainLoop(); - jogging_thread.join(); + servo.stop(); return 0; } diff --git a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp similarity index 58% rename from moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h rename to moveit_ros/moveit_servo/src/joint_state_subscriber.cpp index 8fbf5f2d50..2ed780b650 100644 --- a/moveit_experimental/moveit_jog_arm/include/moveit_jog_arm/collision_check_thread.h +++ b/moveit_ros/moveit_servo/src/joint_state_subscriber.cpp @@ -1,9 +1,4 @@ /******************************************************************************* - * Title : collision_check_thread.h - * Project : moveit_jog_arm - * Created : 1/11/2019 - * Author : Brian O'Neil, Andy Zelenak, Blake Anderson - * * BSD 3-Clause License * * Copyright (c) 2019, Los Alamos National Security, LLC @@ -36,36 +31,40 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *******************************************************************************/ -#pragma once +/* Title : joint_state_subscriber.cpp + * Project : moveit_servo + * Created : 06/11/2020 + * Author : Tyler Weaver + */ -#include -#include "jog_arm_data.h" -#include "low_pass_filter.h" -#include -#include +#include -namespace moveit_jog_arm +namespace moveit_servo { -class CollisionCheckThread +constexpr char LOGNAME[] = "joint_state_subscriber"; + +// Constructor for the class that handles collision checking +JointStateSubscriber::JointStateSubscriber(ros::NodeHandle& nh, const std::string& joint_state_topic_name) { -public: - /** \brief Constructor - * \param parameters: common settings of jog_arm - * \param planning_scene_monitor: PSM should have scene monitor and state monitor - * already started when passed into this class - */ - CollisionCheckThread(const moveit_jog_arm::JogArmParameters& parameters, - const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor); + // subscribe to joints + joint_state_sub_ = nh.subscribe(joint_state_topic_name, ROS_QUEUE_SIZE, &JointStateSubscriber::jointStateCB, this); - // Get thread-safe read-only lock of planning scene - planning_scene_monitor::LockedPlanningSceneRO getLockedPlanningSceneRO() const; + // Wait for initial messages + ROS_INFO_NAMED(LOGNAME, "Waiting for first joint msg."); + ros::topic::waitForMessage(joint_state_topic_name); + ROS_INFO_NAMED(LOGNAME, "Received first joint msg."); +} - void startMainLoop(moveit_jog_arm::JogArmShared& shared_variables); +sensor_msgs::JointStateConstPtr JointStateSubscriber::getLatest() const +{ + const std::lock_guard lock(joint_state_mutex_); + return latest_joint_state_; +} -private: - const moveit_jog_arm::JogArmParameters parameters_; +void JointStateSubscriber::jointStateCB(const sensor_msgs::JointStateConstPtr& msg) +{ + const std::lock_guard lock(joint_state_mutex_); + latest_joint_state_ = msg; +} - // Pointer to the collision environment - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; -}; -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp b/moveit_ros/moveit_servo/src/low_pass_filter.cpp similarity index 94% rename from moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp rename to moveit_ros/moveit_servo/src/low_pass_filter.cpp index 4c74829bbc..7013b3b4c9 100644 --- a/moveit_experimental/moveit_jog_arm/src/low_pass_filter.cpp +++ b/moveit_ros/moveit_servo/src/low_pass_filter.cpp @@ -32,17 +32,17 @@ *******************************************************************************/ /* Title : low_pass_filter.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Andy Zelenak */ -#include +#include #include #include #include -namespace moveit_jog_arm +namespace moveit_servo { namespace { @@ -57,7 +57,7 @@ LowPassFilter::LowPassFilter(double low_pass_filter_coeff) , feedback_term_(1. - low_pass_filter_coeff) { // guarantee this doesn't change because the logic below depends on this length implicity - static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_jog_arm::LowPassFilter::FILTER_LENGTH should be 2"); + static_assert(LowPassFilter::FILTER_LENGTH == 2, "moveit_servo::LowPassFilter::FILTER_LENGTH should be 2"); ROS_ASSERT_MSG(!std::isinf(feedback_term_), "%s: outputs from filter will be inf because feedback term is inf", LOGNAME); @@ -98,4 +98,4 @@ double LowPassFilter::filter(double new_measurement) return new_filtered_measurement; } -} // namespace moveit_jog_arm +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp new file mode 100644 index 0000000000..d8db2630df --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -0,0 +1,321 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Title : servo.cpp + * Project : moveit_servo + * Created : 3/9/2017 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include + +#include + +static const std::string LOGNAME = "servo_node"; + +namespace moveit_servo +{ +Servo::Servo(ros::NodeHandle& nh, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) + : nh_(nh), planning_scene_monitor_(planning_scene_monitor) +{ + // Read ROS parameters, typically from YAML file + if (!readParameters()) + exit(EXIT_FAILURE); + + joint_state_subscriber_ = std::make_shared(nh_, parameters_.joint_topic); + + servo_calcs_ = std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); + + collision_checker_ = + std::make_unique(nh_, parameters_, planning_scene_monitor_, joint_state_subscriber_); +} + +// Read ROS parameters, typically from YAML file +bool Servo::readParameters() +{ + std::size_t error = 0; + + // Specified in the launch file. All other parameters will be read from this namespace. + std::string parameter_ns; + ros::param::get("~parameter_ns", parameter_ns); + if (parameter_ns.empty()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "A namespace must be specified in the launch file, like:"); + ROS_ERROR_STREAM_NAMED(LOGNAME, ""); + return false; + } + + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_period", parameters_.publish_period); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_rate", parameters_.collision_check_rate); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/num_outgoing_halt_msgs_to_publish", + parameters_.num_outgoing_halt_msgs_to_publish); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/linear", parameters_.linear_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/rotational", parameters_.rotational_scale); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/scale/joint", parameters_.joint_scale); + error += + !rosparam_shortcuts::get("", nh_, parameter_ns + "/low_pass_filter_coeff", parameters_.low_pass_filter_coeff); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_topic", parameters_.joint_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_in_type", parameters_.command_in_type); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/cartesian_command_in_topic", + parameters_.cartesian_command_in_topic); + error += + !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_command_in_topic", parameters_.joint_command_in_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/robot_link_command_frame", + parameters_.robot_link_command_frame); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/incoming_command_timeout", + parameters_.incoming_command_timeout); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/lower_singularity_threshold", + parameters_.lower_singularity_threshold); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/hard_stop_singularity_threshold", + parameters_.hard_stop_singularity_threshold); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/move_group_name", parameters_.move_group_name); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/planning_frame", parameters_.planning_frame); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/use_gazebo", parameters_.use_gazebo); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/joint_limit_margin", parameters_.joint_limit_margin); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_topic", parameters_.command_out_topic); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/command_out_type", parameters_.command_out_type); + error += + !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_positions", parameters_.publish_joint_positions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_velocities", + parameters_.publish_joint_velocities); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/publish_joint_accelerations", + parameters_.publish_joint_accelerations); + + // Parameters for collision checking + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/check_collisions", parameters_.check_collisions); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_check_type", parameters_.collision_check_type); + bool have_self_collision_proximity_threshold = rosparam_shortcuts::get( + "", nh_, parameter_ns + "/self_collision_proximity_threshold", parameters_.self_collision_proximity_threshold); + bool have_scene_collision_proximity_threshold = rosparam_shortcuts::get( + "", nh_, parameter_ns + "/scene_collision_proximity_threshold", parameters_.scene_collision_proximity_threshold); + double collision_proximity_threshold; + // 'collision_proximity_threshold' parameter was removed, replaced with separate self- and scene-collision proximity + // thresholds + // TODO(JStech): remove this deprecation warning in ROS Noetic; simplify error case handling + if (nh_.hasParam(parameter_ns + "/collision_proximity_threshold") && + rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_proximity_threshold", collision_proximity_threshold)) + { + ROS_WARN_NAMED(LOGNAME, "'collision_proximity_threshold' parameter is deprecated, and has been replaced by separate" + "'self_collision_proximity_threshold' and 'scene_collision_proximity_threshold' " + "parameters. Please update the servoing yaml file."); + if (!have_self_collision_proximity_threshold) + { + parameters_.self_collision_proximity_threshold = collision_proximity_threshold; + have_self_collision_proximity_threshold = true; + } + if (!have_scene_collision_proximity_threshold) + { + parameters_.scene_collision_proximity_threshold = collision_proximity_threshold; + have_scene_collision_proximity_threshold = true; + } + } + error += !have_self_collision_proximity_threshold; + error += !have_scene_collision_proximity_threshold; + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/collision_distance_safety_factor", + parameters_.collision_distance_safety_factor); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/min_allowable_collision_distance", + parameters_.min_allowable_collision_distance); + + // This parameter name was changed recently. + // Try retrieving from the correct name. If it fails, then try the deprecated name. + // TODO(andyz): remove this deprecation warning in ROS Noetic + if (!rosparam_shortcuts::get("", nh_, parameter_ns + "/status_topic", parameters_.status_topic)) + { + ROS_WARN_NAMED(LOGNAME, "'status_topic' parameter is missing. Recently renamed from 'warning_topic'. Please update " + "the servoing yaml file."); + error += !rosparam_shortcuts::get("", nh_, parameter_ns + "/warning_topic", parameters_.status_topic); + } + + rosparam_shortcuts::shutdownIfError(parameter_ns, error); + + // Input checking + if (parameters_.publish_period <= 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'publish_period' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.num_outgoing_halt_msgs_to_publish < 0) + { + ROS_WARN_NAMED(LOGNAME, + "Parameter 'num_outgoing_halt_msgs_to_publish' should be greater than zero. Check yaml file."); + return false; + } + if (parameters_.hard_stop_singularity_threshold < parameters_.lower_singularity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'hard_stop_singularity_threshold' " + "should be greater than 'lower_singularity_threshold.' " + "Check yaml file."); + return false; + } + if ((parameters_.hard_stop_singularity_threshold < 0.) || (parameters_.lower_singularity_threshold < 0.)) + { + ROS_WARN_NAMED(LOGNAME, "Parameters 'hard_stop_singularity_threshold' " + "and 'lower_singularity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.low_pass_filter_coeff < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'low_pass_filter_coeff' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.joint_limit_margin < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'joint_limit_margin' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.command_in_type != "unitless" && parameters_.command_in_type != "speed_units") + { + ROS_WARN_NAMED(LOGNAME, "command_in_type should be 'unitless' or " + "'speed_units'. Check yaml file."); + return false; + } + if (parameters_.command_out_type != "trajectory_msgs/JointTrajectory" && + parameters_.command_out_type != "std_msgs/Float64MultiArray") + { + ROS_WARN_NAMED(LOGNAME, "Parameter command_out_type should be " + "'trajectory_msgs/JointTrajectory' or " + "'std_msgs/Float64MultiArray'. Check yaml file."); + return false; + } + if (!parameters_.publish_joint_positions && !parameters_.publish_joint_velocities && + !parameters_.publish_joint_accelerations) + { + ROS_WARN_NAMED(LOGNAME, "At least one of publish_joint_positions / " + "publish_joint_velocities / " + "publish_joint_accelerations must be true. Check " + "yaml file."); + return false; + } + if ((parameters_.command_out_type == "std_msgs/Float64MultiArray") && parameters_.publish_joint_positions && + parameters_.publish_joint_velocities) + { + ROS_WARN_NAMED(LOGNAME, "When publishing a std_msgs/Float64MultiArray, " + "you must select positions OR velocities."); + return false; + } + // Collision checking + if (parameters_.collision_check_type != "threshold_distance" && parameters_.collision_check_type != "stop_distance") + { + ROS_WARN_NAMED(LOGNAME, "collision_check_type must be 'threshold_distance' or 'stop_distance'"); + return false; + } + if (parameters_.self_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.scene_collision_proximity_threshold < 0.) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'scene_collision_proximity_threshold' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.scene_collision_proximity_threshold < parameters_.self_collision_proximity_threshold) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'self_collision_proximity_threshold' should probably be less " + "than or equal to 'scene_collision_proximity_threshold'. Check yaml file."); + } + if (parameters_.collision_check_rate < 0) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_check_rate' should be " + "greater than zero. Check yaml file."); + return false; + } + if (parameters_.collision_distance_safety_factor < 1) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'collision_distance_safety_factor' should be " + "greater than or equal to 1. Check yaml file."); + return false; + } + if (parameters_.min_allowable_collision_distance < 0) + { + ROS_WARN_NAMED(LOGNAME, "Parameter 'min_allowable_collision_distance' should be " + "greater than zero. Check yaml file."); + return false; + } + + return true; +} + +void Servo::start() +{ + setPaused(false); + + // Crunch the numbers in this timer + servo_calcs_->start(); + + // Check collisions in this timer + if (parameters_.check_collisions) + collision_checker_->start(); +} + +void Servo::stop() +{ + servo_calcs_->stop(); + collision_checker_->stop(); +} + +Servo::~Servo() +{ + stop(); +} + +void Servo::setPaused(bool paused) +{ + servo_calcs_->setPaused(paused); + collision_checker_->setPaused(paused); +} + +bool Servo::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + return servo_calcs_->getCommandFrameTransform(transform); +} + +const ServoParameters& Servo::getParameters() const +{ + return parameters_; +} + +sensor_msgs::JointStateConstPtr Servo::getLatestJointState() const +{ + return joint_state_subscriber_->getLatest(); +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_calcs.cpp b/moveit_ros/moveit_servo/src/servo_calcs.cpp new file mode 100644 index 0000000000..2753d22ae0 --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo_calcs.cpp @@ -0,0 +1,1054 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Title : servo_calcs.cpp + * Project : moveit_servo + * Created : 1/11/2019 + * Author : Brian O'Neil, Andy Zelenak, Blake Anderson + */ + +#include +#include + +#include +#include + +static const std::string LOGNAME = "servo_calcs"; +constexpr size_t ROS_LOG_THROTTLE_PERIOD = 30; // Seconds to throttle logs inside loops + +namespace moveit_servo +{ +namespace +{ +// Helper function for detecting zeroed message +bool isNonZero(const geometry_msgs::TwistStamped& msg) +{ + return msg.twist.linear.x != 0.0 || msg.twist.linear.y != 0.0 || msg.twist.linear.z != 0.0 || + msg.twist.angular.x != 0.0 || msg.twist.angular.y != 0.0 || msg.twist.angular.z != 0.0; +} + +// Helper function for detecting zeroed message +bool isNonZero(const control_msgs::JointJog& msg) +{ + bool all_zeros = true; + for (double delta : msg.velocities) + { + all_zeros &= (delta == 0.0); + }; + return !all_zeros; +} +} // namespace + +// Constructor for the class that handles servoing calculations +ServoCalcs::ServoCalcs(ros::NodeHandle& nh, const ServoParameters& parameters, + const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, + const std::shared_ptr& joint_state_subscriber) + : nh_(nh) + , parameters_(parameters) + , planning_scene_monitor_(planning_scene_monitor) + , joint_state_subscriber_(joint_state_subscriber) + , period_(parameters.publish_period) +{ + // MoveIt Setup + const robot_model_loader::RobotModelLoaderPtr& model_loader_ptr = planning_scene_monitor_->getRobotModelLoader(); + while (ros::ok() && !model_loader_ptr) + { + ROS_WARN_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Waiting for a non-null robot_model_loader pointer"); + default_sleep_rate_.sleep(); + } + const moveit::core::RobotModelPtr& kinematic_model = model_loader_ptr->getModel(); + kinematic_state_ = std::make_shared(kinematic_model); + kinematic_state_->setToDefaultValues(); + + joint_model_group_ = kinematic_model->getJointModelGroup(parameters_.move_group_name); + prev_joint_velocity_ = Eigen::ArrayXd::Zero(joint_model_group_->getActiveJointModels().size()); + + // Subscribe to command topics + twist_stamped_sub_ = + nh_.subscribe(parameters_.cartesian_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::twistStampedCB, this); + joint_cmd_sub_ = nh_.subscribe(parameters_.joint_command_in_topic, ROS_QUEUE_SIZE, &ServoCalcs::jointCmdCB, this); + + // ROS Server for allowing drift in some dimensions + drift_dimensions_server_ = + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_drift_dimensions", + &ServoCalcs::changeDriftDimensions, this); + + // ROS Server for changing the control dimensions + control_dimensions_server_ = + nh_.advertiseService(nh_.getNamespace() + "/" + ros::this_node::getName() + "/change_control_dimensions", + &ServoCalcs::changeControlDimensions, this); + + // Publish and Subscribe to internal namespace topics + ros::NodeHandle internal_nh("~internal"); + collision_velocity_scale_sub_ = + internal_nh.subscribe("collision_velocity_scale", ROS_QUEUE_SIZE, &ServoCalcs::collisionVelocityScaleCB, this); + worst_case_stop_time_pub_ = internal_nh.advertise("worst_case_stop_time", ROS_QUEUE_SIZE); + + // Publish freshly-calculated joints to the robot. + // Put the outgoing msg in the right format (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + outgoing_cmd_pub_ = nh_.advertise(parameters_.command_out_topic, ROS_QUEUE_SIZE); + + // Publish status + status_pub_ = nh_.advertise(parameters_.status_topic, ROS_QUEUE_SIZE); + + internal_joint_state_.name = joint_model_group_->getActiveJointModelNames(); + num_joints_ = internal_joint_state_.name.size(); + internal_joint_state_.position.resize(num_joints_); + internal_joint_state_.velocity.resize(num_joints_); + + // Set up the "last" published message, in case we need to send it first + auto initial_joint_trajectory = moveit::util::make_shared_from_pool(); + auto latest_joints = joint_state_subscriber_->getLatest(); + initial_joint_trajectory->header.frame_id = parameters_.planning_frame; + initial_joint_trajectory->header.stamp = ros::Time::now(); + initial_joint_trajectory->joint_names = internal_joint_state_.name; + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(parameters_.publish_period); + if (parameters_.publish_joint_positions) + point.positions = latest_joints->position; + if (parameters_.publish_joint_velocities) + { + std::vector velocity(num_joints_); + point.velocities = velocity; + } + if (parameters_.publish_joint_accelerations) + { + // I do not know of a robot that takes acceleration commands. + // However, some controllers check that this data is non-empty. + // Send all zeros, for now. + std::vector acceleration(num_joints_); + point.accelerations = acceleration; + } + initial_joint_trajectory->points.push_back(point); + last_sent_command_ = initial_joint_trajectory; + + // A map for the indices of incoming joint commands + for (std::size_t i = 0; i < num_joints_; ++i) + { + joint_state_name_map_[internal_joint_state_.name[i]] = i; + } + + // Low-pass filters for the joint positions + for (size_t i = 0; i < num_joints_; ++i) + { + position_filters_.emplace_back(parameters_.low_pass_filter_coeff); + } +} + +void ServoCalcs::start() +{ + stop_requested_ = false; + timer_ = nh_.createTimer(period_, &ServoCalcs::run, this); +} + +void ServoCalcs::stop() +{ + stop_requested_ = true; + timer_.stop(); +} + +void ServoCalcs::run(const ros::TimerEvent& timer_event) +{ + // Log warning when the last loop duration was longer than the period + if (timer_event.profile.last_duration.toSec() > period_.toSec()) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "last_duration: " << timer_event.profile.last_duration.toSec() << " (" + << period_.toSec() << ")"); + } + + // Publish status each loop iteration + auto status_msg = moveit::util::make_shared_from_pool(); + status_msg->data = static_cast(status_); + status_pub_.publish(status_msg); + + // Always update the joints and end-effector transform for 2 reasons: + // 1) in case the getCommandFrameTransform() method is being used + // 2) so the low-pass filters are up to date and don't cause a jump + while (!updateJoints() && ros::ok()) + { + if (stop_requested_) + return; + default_sleep_rate_.sleep(); + } + + // Update from latest state + sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); + kinematic_state_->setVariableValues(*latest_joint_state); + { + const std::lock_guard lock(latest_state_mutex_); + if (latest_twist_stamped_) + twist_stamped_cmd_ = *latest_twist_stamped_; + if (latest_joint_cmd_) + joint_servo_cmd_ = *latest_joint_cmd_; + + // Check for stale cmds + twist_command_is_stale_ = + ((ros::Time::now() - latest_twist_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + joint_command_is_stale_ = + ((ros::Time::now() - latest_joint_command_stamp_) >= ros::Duration(parameters_.incoming_command_timeout)); + + have_nonzero_twist_stamped_ = latest_nonzero_twist_stamped_; + have_nonzero_joint_command_ = latest_nonzero_joint_cmd_; + } + + // Get the transform from MoveIt planning frame to servoing command frame + // Calculate this transform to ensure it is available via C++ API + // We solve (planning_frame -> base -> robot_link_command_frame) + // by computing (base->planning_frame)^-1 * (base->robot_link_command_frame) + tf_moveit_to_robot_cmd_frame_ = kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(parameters_.robot_link_command_frame); + + have_nonzero_command_ = have_nonzero_twist_stamped_ || have_nonzero_joint_command_; + + // Don't end this function without updating the filters + updated_filters_ = false; + + // If paused or while waiting for initial servo commands, just keep the low-pass filters up to date with current + // joints so a jump doesn't occur when restarting + if (wait_for_servo_commands_ || paused_) + { + resetLowPassFilters(original_joint_state_); + + // Check if there are any new commands with valid timestamp + wait_for_servo_commands_ = + twist_stamped_cmd_.header.stamp == ros::Time(0.) && joint_servo_cmd_.header.stamp == ros::Time(0.); + + // Early exit + return; + } + + // If not waiting for initial command, and not paused. + // Do servoing calculations only if the robot should move, for efficiency + // Create new outgoing joint trajectory command message + auto joint_trajectory = moveit::util::make_shared_from_pool(); + + // Prioritize cartesian servoing above joint servoing + // Only run commands if not stale and nonzero + if (have_nonzero_twist_stamped_ && !twist_command_is_stale_) + { + if (!cartesianServoCalcs(twist_stamped_cmd_, *joint_trajectory)) + { + resetLowPassFilters(original_joint_state_); + return; + } + } + else if (have_nonzero_joint_command_ && !joint_command_is_stale_) + { + if (!jointServoCalcs(joint_servo_cmd_, *joint_trajectory)) + { + resetLowPassFilters(original_joint_state_); + return; + } + } + else + { + // Joint trajectory is not populated with anything, so set it to the last positions and 0 velocity + *joint_trajectory = *last_sent_command_; + for (auto point : joint_trajectory->points) + { + point.velocities.assign(point.velocities.size(), 0); + } + } + + // Print a warning to the user if both are stale + if (!twist_command_is_stale_ || !joint_command_is_stale_) + { + ROS_WARN_STREAM_THROTTLE_NAMED(10, LOGNAME, "Stale command. " + "Try a larger 'incoming_command_timeout' parameter?"); + } + + // If we should halt + if (!have_nonzero_command_) + { + suddenHalt(*joint_trajectory); + have_nonzero_twist_stamped_ = false; + have_nonzero_joint_command_ = false; + } + + // Skip the servoing publication if all inputs have been zero for several cycles in a row. + // num_outgoing_halt_msgs_to_publish == 0 signifies that we should keep republishing forever. + if (!have_nonzero_command_ && (parameters_.num_outgoing_halt_msgs_to_publish != 0) && + (zero_velocity_count_ > parameters_.num_outgoing_halt_msgs_to_publish)) + { + ok_to_publish_ = false; + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "All-zero command. Doing nothing."); + } + else + { + ok_to_publish_ = true; + } + + // Store last zero-velocity message flag to prevent superfluous warnings. + // Cartesian and joint commands must both be zero. + if (!have_nonzero_command_) + { + // Avoid overflow + if (zero_velocity_count_ < std::numeric_limits::max()) + ++zero_velocity_count_; + } + else + { + zero_velocity_count_ = 0; + } + + if (ok_to_publish_) + { + // Put the outgoing msg in the right format + // (trajectory_msgs/JointTrajectory or std_msgs/Float64MultiArray). + if (parameters_.command_out_type == "trajectory_msgs/JointTrajectory") + { + joint_trajectory->header.stamp = ros::Time::now(); + outgoing_cmd_pub_.publish(joint_trajectory); + } + else if (parameters_.command_out_type == "std_msgs/Float64MultiArray") + { + auto joints = moveit::util::make_shared_from_pool(); + if (parameters_.publish_joint_positions && !joint_trajectory->points.empty()) + joints->data = joint_trajectory->points[0].positions; + else if (parameters_.publish_joint_velocities && !joint_trajectory->points.empty()) + joints->data = joint_trajectory->points[0].velocities; + outgoing_cmd_pub_.publish(joints); + } + + last_sent_command_ = joint_trajectory; + } + + // Update the filters if we haven't yet + if (!updated_filters_) + resetLowPassFilters(original_joint_state_); +} +// Perform the servoing calculations +bool ServoCalcs::cartesianServoCalcs(geometry_msgs::TwistStamped& cmd, + trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // Check for nan's in the incoming command + if (std::isnan(cmd.twist.linear.x) || std::isnan(cmd.twist.linear.y) || std::isnan(cmd.twist.linear.z) || + std::isnan(cmd.twist.angular.x) || std::isnan(cmd.twist.angular.y) || std::isnan(cmd.twist.angular.z)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); + return false; + } + + // If incoming commands should be in the range [-1:1], check for |delta|>1 + if (parameters_.command_in_type == "unitless") + { + if ((fabs(cmd.twist.linear.x) > 1) || (fabs(cmd.twist.linear.y) > 1) || (fabs(cmd.twist.linear.z) > 1) || + (fabs(cmd.twist.angular.x) > 1) || (fabs(cmd.twist.angular.y) > 1) || (fabs(cmd.twist.angular.z) > 1)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Component of incoming command is >1. Skipping this datapoint."); + return false; + } + } + + // Set uncontrolled dimensions to 0 in command frame + if (!control_dimensions_[0]) + cmd.twist.linear.x = 0; + if (!control_dimensions_[1]) + cmd.twist.linear.y = 0; + if (!control_dimensions_[2]) + cmd.twist.linear.z = 0; + if (!control_dimensions_[3]) + cmd.twist.angular.x = 0; + if (!control_dimensions_[4]) + cmd.twist.angular.y = 0; + if (!control_dimensions_[5]) + cmd.twist.angular.z = 0; + + // Transform the command to the MoveGroup planning frame + if (cmd.header.frame_id != parameters_.planning_frame) + { + Eigen::Vector3d translation_vector(cmd.twist.linear.x, cmd.twist.linear.y, cmd.twist.linear.z); + Eigen::Vector3d angular_vector(cmd.twist.angular.x, cmd.twist.angular.y, cmd.twist.angular.z); + + // If the incoming frame is empty or is the command frame, we use the previously calculated tf + if (cmd.header.frame_id.empty() || cmd.header.frame_id == parameters_.robot_link_command_frame) + { + translation_vector = tf_moveit_to_robot_cmd_frame_.linear() * translation_vector; + angular_vector = tf_moveit_to_robot_cmd_frame_.linear() * angular_vector; + } + else + { + // We solve (planning_frame -> base -> cmd.header.frame_id) + // by computing (base->planning_frame)^-1 * (base->cmd.header.frame_id) + const auto tf_moveit_to_incoming_cmd_frame = + kinematic_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() * + kinematic_state_->getGlobalLinkTransform(cmd.header.frame_id); + + translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector; + angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector; + } + + // Put these components back into a TwistStamped + cmd.header.frame_id = parameters_.planning_frame; + cmd.twist.linear.x = translation_vector(0); + cmd.twist.linear.y = translation_vector(1); + cmd.twist.linear.z = translation_vector(2); + cmd.twist.angular.x = angular_vector(0); + cmd.twist.angular.y = angular_vector(1); + cmd.twist.angular.z = angular_vector(2); + } + + Eigen::VectorXd delta_x = scaleCartesianCommand(cmd); + + // Convert from cartesian commands to joint commands + Eigen::MatrixXd jacobian = kinematic_state_->getJacobian(joint_model_group_); + + // May allow some dimensions to drift, based on drift_dimensions + // i.e. take advantage of task redundancy. + // Remove the Jacobian rows corresponding to True in the vector drift_dimensions + // Work backwards through the 6-vector so indices don't get out of order + for (auto dimension = jacobian.rows() - 1; dimension >= 0; --dimension) + { + if (drift_dimensions_[dimension] && jacobian.rows() > 1) + { + removeDimension(jacobian, delta_x, dimension); + } + } + + Eigen::JacobiSVD svd = + Eigen::JacobiSVD(jacobian, Eigen::ComputeThinU | Eigen::ComputeThinV); + Eigen::MatrixXd matrix_s = svd.singularValues().asDiagonal(); + Eigen::MatrixXd pseudo_inverse = svd.matrixV() * matrix_s.inverse() * svd.matrixU().transpose(); + + delta_theta_ = pseudo_inverse * delta_x; + + enforceSRDFAccelVelLimits(delta_theta_); + + // If close to a collision or a singularity, decelerate + applyVelocityScaling(delta_theta_, velocityScalingFactorForSingularity(delta_x, svd, pseudo_inverse)); + if (status_ == StatusCode::HALT_FOR_COLLISION) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(5, LOGNAME, "Halting for collision!"); + delta_theta_.setZero(); + } + + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + + return convertDeltasToOutgoingCmd(joint_trajectory); +} + +bool ServoCalcs::jointServoCalcs(const control_msgs::JointJog& cmd, trajectory_msgs::JointTrajectory& joint_trajectory) +{ + // Check for nan's + for (double velocity : cmd.velocities) + { + if (std::isnan(velocity)) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "nan in incoming command. Skipping this datapoint."); + return false; + } + } + + // Apply user-defined scaling + delta_theta_ = scaleJointCommand(cmd); + + enforceSRDFAccelVelLimits(delta_theta_); + + prev_joint_velocity_ = delta_theta_ / parameters_.publish_period; + + return convertDeltasToOutgoingCmd(joint_trajectory); +} + +bool ServoCalcs::convertDeltasToOutgoingCmd(trajectory_msgs::JointTrajectory& joint_trajectory) +{ + internal_joint_state_ = original_joint_state_; + if (!addJointIncrements(internal_joint_state_, delta_theta_)) + return false; + + lowPassFilterPositions(internal_joint_state_); + + // Calculate joint velocities here so that positions are filtered and SRDF bounds still get checked + calculateJointVelocities(internal_joint_state_, delta_theta_); + + composeJointTrajMessage(internal_joint_state_, joint_trajectory); + + if (!enforceSRDFPositionLimits()) + { + suddenHalt(joint_trajectory); + status_ = StatusCode::JOINT_BOUND; + } + + // done with calculations + if (parameters_.use_gazebo) + { + insertRedundantPointsIntoTrajectory(joint_trajectory, gazebo_redundant_message_count_); + } + + return true; +} + +// Spam several redundant points into the trajectory. The first few may be skipped if the +// time stamp is in the past when it reaches the client. Needed for gazebo simulation. +// Start from 2 because the first point's timestamp is already 1*parameters_.publish_period +void ServoCalcs::insertRedundantPointsIntoTrajectory(trajectory_msgs::JointTrajectory& joint_trajectory, + int count) const +{ + joint_trajectory.points.resize(count); + auto point = joint_trajectory.points[0]; + // Start from 2 because we already have the first point. End at count+1 so (total #) == count + for (int i = 2; i < count; ++i) + { + point.time_from_start = ros::Duration(i * parameters_.publish_period); + joint_trajectory.points[i] = point; + } +} + +void ServoCalcs::lowPassFilterPositions(sensor_msgs::JointState& joint_state) +{ + for (size_t i = 0; i < position_filters_.size(); ++i) + { + joint_state.position[i] = position_filters_[i].filter(joint_state.position[i]); + } + + updated_filters_ = true; +} + +void ServoCalcs::resetLowPassFilters(const sensor_msgs::JointState& joint_state) +{ + for (std::size_t i = 0; i < position_filters_.size(); ++i) + { + position_filters_[i].reset(joint_state.position[i]); + } + + updated_filters_ = true; +} + +void ServoCalcs::calculateJointVelocities(sensor_msgs::JointState& joint_state, const Eigen::ArrayXd& delta_theta) +{ + for (int i = 0; i < delta_theta.size(); ++i) + { + joint_state.velocity[i] = delta_theta[i] / parameters_.publish_period; + } +} + +void ServoCalcs::composeJointTrajMessage(const sensor_msgs::JointState& joint_state, + trajectory_msgs::JointTrajectory& joint_trajectory) const +{ + joint_trajectory.header.frame_id = parameters_.planning_frame; + joint_trajectory.header.stamp = ros::Time::now(); + joint_trajectory.joint_names = joint_state.name; + + trajectory_msgs::JointTrajectoryPoint point; + point.time_from_start = ros::Duration(parameters_.publish_period); + if (parameters_.publish_joint_positions) + point.positions = joint_state.position; + if (parameters_.publish_joint_velocities) + point.velocities = joint_state.velocity; + if (parameters_.publish_joint_accelerations) + { + // I do not know of a robot that takes acceleration commands. + // However, some controllers check that this data is non-empty. + // Send all zeros, for now. + std::vector acceleration(num_joints_); + point.accelerations = acceleration; + } + joint_trajectory.points.push_back(point); +} + +// Apply velocity scaling for proximity of collisions and singularities. +void ServoCalcs::applyVelocityScaling(Eigen::ArrayXd& delta_theta, double singularity_scale) +{ + double collision_scale = collision_velocity_scale_; + + if (collision_scale > 0 && collision_scale < 1) + { + status_ = StatusCode::DECELERATE_FOR_COLLISION; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + else if (collision_scale == 0) + { + status_ = StatusCode::HALT_FOR_COLLISION; + } + + delta_theta = collision_scale * singularity_scale * delta_theta; +} + +// Possibly calculate a velocity scaling factor, due to proximity of singularity and direction of motion +double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& commanded_velocity, + const Eigen::JacobiSVD& svd, + const Eigen::MatrixXd& pseudo_inverse) +{ + double velocity_scale = 1; + std::size_t num_dimensions = commanded_velocity.size(); + + // Find the direction away from nearest singularity. + // The last column of U from the SVD of the Jacobian points directly toward or away from the singularity. + // The sign can flip at any time, so we have to do some extra checking. + // Look ahead to see if the Jacobian's condition will decrease. + Eigen::VectorXd vector_toward_singularity = svd.matrixU().col(num_dimensions - 1); + + double ini_condition = svd.singularValues()(0) / svd.singularValues()(svd.singularValues().size() - 1); + + // This singular vector tends to flip direction unpredictably. See R. Bro, + // "Resolving the Sign Ambiguity in the Singular Value Decomposition". + // Look ahead to see if the Jacobian's condition will decrease in this + // direction. Start with a scaled version of the singular vector + Eigen::VectorXd delta_x(num_dimensions); + double scale = 100; + delta_x = vector_toward_singularity / scale; + + // Calculate a small change in joints + Eigen::VectorXd new_theta; + kinematic_state_->copyJointGroupPositions(joint_model_group_, new_theta); + new_theta += pseudo_inverse * delta_x; + kinematic_state_->setJointGroupPositions(joint_model_group_, new_theta); + auto new_jacobian = kinematic_state_->getJacobian(joint_model_group_); + + Eigen::JacobiSVD new_svd(new_jacobian); + double new_condition = new_svd.singularValues()(0) / new_svd.singularValues()(new_svd.singularValues().size() - 1); + // If new_condition < ini_condition, the singular vector does point towards a + // singularity. Otherwise, flip its direction. + if (ini_condition >= new_condition) + { + vector_toward_singularity *= -1; + } + + // If this dot product is positive, we're moving toward singularity ==> decelerate + double dot = vector_toward_singularity.dot(commanded_velocity); + if (dot > 0) + { + // Ramp velocity down linearly when the Jacobian condition is between lower_singularity_threshold and + // hard_stop_singularity_threshold, and we're moving towards the singularity + if ((ini_condition > parameters_.lower_singularity_threshold) && + (ini_condition < parameters_.hard_stop_singularity_threshold)) + { + velocity_scale = 1. - + (ini_condition - parameters_.lower_singularity_threshold) / + (parameters_.hard_stop_singularity_threshold - parameters_.lower_singularity_threshold); + status_ = StatusCode::DECELERATE_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + + // Very close to singularity, so halt. + else if (ini_condition > parameters_.hard_stop_singularity_threshold) + { + velocity_scale = 0; + status_ = StatusCode::HALT_FOR_SINGULARITY; + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, SERVO_STATUS_CODE_MAP.at(status_)); + } + } + + return velocity_scale; +} + +void ServoCalcs::enforceSRDFAccelVelLimits(Eigen::ArrayXd& delta_theta) +{ + Eigen::ArrayXd velocity = delta_theta / parameters_.publish_period; + const Eigen::ArrayXd acceleration = (velocity - prev_joint_velocity_) / parameters_.publish_period; + + std::size_t joint_delta_index = 0; + for (auto joint : joint_model_group_->getActiveJointModels()) + { + // Some joints do not have bounds defined + const auto bounds = joint->getVariableBounds(joint->getName()); + if (bounds.acceleration_bounded_) + { + bool clip_acceleration = false; + double acceleration_limit = 0.0; + if (acceleration(joint_delta_index) < bounds.min_acceleration_) + { + clip_acceleration = true; + acceleration_limit = bounds.min_acceleration_; + } + else if (acceleration(joint_delta_index) > bounds.max_acceleration_) + { + clip_acceleration = true; + acceleration_limit = bounds.max_acceleration_; + } + + // Apply acceleration bounds + if (clip_acceleration) + { + // accel = (vel - vel_prev) / delta_t = ((delta_theta / delta_t) - vel_prev) / delta_t + // --> delta_theta = (accel * delta_t _ + vel_prev) * delta_t + const double relative_change = + ((acceleration_limit * parameters_.publish_period + prev_joint_velocity_(joint_delta_index)) * + parameters_.publish_period) / + delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) + delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); + } + } + + if (bounds.velocity_bounded_) + { + velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_.publish_period; + + bool clip_velocity = false; + double velocity_limit = 0.0; + if (velocity(joint_delta_index) < bounds.min_velocity_) + { + clip_velocity = true; + velocity_limit = bounds.min_velocity_; + } + else if (velocity(joint_delta_index) > bounds.max_velocity_) + { + clip_velocity = true; + velocity_limit = bounds.max_velocity_; + } + + // Apply velocity bounds + if (clip_velocity) + { + // delta_theta = joint_velocity * delta_t + const double relative_change = (velocity_limit * parameters_.publish_period) / delta_theta(joint_delta_index); + // Avoid nan + if (fabs(relative_change) < 1) + { + delta_theta(joint_delta_index) = relative_change * delta_theta(joint_delta_index); + velocity(joint_delta_index) = relative_change * velocity(joint_delta_index); + } + } + } + ++joint_delta_index; + } +} + +bool ServoCalcs::enforceSRDFPositionLimits() +{ + bool halting = false; + + for (auto joint : joint_model_group_->getActiveJointModels()) + { + // Halt if we're past a joint margin and joint velocity is moving even farther past + double joint_angle = 0; + for (std::size_t c = 0; c < original_joint_state_.name.size(); ++c) + { + if (original_joint_state_.name[c] == joint->getName()) + { + joint_angle = original_joint_state_.position.at(c); + break; + } + } + if (!kinematic_state_->satisfiesPositionBounds(joint, -parameters_.joint_limit_margin)) + { + const std::vector limits = joint->getVariableBoundsMsg(); + + // Joint limits are not defined for some joints. Skip them. + if (!limits.empty()) + { + if ((kinematic_state_->getJointVelocities(joint)[0] < 0 && + (joint_angle < (limits[0].min_position + parameters_.joint_limit_margin))) || + (kinematic_state_->getJointVelocities(joint)[0] > 0 && + (joint_angle > (limits[0].max_position - parameters_.joint_limit_margin)))) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() + << " " << joint->getName() + << " close to a " + " position limit. Halting."); + halting = true; + } + } + } + } + return !halting; +} + +// Suddenly halt for a joint limit or other critical issue. +// Is handled differently for position vs. velocity control. +void ServoCalcs::suddenHalt(trajectory_msgs::JointTrajectory& joint_trajectory) +{ + if (joint_trajectory.points.empty()) + { + joint_trajectory.points.push_back(trajectory_msgs::JointTrajectoryPoint()); + joint_trajectory.points[0].positions.resize(num_joints_); + joint_trajectory.points[0].velocities.resize(num_joints_); + } + + for (std::size_t i = 0; i < num_joints_; ++i) + { + // For position-controlled robots, can reset the joints to a known, good state + if (parameters_.publish_joint_positions) + joint_trajectory.points[0].positions[i] = original_joint_state_.position[i]; + + // For velocity-controlled robots, stop + if (parameters_.publish_joint_velocities) + joint_trajectory.points[0].velocities[i] = 0; + } +} + +// Parse the incoming joint msg for the joints of our MoveGroup +bool ServoCalcs::updateJoints() +{ + sensor_msgs::JointStateConstPtr latest_joint_state = joint_state_subscriber_->getLatest(); + + // Check that the msg contains enough joints + if (latest_joint_state->name.size() < num_joints_) + return false; + + // Store joints in a member variable + for (std::size_t m = 0; m < latest_joint_state->name.size(); ++m) + { + std::size_t c; + try + { + c = joint_state_name_map_.at(latest_joint_state->name[m]); + } + catch (const std::out_of_range& e) + { + ROS_DEBUG_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Ignoring joint " + << latest_joint_state->name[m]); + continue; + } + + internal_joint_state_.position[c] = latest_joint_state->position[m]; + } + + // Cache the original joints in case they need to be reset + original_joint_state_ = internal_joint_state_; + + // Calculate worst case joint stop time, for collision checking + std::string joint_name = ""; + moveit::core::JointModel::Bounds kinematic_bounds; + double accel_limit = 0; + double joint_velocity = 0; + double worst_case_stop_time = 0; + for (size_t jt_state_idx = 0; jt_state_idx < latest_joint_state->velocity.size(); ++jt_state_idx) + { + joint_name = latest_joint_state->name[jt_state_idx]; + + // Get acceleration limit for this joint + for (auto joint_model : joint_model_group_->getActiveJointModels()) + { + if (joint_model->getName() == joint_name) + { + kinematic_bounds = joint_model->getVariableBounds(); + // Some joints do not have acceleration limits + if (kinematic_bounds[0].acceleration_bounded_) + { + // Be conservative when calculating overall acceleration limit from min and max limits + accel_limit = + std::min(fabs(kinematic_bounds[0].min_acceleration_), fabs(kinematic_bounds[0].max_acceleration_)); + } + else + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "An acceleration limit is not defined for this joint; minimum stop distance " + "should not be used for collision checking"); + } + break; + } + } + + // Get the current joint velocity + joint_velocity = latest_joint_state->velocity[jt_state_idx]; + + // Calculate worst case stop time + worst_case_stop_time = std::max(worst_case_stop_time, fabs(joint_velocity / accel_limit)); + } + + // publish message + { + auto msg = moveit::util::make_shared_from_pool(); + msg->data = worst_case_stop_time; + worst_case_stop_time_pub_.publish(msg); + } + + return true; +} + +// Scale the incoming servo command +Eigen::VectorXd ServoCalcs::scaleCartesianCommand(const geometry_msgs::TwistStamped& command) const +{ + Eigen::VectorXd result(6); + + // Apply user-defined scaling if inputs are unitless [-1:1] + if (parameters_.command_in_type == "unitless") + { + result[0] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.x; + result[1] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.y; + result[2] = parameters_.linear_scale * parameters_.publish_period * command.twist.linear.z; + result[3] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.x; + result[4] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.y; + result[5] = parameters_.rotational_scale * parameters_.publish_period * command.twist.angular.z; + } + // Otherwise, commands are in m/s and rad/s + else if (parameters_.command_in_type == "speed_units") + { + result[0] = command.twist.linear.x * parameters_.publish_period; + result[1] = command.twist.linear.y * parameters_.publish_period; + result[2] = command.twist.linear.z * parameters_.publish_period; + result[3] = command.twist.angular.x * parameters_.publish_period; + result[4] = command.twist.angular.y * parameters_.publish_period; + result[5] = command.twist.angular.z * parameters_.publish_period; + } + else + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type"); + + return result; +} + +Eigen::VectorXd ServoCalcs::scaleJointCommand(const control_msgs::JointJog& command) const +{ + Eigen::VectorXd result(num_joints_); + result.setZero(); + + std::size_t c; + for (std::size_t m = 0; m < command.joint_names.size(); ++m) + { + try + { + c = joint_state_name_map_.at(command.joint_names[m]); + } + catch (const std::out_of_range& e) + { + ROS_WARN_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, + "Ignoring joint " << joint_state_subscriber_->getLatest()->name[m]); + continue; + } + // Apply user-defined scaling if inputs are unitless [-1:1] + if (parameters_.command_in_type == "unitless") + result[c] = command.velocities[m] * parameters_.joint_scale * parameters_.publish_period; + // Otherwise, commands are in m/s and rad/s + else if (parameters_.command_in_type == "speed_units") + result[c] = command.velocities[m] * parameters_.publish_period; + else + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, "Unexpected command_in_type, check yaml file."); + } + + return result; +} + +// Add the deltas to each joint +bool ServoCalcs::addJointIncrements(sensor_msgs::JointState& output, const Eigen::VectorXd& increments) const +{ + for (std::size_t i = 0, size = static_cast(increments.size()); i < size; ++i) + { + try + { + output.position[i] += increments[i]; + } + catch (const std::out_of_range& e) + { + ROS_ERROR_STREAM_THROTTLE_NAMED(ROS_LOG_THROTTLE_PERIOD, LOGNAME, ros::this_node::getName() + << " Lengths of output and " + "increments do not match."); + return false; + } + } + + return true; +} + +void ServoCalcs::removeDimension(Eigen::MatrixXd& jacobian, Eigen::VectorXd& delta_x, unsigned int row_to_remove) +{ + unsigned int num_rows = jacobian.rows() - 1; + unsigned int num_cols = jacobian.cols(); + + if (row_to_remove < num_rows) + { + jacobian.block(row_to_remove, 0, num_rows - row_to_remove, num_cols) = + jacobian.block(row_to_remove + 1, 0, num_rows - row_to_remove, num_cols); + delta_x.segment(row_to_remove, num_rows - row_to_remove) = + delta_x.segment(row_to_remove + 1, num_rows - row_to_remove); + } + jacobian.conservativeResize(num_rows, num_cols); + delta_x.conservativeResize(num_rows); +} + +bool ServoCalcs::getCommandFrameTransform(Eigen::Isometry3d& transform) +{ + const std::lock_guard lock(latest_state_mutex_); + transform = tf_moveit_to_robot_cmd_frame_; + + // All zeros means the transform wasn't initialized, so return false + return !transform.matrix().isZero(0); +} + +void ServoCalcs::twistStampedCB(const geometry_msgs::TwistStampedConstPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + latest_twist_stamped_ = msg; + latest_nonzero_twist_stamped_ = isNonZero(*latest_twist_stamped_); + + if (msg->header.stamp != ros::Time(0.)) + latest_twist_command_stamp_ = msg->header.stamp; +} + +void ServoCalcs::jointCmdCB(const control_msgs::JointJogConstPtr& msg) +{ + const std::lock_guard lock(latest_state_mutex_); + latest_joint_cmd_ = msg; + latest_nonzero_joint_cmd_ = isNonZero(*latest_joint_cmd_); + + if (msg->header.stamp != ros::Time(0.)) + latest_joint_command_stamp_ = msg->header.stamp; +} + +void ServoCalcs::collisionVelocityScaleCB(const std_msgs::Float64ConstPtr& msg) +{ + collision_velocity_scale_ = msg->data; +} + +bool ServoCalcs::changeDriftDimensions(moveit_msgs::ChangeDriftDimensions::Request& req, + moveit_msgs::ChangeDriftDimensions::Response& res) +{ + drift_dimensions_[0] = req.drift_x_translation; + drift_dimensions_[1] = req.drift_y_translation; + drift_dimensions_[2] = req.drift_z_translation; + drift_dimensions_[3] = req.drift_x_rotation; + drift_dimensions_[4] = req.drift_y_rotation; + drift_dimensions_[5] = req.drift_z_rotation; + + res.success = true; + return true; +} + +bool ServoCalcs::changeControlDimensions(moveit_msgs::ChangeControlDimensions::Request& req, + moveit_msgs::ChangeControlDimensions::Response& res) +{ + control_dimensions_[0] = req.control_x_translation; + control_dimensions_[1] = req.control_y_translation; + control_dimensions_[2] = req.control_z_translation; + control_dimensions_[3] = req.control_x_rotation; + control_dimensions_[4] = req.control_y_rotation; + control_dimensions_[5] = req.control_z_rotation; + + res.success = true; + return true; +} + +void ServoCalcs::setPaused(bool paused) +{ + paused_ = paused; +} + +} // namespace moveit_servo diff --git a/moveit_ros/moveit_servo/src/servo_server.cpp b/moveit_ros/moveit_servo/src/servo_server.cpp new file mode 100644 index 0000000000..ef67b50277 --- /dev/null +++ b/moveit_ros/moveit_servo/src/servo_server.cpp @@ -0,0 +1,86 @@ +/******************************************************************************* + * BSD 3-Clause License + * + * Copyright (c) 2019, Los Alamos National Security, LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*******************************************************************************/ + +/* Title : servo_server.cpp + * Project : moveit_servo + * Created : 12/31/2018 + * Author : Andy Zelenak + */ + +#include + +namespace +{ +constexpr char LOGNAME[] = "servo_server"; +constexpr char ROS_THREADS = 8; + +} // namespace + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + ros::AsyncSpinner spinner(ROS_THREADS); + spinner.start(); + + ros::NodeHandle nh; + + // Load the planning scene monitor + auto planning_scene_monitor = std::make_shared("robot_description"); + if (!planning_scene_monitor->getPlanningScene()) + { + ROS_ERROR_STREAM_NAMED(LOGNAME, "Error in setting up the PlanningSceneMonitor."); + exit(EXIT_FAILURE); + } + + // Start the planning scene monitor + planning_scene_monitor->startSceneMonitor(); + planning_scene_monitor->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + planning_scene_monitor->startStateMonitor(); + + // Create the servo server + moveit_servo::Servo servo(nh, planning_scene_monitor); + + // Start the servo server (runs in the ros spinner) + servo.start(); + + // Wait for ros to shutdown + ros::waitForShutdown(); + + // Stop the servo server + servo.stop(); + + return 0; +} diff --git a/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp similarity index 87% rename from moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp rename to moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp index c7f853843d..8294ba897b 100644 --- a/moveit_experimental/moveit_jog_arm/src/teleop_examples/spacenav_to_twist.cpp +++ b/moveit_ros/moveit_servo/src/teleop_examples/spacenav_to_twist.cpp @@ -1,6 +1,6 @@ /******************************************************************************* * Title : spacenav_to_twist.cpp - * Project : moveit_jog_arm + * Project : moveit_servo * Created : 1/11/2019 * Author : Brian O'Neil, Andy Zelenak, Blake Anderson * @@ -41,7 +41,7 @@ #include "ros/ros.h" #include "sensor_msgs/Joy.h" -namespace moveit_jog_arm +namespace moveit_servo { static const int NUM_SPINNERS = 1; static const int QUEUE_LENGTH = 1; @@ -52,20 +52,20 @@ class SpaceNavToTwist SpaceNavToTwist() : spinner_(NUM_SPINNERS) { joy_sub_ = n_.subscribe("spacenav/joy", QUEUE_LENGTH, &SpaceNavToTwist::joyCallback, this); - twist_pub_ = n_.advertise("jog_server/delta_jog_cmds", QUEUE_LENGTH); - joint_delta_pub_ = n_.advertise("jog_server/joint_delta_jog_cmds", QUEUE_LENGTH); + twist_pub_ = n_.advertise("servo_server/delta_twist_cmds", QUEUE_LENGTH); + joint_delta_pub_ = n_.advertise("servo_server/delta_joint_cmds", QUEUE_LENGTH); spinner_.start(); ros::waitForShutdown(); }; private: - // Convert incoming joy commands to TwistStamped commands for jogging. - // The TwistStamped component goes to jogging, while buttons 0 & 1 control + // Convert incoming joy commands to TwistStamped commands for servoing. + // The TwistStamped component goes to servoing, while buttons 0 & 1 control // joints directly. void joyCallback(const sensor_msgs::Joy::ConstPtr& msg) { - // Cartesian jogging with the axes + // Cartesian servoing with the axes geometry_msgs::TwistStamped twist; twist.header.stamp = ros::Time::now(); twist.twist.linear.x = msg->axes[0]; @@ -76,7 +76,7 @@ class SpaceNavToTwist twist.twist.angular.y = msg->axes[4]; twist.twist.angular.z = msg->axes[5]; - // Joint jogging with the buttons + // Joint servoing with the buttons control_msgs::JointJog joint_deltas; // This example is for a UR5. joint_deltas.joint_names.push_back("shoulder_pan_joint"); @@ -95,13 +95,13 @@ class SpaceNavToTwist ros::Publisher twist_pub_, joint_delta_pub_; ros::AsyncSpinner spinner_; }; -} // namespace moveit_jog_arm +} // namespace moveit_servo int main(int argc, char** argv) { ros::init(argc, argv, "spacenav_to_twist"); - moveit_jog_arm::SpaceNavToTwist to_twist; + moveit_servo::SpaceNavToTwist to_twist; return 0; } \ No newline at end of file diff --git a/moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml b/moveit_ros/moveit_servo/test/config/initial_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/config/initial_position.yaml rename to moveit_ros/moveit_servo/test/config/initial_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml b/moveit_ros/moveit_servo/test/config/servo_settings.yaml similarity index 71% rename from moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml rename to moveit_ros/moveit_servo/test/config/servo_settings.yaml index a26f72a2b3..e98719f5f6 100644 --- a/moveit_experimental/moveit_jog_arm/test/config/jog_settings.yaml +++ b/moveit_ros/moveit_servo/test/config/servo_settings.yaml @@ -1,5 +1,5 @@ ############################################### -# Modify all parameters related to jogging here +# Modify all parameters related to servoing here ############################################### use_gazebo: false # Whether the robot is started in a Gazebo simulation environment @@ -33,7 +33,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' planning_frame: panda_link0 # The MoveIt planning frame. Often 'panda_link0' or 'world' ## Stopping behaviour -incoming_command_timeout: 1 # Stop jogging if X seconds elapse without a new command +incoming_command_timeout: 1 # Stop servoing if X seconds elapse without a new command # If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. # Important because ROS may drop some messages and we need the robot to halt reliably. num_outgoing_halt_msgs_to_publish: 1 @@ -44,14 +44,22 @@ hard_stop_singularity_threshold: 45 # Stop when the condition number hits this joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names -cartesian_command_in_topic: jog_server/delta_jog_cmds # Topic for incoming Cartesian twist commands -joint_command_in_topic: jog_server/joint_delta_jog_cmds # Topic for incoming joint angle commands +cartesian_command_in_topic: servo_server/delta_twist_cmds # Topic for incoming Cartesian twist commands +joint_command_in_topic: servo_server/delta_joint_cmds # Topic for incoming joint angle commands joint_topic: joint_states -status_topic: jog_server/status # Publish status to this topic -command_out_topic: jog_server/command # Publish outgoing commands here +status_topic: servo_server/status # Publish status to this topic +command_out_topic: servo_server/command # Publish outgoing commands here ## Collision checking for the entire robot body check_collisions: true # Check collisions? collision_check_rate: 5 # [Hz] Collision-checking can easily bog down a CPU if done too often. +# Two collision check algorithms are available: +# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. +# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits +collision_check_type: stop_distance +# Parameters for "threshold_distance"-type collision checking self_collision_proximity_threshold: 0.01 # Start decelerating when a collision is this far [m] scene_collision_proximity_threshold: 0.03 # Start decelerating when a collision is this far [m] +# Parameters for "stop_distance"-type collision checking +collision_distance_safety_factor: 1000 # Must be >= 1. A large safety factor is recommended to account for latency +min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m] diff --git a/moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml b/moveit_ros/moveit_servo/test/config/singular_position.yaml similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/config/singular_position.yaml rename to moveit_ros/moveit_servo/test/config/singular_position.yaml diff --git a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch similarity index 56% rename from moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch rename to moveit_ros/moveit_servo/test/launch/common_test_setup.launch index f406c10bdb..3d587e12e9 100644 --- a/moveit_experimental/moveit_jog_arm/test/launch/common_test_setup.launch +++ b/moveit_ros/moveit_servo/test/launch/common_test_setup.launch @@ -12,14 +12,14 @@ - - + + - - - + + + diff --git a/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test b/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test new file mode 100644 index 0000000000..b349292a6d --- /dev/null +++ b/moveit_ros/moveit_servo/test/launch/servo_drift_dimensions_service_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test b/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test new file mode 100644 index 0000000000..ef78ce1870 --- /dev/null +++ b/moveit_ros/moveit_servo/test/launch/servo_halt_msg_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test b/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test new file mode 100644 index 0000000000..38de3317a0 --- /dev/null +++ b/moveit_ros/moveit_servo/test/launch/servo_msg_reception_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test b/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test new file mode 100644 index 0000000000..918556cd82 --- /dev/null +++ b/moveit_ros/moveit_servo/test/launch/servo_vel_accel_limits_test.test @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py b/moveit_ros/moveit_servo/test/python_tests/__init__.py similarity index 100% rename from moveit_experimental/moveit_jog_arm/test/python_tests/__init__.py rename to moveit_ros/moveit_servo/test/python_tests/__init__.py diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py similarity index 79% rename from moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py rename to moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py index d28b5c5a6d..9d44acb223 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py +++ b/moveit_ros/moveit_servo/test/python_tests/drift_dimensions_service/test_drift_dimensions_service.py @@ -17,13 +17,13 @@ # Send a service call to allow drift in all but the y-dimension. # In other words, only the y-dimension will be controlled exactly. -# Check that the service returns and the jog node continues to publish commands to the robot. +# Check that the service returns and the servo node continues to publish commands to the robot. -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' +COMMAND_OUT_TOPIC = 'servo_server/command' -SERVICE_NAME = 'jog_server/change_drift_dimensions' +SERVICE_NAME = 'servo_server/change_drift_dimensions' @pytest.fixture @@ -32,7 +32,7 @@ def node(): def test_drift_dimensions_service(node): - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_servo_initialization() # Service to change drift dimensions drift_service = rospy.ServiceProxy(SERVICE_NAME, ChangeDriftDimensions) diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py similarity index 59% rename from moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py rename to moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py index bb083877b5..1f09573fc5 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/halt_msg/test_jog_arm_halt_msg.py +++ b/moveit_ros/moveit_servo/test/python_tests/halt_msg/test_servo_halt_msg.py @@ -14,18 +14,15 @@ import util # The robot starts at a singular position (see config file). -# The jogger should halt and publish a warning. -# Listen for a warning message from the jogger. +# The servo node should halt and publish a warning. +# Listen for a warning message from the servo node. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_jog_arm_halt_msg.py +# rosrun moveit_servo test_servo_halt_msg.py -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' -# jog_arm should publish a nonzero warning code here -HALT_TOPIC = 'jog_server/status' - -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' +# servo should publish a nonzero warning code here +STATUS_TOPIC = 'servo_server/status' @pytest.fixture @@ -33,10 +30,10 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class CartesianJogCmd(object): +class CartesianCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 + CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): @@ -47,17 +44,17 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_halt_msg(node): - assert util.wait_for_jogger_initialization(SERVICE_NAME) +def test_servo_halt_msg(node): + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( - HALT_TOPIC, Int8, lambda msg: received.append(msg) + STATUS_TOPIC, Int8, lambda msg: received.append(msg) ) - cartesian_cmd = CartesianJogCmd() + cartesian_cmd = CartesianCmd() - # This nonzero command should produce jogging output - # A subscriber in a different thread fills `received` + # This nonzero command should produce servoing output + # A subscriber in a different timer fills `received` TEST_DURATION = 1 start_time = rospy.get_rostime() received = [] @@ -68,9 +65,8 @@ def test_jog_arm_halt_msg(node): # Check the received messages # A non-zero value signifies a warning assert len(received) > 3 - assert (received[-1].data != 0) or (received[-2].data != 0) or (received[-3].data != 0) - + assert any(i != 0 for i in received[-3:]) if __name__ == '__main__': node = node() - test_jog_arm_halt_msg(node) + test_servo_halt_msg(node) diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py similarity index 63% rename from moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py rename to moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py index 9147559d43..116f6892ad 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/msg_reception/test_jog_arm_msg_reception.py +++ b/moveit_ros/moveit_servo/test/python_tests/msg_reception/test_servo_msg_reception.py @@ -13,17 +13,14 @@ sys.path.append(path.dirname(path.dirname(path.abspath(__file__)))) import util -# Test that the jogger publishes controller commands when it receives Cartesian or joint commands. +# Test that the servo node publishes controller commands when it receives Cartesian or joint commands. # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_jog_arm_integration.py +# rosrun moveit_servo test_servo_integration.py -JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' -CARTESIAN_JOG_COMMAND_TOPIC = 'jog_server/delta_jog_cmds' +JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' +CARTESIAN_COMMAND_TOPIC = 'servo_server/delta_twist_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' - -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' +COMMAND_OUT_TOPIC = 'servo_server/command' @pytest.fixture @@ -31,9 +28,9 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class JointJogCmd(object): +class JointCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) + self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -43,10 +40,10 @@ def send_joint_velocity_cmd(self, joint_pos): self._pub.publish(jj) -class CartesianJogCmd(object): +class CartesianCmd(object): def __init__(self): self._pub = rospy.Publisher( - CARTESIAN_JOG_COMMAND_TOPIC, TwistStamped, queue_size=1 + CARTESIAN_COMMAND_TOPIC, TwistStamped, queue_size=10 ) def send_cmd(self, linear, angular): @@ -57,33 +54,32 @@ def send_cmd(self, linear, angular): self._pub.publish(ts) -def test_jog_arm_cartesian_command(node): +def test_servo_cartesian_command(node): # Test sending a cartesian velocity command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - cartesian_cmd = CartesianJogCmd() + cartesian_cmd = CartesianCmd() # Repeated zero-commands should produce no output, other than a few halt messages - # A subscriber in a different thread fills 'received' - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) - cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) + # A subscriber in a different timer fills 'received' + for i in range(4): + cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 0]) + rospy.sleep(0.1) received = [] rospy.sleep(1) assert len(received) <= 4 # 'num_outgoing_halt_msgs_to_publish' in the config file - # This nonzero command should produce jogging output - # A subscriber in a different thread fills `received` + # This nonzero command should produce servoing output + # A subscriber in a different timer fills `received` TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file - # Send a command to start the jogger + # Send a command to start the servo node cartesian_cmd.send_cmd([0, 0, 0], [0, 0, 1]) start_time = rospy.get_rostime() @@ -97,22 +93,22 @@ def test_jog_arm_cartesian_command(node): assert len(received) <= TEST_DURATION/PUBLISH_PERIOD + 20 -def test_jog_arm_joint_command(node): +def test_servo_joint_command(node): # Test sending a joint command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() + joint_cmd = JointCmd() TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file velocities = [0.1] - # Send a command to start the jogger + # Send a command to start the servo node joint_cmd.send_joint_velocity_cmd(velocities) start_time = rospy.get_rostime() @@ -128,6 +124,6 @@ def test_jog_arm_joint_command(node): if __name__ == '__main__': node = node() - time.sleep(JOG_ARM_SETTLE_TIME_S) # wait for jog_arm server to init - test_jog_arm_cartesian_command(node) - test_jog_arm_joint_command(node) + time.sleep(SERVO_SETTLE_TIME_S) # wait for servo server to init + test_servo_cartesian_command(node) + test_servo_joint_command(node) diff --git a/moveit_ros/moveit_servo/test/python_tests/util.py b/moveit_ros/moveit_servo/test/python_tests/util.py new file mode 100644 index 0000000000..f813e8dea7 --- /dev/null +++ b/moveit_ros/moveit_servo/test/python_tests/util.py @@ -0,0 +1,14 @@ +import rospy +from std_msgs.msg import Int8 + +# servo should publish a nonzero warning code here +STATUS_TOPIC = 'servo_server/status' + +def wait_for_servo_initialization(timeout=15): + try: + rospy.wait_for_message(STATUS_TOPIC, Int8, timeout=timeout) + except rospy.ROSException as exc: + rospy.logerr("The servo topic " + STATUS_TOPIC + " is not available: " + str(exc)) + return False + + return True diff --git a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py similarity index 67% rename from moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py rename to moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py index 2baf6a594a..38dcdb2b8f 100755 --- a/moveit_experimental/moveit_jog_arm/test/python_tests/vel_accel_limits/test_vel_accel_limits.py +++ b/moveit_ros/moveit_servo/test/python_tests/vel_accel_limits/test_vel_accel_limits.py @@ -14,14 +14,11 @@ # Test that commands that are too fast are caught and flagged # This can be run as part of a pytest, or like a normal ROS executable: -# rosrun moveit_jog_arm test_vel_accel_limits.py +# rosrun moveit_servo test_vel_accel_limits.py -JOINT_JOG_COMMAND_TOPIC = 'jog_server/joint_delta_jog_cmds' +JOINT_COMMAND_TOPIC = 'servo_server/delta_joint_cmds' -COMMAND_OUT_TOPIC = 'jog_server/command' - -# Check if jogger is initialized with this service -SERVICE_NAME = 'jog_server/change_drift_dimensions' +COMMAND_OUT_TOPIC = 'servo_server/command' @pytest.fixture @@ -29,9 +26,9 @@ def node(): return rospy.init_node('pytest', anonymous=True) -class JointJogCmd(object): +class JointServoCmd(object): def __init__(self): - self._pub = rospy.Publisher(JOINT_JOG_COMMAND_TOPIC, JointJog, queue_size=1) + self._pub = rospy.Publisher(JOINT_COMMAND_TOPIC, JointJog, queue_size=10) def send_joint_velocity_cmd(self, joint_pos): jj = JointJog() @@ -44,24 +41,24 @@ def send_joint_velocity_cmd(self, joint_pos): def test_vel_limit(node): # Test sending a joint command - assert util.wait_for_jogger_initialization(SERVICE_NAME) + assert util.wait_for_servo_initialization() received = [] sub = rospy.Subscriber( COMMAND_OUT_TOPIC, JointTrajectory, lambda msg: received.append(msg) ) - joint_cmd = JointJogCmd() + joint_cmd = JointServoCmd() TEST_DURATION = 1 - PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from jog_arm config file + PUBLISH_PERIOD = 0.01 # 'PUBLISH_PERIOD' from servo config file # Panda arm limit, from joint_limits.yaml VELOCITY_LIMIT = rospy.get_param("/robot_description_planning/joint_limits/panda_joint1/max_velocity") # Send a velocity command that exceeds the limit velocities = [10 * VELOCITY_LIMIT] - # Send a command to start the jogger + # Send a command to start the servo node joint_cmd.send_joint_velocity_cmd(velocities) start_time = rospy.get_rostime() @@ -70,15 +67,19 @@ def test_vel_limit(node): joint_cmd.send_joint_velocity_cmd(velocities) time.sleep(0.1) - # Period of outgoing commands from the jogger, from yaml - JOGGER_COMMAND_PERIOD = rospy.get_param("/jog_server/publish_period") + # Period of outgoing commands from the servo node, from yaml + SERVO_COMMAND_PERIOD = rospy.get_param("/servo_server/publish_period") # Should be no velocities greater than the limit assert len(received) > 2 for msg_idx in range(1, len(received)): - velocity = \ - (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / JOGGER_COMMAND_PERIOD - assert abs(velocity) <= VELOCITY_LIMIT + try: + velocity = \ + (received[msg_idx].points[0].positions[0] - received[msg_idx - 1].points[0].positions[0]) / SERVO_COMMAND_PERIOD + assert abs(velocity) <= VELOCITY_LIMIT + except IndexError: + # Sometimes a message doesn't have any points + pass if __name__ == '__main__': node = node() diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp new file mode 100644 index 0000000000..4fd01fc73e --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.cpp @@ -0,0 +1,195 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, PickNik LLC + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik LLC nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Dave Coleman and Tyler Weaver + Desc: Test for the C++ interface to moveit_servo +*/ + +// C++ +#include + +// ROS +#include + +// Testing +#include + +// Servo +#include +#include + +static const std::string LOGNAME = "servo_cpp_interface_test"; + +namespace moveit_servo +{ +class ServoFixture : public ::testing::Test +{ +public: + void SetUp() override + { + // Load the planning scene monitor + planning_scene_monitor_ = std::make_shared("robot_description"); + planning_scene_monitor_->startSceneMonitor(); + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->startWorldGeometryMonitor( + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_COLLISION_OBJECT_TOPIC, + planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_WORLD_TOPIC, + false /* skip octomap monitor */); + + // Create moveit_servo + servo_ = std::make_shared(nh_, planning_scene_monitor_); + } + void TearDown() override + { + } + + bool waitForFirstStatus() + { + auto msg = ros::topic::waitForMessage(servo_->getParameters().status_topic, nh_, ros::Duration(15)); + return static_cast(msg); + } + +protected: + ros::NodeHandle nh_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit_servo::ServoPtr servo_; +}; // class ServoFixture + +TEST_F(ServoFixture, StartStopTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + servo_->stop(); + + ros::Duration(1.0).sleep(); + + // Start and stop again + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + servo_->stop(); +} + +TEST_F(ServoFixture, SendTwistStampedTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto twist_stamped_pub = nh_.advertise(parameters.cartesian_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few Cartesian velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link0"; + msg->twist.angular.y = 1.0; + + // Send the message + twist_stamped_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + servo_->stop(); +} + +TEST_F(ServoFixture, SendJointServoTest) +{ + servo_->start(); + EXPECT_TRUE(waitForFirstStatus()) << "Timeout waiting for Status message"; + + auto parameters = servo_->getParameters(); + + // count trajectory messages sent by servo + size_t received_count = 0; + boost::function traj_callback = + [&received_count](const trajectory_msgs::JointTrajectoryConstPtr& msg) { received_count++; }; + auto traj_sub = nh_.subscribe(parameters.command_out_topic, 1, traj_callback); + + // Create publisher to send servo commands + auto joint_servo_pub = nh_.advertise(parameters.joint_command_in_topic, 1); + + constexpr double test_duration = 1.0; + const double publish_period = parameters.publish_period; + const size_t num_commands = static_cast(test_duration / publish_period); + + ros::Rate publish_rate(1. / publish_period); + + // Send a few joint velocity commands + for (size_t i = 0; i < num_commands && ros::ok(); ++i) + { + auto msg = moveit::util::make_shared_from_pool(); + msg->header.stamp = ros::Time::now(); + msg->header.frame_id = "panda_link3"; + msg->velocities.push_back(0.1); + + // Send the message + joint_servo_pub.publish(msg); + publish_rate.sleep(); + } + + EXPECT_GT(received_count, num_commands - 20); + EXPECT_LT(received_count, num_commands + 20); + servo_->stop(); +} + +} // namespace moveit_servo + +int main(int argc, char** argv) +{ + ros::init(argc, argv, LOGNAME); + testing::InitGoogleTest(&argc, argv); + + ros::AsyncSpinner spinner(8); + spinner.start(); + + int result = RUN_ALL_TESTS(); + return result; +} diff --git a/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test new file mode 100644 index 0000000000..f34d8503da --- /dev/null +++ b/moveit_ros/moveit_servo/test/servo_cpp_interface_test.test @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + +