Add modify allowed collisions command

Author: Levi-Armstrong

tesseract_environment/CMakeLists.txt

@@ -77,7 +77,6 @@ target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CUR
 add_library(
   ${PROJECT_NAME}_commands
   src/command.cpp
-  src/commands/add_allowed_collision_command.cpp
   src/commands/add_contact_managers_plugin_info_command.cpp
   src/commands/add_kinematics_information_command.cpp
   src/commands/add_link_command.cpp
@@ -90,9 +89,9 @@ add_library(
   src/commands/change_link_collision_enabled_command.cpp
   src/commands/change_link_origin_command.cpp
   src/commands/change_link_visibility_command.cpp
+  src/commands/modify_allowed_collisions_command.cpp
   src/commands/move_joint_command.cpp
   src/commands/move_link_command.cpp
-  src/commands/remove_allowed_collision_command.cpp
   src/commands/remove_allowed_collision_link_command.cpp
   src/commands/remove_joint_command.cpp
   src/commands/remove_link_command.cpp

tesseract_environment/include/tesseract_environment/command.h

@@ -49,19 +49,18 @@ enum class CommandType
   CHANGE_JOINT_ORIGIN = 6,
   CHANGE_LINK_COLLISION_ENABLED = 7,
   CHANGE_LINK_VISIBILITY = 8,
-  ADD_ALLOWED_COLLISION = 9,
-  REMOVE_ALLOWED_COLLISION = 10,
-  REMOVE_ALLOWED_COLLISION_LINK = 11,
-  ADD_SCENE_GRAPH = 12,
-  CHANGE_JOINT_POSITION_LIMITS = 13,
-  CHANGE_JOINT_VELOCITY_LIMITS = 14,
-  CHANGE_JOINT_ACCELERATION_LIMITS = 15,
-  ADD_KINEMATICS_INFORMATION = 16,
-  REPLACE_JOINT = 17,
-  CHANGE_COLLISION_MARGINS = 18,
-  ADD_CONTACT_MANAGERS_PLUGIN_INFO = 19,
-  SET_ACTIVE_DISCRETE_CONTACT_MANAGER = 20,
-  SET_ACTIVE_CONTINUOUS_CONTACT_MANAGER = 21
+  MODIFY_ALLOWED_COLLISIONS = 9,
+  REMOVE_ALLOWED_COLLISION_LINK = 10,
+  ADD_SCENE_GRAPH = 11,
+  CHANGE_JOINT_POSITION_LIMITS = 12,
+  CHANGE_JOINT_VELOCITY_LIMITS = 13,
+  CHANGE_JOINT_ACCELERATION_LIMITS = 14,
+  ADD_KINEMATICS_INFORMATION = 15,
+  REPLACE_JOINT = 16,
+  CHANGE_COLLISION_MARGINS = 17,
+  ADD_CONTACT_MANAGERS_PLUGIN_INFO = 18,
+  SET_ACTIVE_DISCRETE_CONTACT_MANAGER = 19,
+  SET_ACTIVE_CONTINUOUS_CONTACT_MANAGER = 20
 };
 
 template <class Archive>
@@ -79,7 +78,7 @@ class Command
   using Ptr = std::shared_ptr<Command>;
   using ConstPtr = std::shared_ptr<const Command>;
 
-  Command(CommandType type = CommandType::UNINITIALIZED) : type_(type){};
+  Command(CommandType type = CommandType::UNINITIALIZED);
   virtual ~Command() = default;
   Command(const Command&) = default;
   Command& operator=(const Command&) = default;

tesseract_environment/include/tesseract_environment/commands.h

@@ -28,7 +28,6 @@
 #ifndef TESSERACT_ENVIRONMENT_COMMANDS_H
 #define TESSERACT_ENVIRONMENT_COMMANDS_H
 
-#include <tesseract_environment/commands/add_allowed_collision_command.h>
 #include <tesseract_environment/commands/add_contact_managers_plugin_info_command.h>
 #include <tesseract_environment/commands/add_link_command.h>
 #include <tesseract_environment/commands/add_kinematics_information_command.h>
@@ -40,9 +39,9 @@
 #include <tesseract_environment/commands/change_link_collision_enabled_command.h>
 #include <tesseract_environment/commands/change_link_origin_command.h>
 #include <tesseract_environment/commands/change_link_visibility_command.h>
+#include <tesseract_environment/commands/modify_allowed_collisions_command.h>
 #include <tesseract_environment/commands/move_joint_command.h>
 #include <tesseract_environment/commands/move_link_command.h>
-#include <tesseract_environment/commands/remove_allowed_collision_command.h>
 #include <tesseract_environment/commands/remove_allowed_collision_link_command.h>
 #include <tesseract_environment/commands/remove_joint_command.h>
 #include <tesseract_environment/commands/remove_link_command.h>

tesseract_environment/include/tesseract_environment/commands/add_allowed_collision_command.h

@@ -0,0 +1,78 @@
+/**
+ * @file modify_allowed_collision_command.h
+ * @brief Used to modify an allowed collision to the environment
+ *
+ * @author Levi Armstrong
+ * @date Dec 18, 2017
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2017, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_ENVIRONMENT_MODIFY_ALLOWED_COLLISIONS_MATRIX_COMMAND_H
+#define TESSERACT_ENVIRONMENT_MODIFY_ALLOWED_COLLISIONS_MATRIX_COMMAND_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <boost/serialization/access.hpp>
+#include <memory>
+#include <string>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_environment/command.h>
+#include <tesseract_common/allowed_collision_matrix.h>
+
+namespace tesseract_environment
+{
+enum class ModifyAllowedCollisionsType
+{
+  ADD,
+  REMOVE,
+  REPLACE
+};
+
+class ModifyAllowedCollisionsCommand : public Command
+{
+public:
+  using Ptr = std::shared_ptr<ModifyAllowedCollisionsCommand>;
+  using ConstPtr = std::shared_ptr<const ModifyAllowedCollisionsCommand>;
+
+  ModifyAllowedCollisionsCommand();
+
+  ModifyAllowedCollisionsCommand(tesseract_common::AllowedCollisionMatrix acm, ModifyAllowedCollisionsType type);
+
+  ModifyAllowedCollisionsType getModifyType() const;
+  const tesseract_common::AllowedCollisionMatrix& getAllowedCollisionMatrix() const;
+
+  bool operator==(const ModifyAllowedCollisionsCommand& rhs) const;
+  bool operator!=(const ModifyAllowedCollisionsCommand& rhs) const;
+
+private:
+  ModifyAllowedCollisionsType type_{ ModifyAllowedCollisionsType::ADD };
+  tesseract_common::AllowedCollisionMatrix acm_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+};
+}  // namespace tesseract_environment
+
+#include <boost/serialization/export.hpp>
+#include <boost/serialization/tracking.hpp>
+BOOST_CLASS_EXPORT_KEY2(tesseract_environment::ModifyAllowedCollisionsCommand, "ModifyAllowedCollisionsCommand")
+
+#endif  // TESSERACT_ENVIRONMENT_MODIFY_ALLOWED_COLLISIONS_MATRIX_COMMAND_H

tesseract_environment/include/tesseract_environment/commands/modify_allowed_collisions_command.h

@@ -709,8 +709,7 @@ class Environment
   bool applyChangeJointOriginCommand(const ChangeJointOriginCommand::ConstPtr& cmd);
   bool applyChangeLinkCollisionEnabledCommand(const ChangeLinkCollisionEnabledCommand::ConstPtr& cmd);
   bool applyChangeLinkVisibilityCommand(const ChangeLinkVisibilityCommand::ConstPtr& cmd);
-  bool applyAddAllowedCollisionCommand(const AddAllowedCollisionCommand::ConstPtr& cmd);
-  bool applyRemoveAllowedCollisionCommand(const RemoveAllowedCollisionCommand::ConstPtr& cmd);
+  bool applyModifyAllowedCollisionsCommand(const ModifyAllowedCollisionsCommand::ConstPtr& cmd);
   bool applyRemoveAllowedCollisionLinkCommand(const RemoveAllowedCollisionLinkCommand::ConstPtr& cmd);
   bool applyAddSceneGraphCommand(AddSceneGraphCommand::ConstPtr cmd);
   bool applyChangeJointPositionLimitsCommand(const ChangeJointPositionLimitsCommand::ConstPtr& cmd);

tesseract_environment/include/tesseract_environment/commands/remove_allowed_collision_command.h

@@ -36,6 +36,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 namespace tesseract_environment
 {
+Command::Command(CommandType type) : type_(type) {}
+
 template <class Archive>
 void save(Archive& ar, const CommandType& g, const unsigned int /*version*/)
 {