Merge moveit/moveit@9225971

.docker/ci-testing/Dockerfile

@@ -8,7 +8,7 @@ LABEL maintainer Robert Haschke [email protected]
 # Switch to ros-testing
 RUN echo "deb http://packages.ros.org/ros2-testing/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2-latest.list
 
-# Upgrade packages to ros-shadow-fixed and clean apt-cache within one RUN command
+# Upgrade packages to ros-testing and clean apt-cache within one RUN command
 RUN apt-get -qq update && \
     apt-get -qq upgrade && \
     #

moveit_commander/test/python_moveit_commander.py

@@ -45,7 +45,11 @@
 from moveit_msgs.msg import RobotState
 from sensor_msgs.msg import JointState
 
-from moveit_commander import RobotCommander, PlanningSceneInterface
+from moveit_commander import (
+    RobotCommander,
+    PlanningSceneInterface,
+    MoveItCommanderException,
+)
 
 
 class PythonMoveitCommanderTest(unittest.TestCase):
@@ -113,6 +117,13 @@ def test_target_setting(self):
         )
         self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5)
 
+        js_target = JointState(name=self.JOINT_NAMES, position=[0.1] * n)
+        self.check_target_setting([0.1] * n, js_target)
+        # name and position should have the same size, or raise exception
+        with self.assertRaises(MoveItCommanderException):
+            js_target.position = []
+            self.check_target_setting(None, js_target)
+
     def plan(self, target):
         self.group.set_joint_value_target(target)
         return self.group.plan()

moveit_core/CMakeLists.txt

@@ -5,15 +5,6 @@ project(moveit_core LANGUAGES CXX)
 find_package(moveit_common REQUIRED)
 moveit_package()
 
-# Generate and install version.h
-string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR "${moveit_core_VERSION}")
-string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${moveit_core_VERSION}")
-string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${moveit_core_VERSION}")
-set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}")
-message(STATUS " *** Building MoveIt ${MOVEIT_VERSION} ***")
-configure_file("version/version.h.in" "${CMAKE_BINARY_DIR}/include/moveit/version.h")
-install(FILES "${CMAKE_BINARY_DIR}/include/moveit/version.h" DESTINATION include/moveit_core/moveit)
-
 find_package(PkgConfig REQUIRED)
 pkg_check_modules(LIBFCL REQUIRED "fcl>=0.5.0")
 # replace LIBFCL_LIBRARIES with full paths to the libraries

moveit_core/version/CMakeLists.txt

@@ -1,3 +1,31 @@
+# Generate and install version.h
+
+if(NOT "$ENV{USER}" STREQUAL "buildfarm") # Don't define version postfix on buildfarm
+  set(MOVEIT_VERSION_EXTRA "devel" CACHE STRING "version string postfix")
+endif()
+
+message(STATUS " *** Building MoveIt ${moveit_core_VERSION} ${MOVEIT_VERSION_EXTRA} ***")
+
+# https://stackoverflow.com/questions/13920072/how-to-always-run-command-when-building-regardless-of-any-dependency
+add_custom_command(
+	OUTPUT ${CMAKE_BINARY_DIR}/include/moveit/version.h always_rebuild
+	COMMAND ${CMAKE_COMMAND}
+	-DVERSION_FILE_PATH="${CMAKE_BINARY_DIR}/include"
+	-DMOVEIT_VERSION="${moveit_core_VERSION}"
+	-DMOVEIT_VERSION_EXTRA="${MOVEIT_VERSION_EXTRA}"
+	-P ${CMAKE_CURRENT_SOURCE_DIR}/version.cmake
+	WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+)
+
+add_custom_target(version_h
+	DEPENDS always_rebuild
+	COMMENT "Generating version.h"
+)
+
 add_executable(moveit_version version.cpp)
-target_include_directories(moveit_version PRIVATE ${CMAKE_BINARY_DIR}/include)
+add_dependencies(moveit_version version_h)
+
+target_include_directories(moveit_version PRIVATE ${CMAKE_BINARY_DIR}/include/moveit_core)
+
+install(FILES "${CMAKE_BINARY_DIR}/include/moveit_core/moveit/version.h" DESTINATION include/moveit_core/moveit)
 install(TARGETS moveit_version RUNTIME DESTINATION bin)

moveit_core/version/version.cmake

@@ -0,0 +1,39 @@
+# Retrieve (active) branch name
+execute_process(
+	COMMAND git rev-parse --abbrev-ref HEAD
+	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+	OUTPUT_VARIABLE MOVEIT_GIT_NAME
+	OUTPUT_STRIP_TRAILING_WHITESPACE
+	ERROR_QUIET
+)
+
+if("${MOVEIT_GIT_NAME}" STREQUAL "HEAD")
+	# Retrieve any associated name (tag or branch)
+	execute_process(
+		COMMAND git describe --contains --all HEAD
+		WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+		OUTPUT_VARIABLE MOVEIT_GIT_NAME
+		OUTPUT_STRIP_TRAILING_WHITESPACE
+		ERROR_QUIET
+	)
+endif()
+
+# Retrieve (short) commit hash
+execute_process(
+	COMMAND git rev-parse --short HEAD
+	WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
+	OUTPUT_VARIABLE MOVEIT_GIT_COMMIT_HASH
+	OUTPUT_STRIP_TRAILING_WHITESPACE
+	ERROR_QUIET
+)
+
+string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR "${moveit_core_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${moveit_core_VERSION}")
+string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${moveit_core_VERSION}")
+set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}")
+
+if(NOT "${MOVEIT_VERSION_EXTRA}" STREQUAL "")
+	string(APPEND MOVEIT_VERSION "-${MOVEIT_VERSION_EXTRA}")
+endif()
+
+configure_file("version.h.in" "${VERSION_FILE_PATH}/moveit_core/moveit/version.h")

moveit_core/version/version.cpp

@@ -35,10 +35,18 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/version.h>
-#include <cstdio>
+#include <iostream>
+#include <cstring>
 
 int main(int /*argc*/, char** /*argv*/)
 {
-  printf("%s\n", MOVEIT_VERSION_STR);
+  std::cout << MOVEIT_VERSION_STR;
+  if (strlen(MOVEIT_GIT_COMMIT_HASH))
+  {
+    std::cout << ", git commit: " << MOVEIT_GIT_COMMIT_HASH;
+    if (strlen(MOVEIT_GIT_NAME))
+      std::cout << " (" << MOVEIT_GIT_NAME << ")";
+  }
+  std::cout << std::endl;
   return 0;
 }

moveit_core/version/version.h.in

@@ -40,6 +40,9 @@
 #define MOVEIT_VERSION_MINOR @MOVEIT_VERSION_MINOR@
 #define MOVEIT_VERSION_PATCH @MOVEIT_VERSION_PATCH@
 
+#define MOVEIT_GIT_NAME "@MOVEIT_GIT_NAME@"
+#define MOVEIT_GIT_COMMIT_HASH "@MOVEIT_GIT_COMMIT_HASH@"
+
 /// MOVEIT_VERSION is (major << 16) + (minor << 8) + patch.
 #define MOVEIT_VERSION MOVEIT_VERSION_CHECK(MOVEIT_VERSION_MAJOR, MOVEIT_VERSION_MINOR, MOVEIT_VERSION_PATCH)
 

moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp

@@ -137,7 +137,12 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
   }
   else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
   {
-    if (!(trajectory.fillInFromTrajectory(*res.trajectory_[0])))
+    if (res.trajectory_.empty())
+    {
+      RCLCPP_ERROR(LOGGER, "No input trajectory specified");
+      return false;
+    }
+    else if (!(trajectory.fillInFromTrajectory(*res.trajectory_[0])))
     {
       RCLCPP_ERROR(LOGGER, "Input trajectory has less than 2 points, "
                            "trajectory must contain at least start and goal state");
@@ -251,6 +256,8 @@ bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_s
 
   res.trajectory_.resize(1);
   res.trajectory_[0] = result;
+  res.description_.resize(1);
+  res.description_[0] = "plan";
 
   RCLCPP_DEBUG(LOGGER, "Bottom took %ld sec to create", (std::chrono::system_clock::now() - create_time).count());
   RCLCPP_DEBUG(LOGGER, "Serviced planning request in %ld wall-seconds",

moveit_planners/trajopt/CMakeLists.txt

@@ -10,8 +10,8 @@ find_package(catkin REQUIRED
       pluginlib
       roscpp
       rosparam_shortcuts
-      trajopt
 )
+find_package(trajopt REQUIRED)
 
 catkin_package(
     INCLUDE_DIRS include
@@ -48,6 +48,7 @@ target_link_libraries(
   ${PROJECT_NAME}
   ${catkin_LIBRARIES}
   ${Boost_LIBRARIES}
+  trajopt::trajopt
 )
 
 set_target_properties(

moveit_planners/trajopt/src/problem_description.cpp

@@ -42,6 +42,7 @@
 #include <trajopt/trajectory_costs.hpp>
 #include <trajopt_sco/expr_op_overloads.hpp>
 #include <trajopt_sco/expr_ops.hpp>
+#include <trajopt_utils/eigen_conversions.hpp>
 #include <trajopt_utils/eigen_slicing.hpp>
 #include <trajopt_utils/logging.hpp>
 #include <trajopt_utils/vector_ops.hpp>
@@ -291,13 +292,15 @@ void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob)
   }
   else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME))
   {
-    sco::VectorOfVectorPtr f = std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
+    sco::VectorOfVector::Ptr f =
+        std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
     prob.addCost(std::make_shared<sco::CostFromErrFunc>(f, prob.GetVarRow(timestep, 0, n_dof),
                                                         concatVector(rot_coeffs, pos_coeffs), sco::ABS, name));
   }
   else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME))
   {
-    sco::VectorOfVectorPtr f = std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
+    sco::VectorOfVector::Ptr f =
+        std::make_shared<CartPoseErrCalculator>(input_pose, prob.GetPlanningScene(), link, tcp);
     prob.addConstraint(std::make_shared<sco::ConstraintFromErrFunc>(
         f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name));
   }

moveit_planners/trajopt/src/trajopt_interface.cpp

@@ -227,7 +227,7 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_merit_coeff_increases: " << params_.max_merit_coeff_increases);
   ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_coeff_increase_ratio: " << params_.merit_coeff_increase_ratio);
   ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_time: " << params_.max_time);
-  ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_error_coeff: " << params_.merit_error_coeff);
+  ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.initial_merit_error_coeff: " << params_.initial_merit_error_coeff);
   ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_box_size: " << params_.trust_box_size);
   ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.n_steps: " << problem_info.basic_info.n_steps);
   ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_upper_lim: " << problem_info.basic_info.dt_upper_lim);
@@ -362,7 +362,7 @@ void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& para
   nh_.param("trajopt_param/max_merit_coeff_increases", params.max_merit_coeff_increases, 5.0);
   nh_.param("trajopt_param/merit_coeff_increase_ratio", params.merit_coeff_increase_ratio, 10.0);
   nh_.param("trajopt_param/max_time", params.max_time, static_cast<double>(INFINITY));
-  nh_.param("trajopt_param/merit_error_coeff", params.merit_error_coeff, 10.0);
+  nh_.param("trajopt_param/merit_error_coeff", params.initial_merit_error_coeff, 10.0);
   nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1);
 }
 

moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp

@@ -1797,6 +1797,11 @@ bool MoveGroupInterface::setJointValueTarget(const std::map<std::string, double>
 bool MoveGroupInterface::setJointValueTarget(const std::vector<std::string>& variable_names,
                                              const std::vector<double>& variable_values)
 {
+  if (variable_names.size() != variable_values.size())
+  {
+    RCLCPP_ERROR_STREAM(LOGGER, "sizes of name and position arrays do not match");
+    return false;
+  }
   const auto& allowed = impl_->getJointModelGroup()->getVariableNames();
   for (const auto& variable_name : variable_names)
   {