moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
sequence.cpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2019 Pilz GmbH & Co. KG
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Pilz GmbH & Co. KG nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
36
37#include <algorithm>
38
40{
45{
46public:
47 template <typename T>
49 {
50 return cmd.toRequest();
51 }
52};
53
58{
59public:
60 template <typename T>
61 MotionCmd& operator()(T& cmd) const
62 {
63 return cmd;
64 }
65};
66
67moveit_msgs::msg::MotionSequenceRequest Sequence::toRequest() const
68{
69 moveit_msgs::msg::MotionSequenceRequest req;
70
71 std::vector<std::string> group_names;
72 for (const auto& cmd : cmds_)
73 {
74 moveit_msgs::msg::MotionSequenceItem item;
75 item.req = std::visit(ToReqVisitor(), cmd.first);
76
77 if (std::find(group_names.begin(), group_names.end(), item.req.group_name) != group_names.end())
78 {
79 // Remove start state because only the first request of a group
80 // is allowed to have a start state in a sequence.
81 item.req.start_state = moveit_msgs::msg::RobotState();
82 }
83 else
84 {
85 group_names.emplace_back(item.req.group_name);
86 }
87
88 item.blend_radius = cmd.second;
89 req.items.push_back(item);
90 }
91 return req;
92}
93
94void Sequence::erase(const size_t start, const size_t end)
95{
96 const size_t orig_n{ size() };
97 if (start > orig_n || end > orig_n)
98 {
99 std::string msg;
100 msg.append("Parameter start=").append(std::to_string(start));
101 msg.append(" and end=").append(std::to_string(end));
102 msg.append(" must not be greater then the number of #commands=");
103 msg.append(std::to_string(size()));
104 throw std::invalid_argument(msg);
105 }
106 cmds_.erase(cmds_.begin() + start, cmds_.begin() + end);
107 if (end == orig_n)
108 {
109 // Make sure last radius is set zero
110 cmds_.at(size() - 1).second = 0.;
111 }
112}
113
114MotionCmd& Sequence::getCmd(const size_t index_cmd)
115{
116 return std::visit(ToBaseVisitor(), cmds_.at(index_cmd).first);
117}
118
119} // namespace pilz_industrial_motion_planner_testutils
Base class for commands storing all general information of a command.
Definition motioncmd.h:48
size_t size() const
Returns the number of commands.
Definition sequence.h:108
void erase(const size_t start, const size_t end)
Deletes all commands from index 'start' to index 'end'.
Definition sequence.cpp:94
moveit_msgs::msg::MotionSequenceRequest toRequest() const
Definition sequence.cpp:67
Visitor returning not the specific command type but the base type.
Definition sequence.cpp:58
Visitor transforming the stored command into a MotionPlanRequest.
Definition sequence.cpp:45
planning_interface::MotionPlanRequest operator()(T &cmd) const
Definition sequence.cpp:48
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest