moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
sequence.h
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
35#pragma once
36
37#include <stdexcept>
38#include <vector>
39#include <utility>
40#include <typeinfo>
41
42#include <moveit_msgs/msg/motion_sequence_request.hpp>
43
45#include "motioncmd.h"
46#include <variant>
47
49{
54{
55public:
60 void add(const CmdVariant& cmd, const double blend_radius = 0.);
61
65 size_t size() const;
66
67 template <class T>
68 T& getCmd(const size_t index_cmd);
69
70 template <class T>
71 const T& getCmd(const size_t index_cmd) const;
72
77 template <class T>
78 bool cmdIsOfType(const size_t index_cmd) const;
79
85 MotionCmd& getCmd(const size_t index_cmd);
86
88 void setBlendRadius(const size_t index_cmd, const double blend_radius);
89 double getBlendRadius(const size_t index_cmd) const;
90
94 void erase(const size_t start, const size_t end);
95
96 moveit_msgs::msg::MotionSequenceRequest toRequest() const;
97
98private:
99 using TCmdRadiiPair = std::pair<CmdVariant, double>;
100 std::vector<TCmdRadiiPair> cmds_;
101};
102
103inline void Sequence::add(const CmdVariant& cmd, const double blend_radius)
104{
105 cmds_.emplace_back(cmd, blend_radius);
106}
107
108inline size_t Sequence::size() const
109{
110 return cmds_.size();
111}
112
113template <class T>
114inline T& Sequence::getCmd(const size_t index_cmd)
115{
116 return std::get<T>(cmds_.at(index_cmd).first);
117}
118
119template <class T>
120inline const T& Sequence::getCmd(const size_t index_cmd) const
121{
122 return std::get<T>(cmds_.at(index_cmd).first);
123}
124
125inline double Sequence::getBlendRadius(const size_t index_cmd) const
126{
127 return cmds_.at(index_cmd).second;
128}
129
130inline void Sequence::setBlendRadius(const size_t index_cmd, const double blend_radius)
131{
132 cmds_.at(index_cmd).second = blend_radius;
133}
134
136{
137 std::for_each(cmds_.begin(), cmds_.end(), [](TCmdRadiiPair& cmd) { cmd.second = 0.; });
138}
139
140template <class T>
141inline bool Sequence::cmdIsOfType(const size_t index_cmd) const
142{
143 return std::holds_alternative<T>(cmds_.at(index_cmd).first);
144}
145} // namespace pilz_industrial_motion_planner_testutils
Base class for commands storing all general information of a command.
Definition motioncmd.h:48
Data class storing all information regarding a Sequence command.
Definition sequence.h:54
size_t size() const
Returns the number of commands.
Definition sequence.h:108
bool cmdIsOfType(const size_t index_cmd) const
Definition sequence.h:141
void setBlendRadius(const size_t index_cmd, const double blend_radius)
Definition sequence.h:130
void erase(const size_t start, const size_t end)
Deletes all commands from index 'start' to index 'end'.
Definition sequence.cpp:94
void add(const CmdVariant &cmd, const double blend_radius=0.)
Adds a command to the end of the sequence.
Definition sequence.h:103
double getBlendRadius(const size_t index_cmd) const
Definition sequence.h:125
moveit_msgs::msg::MotionSequenceRequest toRequest() const
Definition sequence.cpp:67
std::variant< PtpJoint, PtpJointCart, PtpCart, LinJoint, LinCart, CircCenterCart, CircInterimCart, CircJointCenterCart, CircJointInterimCart, Gripper > CmdVariant