Skip to content

Commit

Permalink
Merge branch 'main' into pr-delete_untagged_images
Browse files Browse the repository at this point in the history
  • Loading branch information
Vatan Aksoy Tezer authored Nov 9, 2021
2 parents b143fce + f96c84c commit e45df23
Show file tree
Hide file tree
Showing 12 changed files with 254 additions and 59 deletions.
128 changes: 105 additions & 23 deletions moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
#include <deque>
#include <memory>

#include "rcl/error_handling.h"
#include "rcl/time.h"
Expand All @@ -58,8 +59,15 @@ MOVEIT_CLASS_FORWARD(RobotTrajectory); // Defines RobotTrajectoryPtr, ConstPtr,
class RobotTrajectory
{
public:
/** @brief construct a trajectory */
explicit RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model);

/** @brief construct a trajectory for the named JointModelGroup */
RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group);

/** @brief construct a trajectory for the JointModelGroup
* If group is nullptr this is equivalent to the first constructor
*/
RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const moveit::core::JointModelGroup* group);

/** Assignment operator, performing a shallow copy, i.e. copying waypoints by pointer */
Expand All @@ -83,13 +91,23 @@ class RobotTrajectory

const std::string& getGroupName() const;

void setGroupName(const std::string& group_name);
RobotTrajectory& setGroupName(const std::string& group_name)
{
group_ = robot_model_->getJointModelGroup(group_name);
return *this;
}

std::size_t getWayPointCount() const
{
return waypoints_.size();
}

std::size_t size() const
{
assert(waypoints_.size() == duration_from_previous_.size());
return waypoints_.size();
}

const moveit::core::RobotState& getWayPoint(std::size_t index) const
{
return *waypoints_[index];
Expand Down Expand Up @@ -141,11 +159,12 @@ class RobotTrajectory
return 0.0;
}

void setWayPointDurationFromPrevious(std::size_t index, double value)
RobotTrajectory& setWayPointDurationFromPrevious(std::size_t index, double value)
{
if (duration_from_previous_.size() <= index)
duration_from_previous_.resize(index + 1, 0.0);
duration_from_previous_[index] = value;
return *this;
}

bool empty() const
Expand All @@ -158,45 +177,48 @@ class RobotTrajectory
* \param state - current robot state
* \param dt - duration from previous
*/
void addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotState& state, double dt)
{
addSuffixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return addSuffixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
}

/**
* \brief Add a point to the trajectory
* \param state - current robot state
* \param dt - duration from previous
*/
void addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& addSuffixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.push_back(state);
duration_from_previous_.push_back(dt);
return *this;
}

void addPrefixWayPoint(const moveit::core::RobotState& state, double dt)
RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotState& state, double dt)
{
addPrefixWayPoint(moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return addPrefixWayPoint(std::make_shared<moveit::core::RobotState>(state), dt);
}

void addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& addPrefixWayPoint(const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.push_front(state);
duration_from_previous_.push_front(dt);
return *this;
}

void insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotState& state, double dt)
{
insertWayPoint(index, moveit::core::RobotStatePtr(new moveit::core::RobotState(state)), dt);
return insertWayPoint(index, std::make_shared<moveit::core::RobotState>(state), dt);
}

void insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
RobotTrajectory& insertWayPoint(std::size_t index, const moveit::core::RobotStatePtr& state, double dt)
{
state->update();
waypoints_.insert(waypoints_.begin() + index, state);
duration_from_previous_.insert(duration_from_previous_.begin() + index, dt);
return *this;
}

/**
Expand All @@ -209,12 +231,17 @@ class RobotTrajectory
* \param end_index - index of last traj point of the part to append from the source traj, the default is to add until
* the end of the source traj
*/
void append(const RobotTrajectory& source, double dt, size_t start_index = 0,
size_t end_index = std::numeric_limits<std::size_t>::max());
RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0,
size_t end_index = std::numeric_limits<std::size_t>::max());

void swap(robot_trajectory::RobotTrajectory& other);

void clear();
RobotTrajectory& clear()
{
waypoints_.clear();
duration_from_previous_.clear();
return *this;
}

double getDuration() const;

Expand All @@ -229,17 +256,17 @@ class RobotTrajectory
point in the trajectory
to be constructed internally is obtained by copying the reference state and overwriting the content from a
trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::msg::JointTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::msg::JointTrajectory& trajectory);

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
for all joints. For this reason a full starting state must be specified as reference (\e reference_state). Each
point in the trajectory
to be constructed internally is obtained by copying the reference state and overwriting the content from a
trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotTrajectory& trajectory);

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
Expand All @@ -248,13 +275,14 @@ class RobotTrajectory
using \e state. Each point in the trajectory to be constructed internally is obtained by copying the reference
state and overwriting the content
from a trajectory point in \e trajectory. */
void setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state, const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::RobotTrajectory& trajectory);
RobotTrajectory& setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::RobotTrajectory& trajectory);

void reverse();
RobotTrajectory& reverse();

void unwind();
void unwind(const moveit::core::RobotState& state);
RobotTrajectory& unwind();
RobotTrajectory& unwind(const moveit::core::RobotState& state);

/** @brief Finds the waypoint indicies before and after a duration from start.
* @param The duration from start.
Expand All @@ -273,6 +301,60 @@ class RobotTrajectory
*/
bool getStateAtDurationFromStart(const double request_duration, moveit::core::RobotStatePtr& output_state) const;

class Iterator
{
std::deque<moveit::core::RobotStatePtr>::iterator waypoint_iterator;
std::deque<double>::iterator duration_iterator;

public:
explicit Iterator(std::deque<moveit::core::RobotStatePtr>::iterator _waypoint_iterator,
std::deque<double>::iterator _duration_iterator)
: waypoint_iterator(_waypoint_iterator), duration_iterator(_duration_iterator)
{
}
Iterator& operator++()
{
waypoint_iterator++;
duration_iterator++;
return *this;
}
Iterator operator++(int)
{
Iterator retval = *this;
++(*this);
return retval;
}
bool operator==(Iterator other) const
{
return ((waypoint_iterator == other.waypoint_iterator) && (duration_iterator == other.duration_iterator));
}
bool operator!=(Iterator other) const
{
return !(*this == other);
}
std::pair<moveit::core::RobotStatePtr, double> operator*() const
{
return std::pair{ *waypoint_iterator, *duration_iterator };
}

// iterator traits
using difference_type = long;
using value_type = std::pair<moveit::core::RobotStatePtr, double>;
using pointer = const std::pair<moveit::core::RobotStatePtr, double>*;
using reference = std::pair<moveit::core::RobotStatePtr, double>;
using iterator_category = std::input_iterator_tag;
};

RobotTrajectory::Iterator begin()
{
assert(waypoints_.size() == duration_from_previous_.size());
return Iterator(waypoints_.begin(), duration_from_previous_.begin());
}
RobotTrajectory::Iterator end()
{
assert(waypoints_.size() == duration_from_previous_.size());
return Iterator(waypoints_.end(), duration_from_previous_.end());
}
/** @brief Print information about the trajectory
* @param out Stream to print to
* @param variable_indexes The indexes of the variables to print.
Expand Down
57 changes: 31 additions & 26 deletions moveit_core/robot_trajectory/src/robot_trajectory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,6 +47,11 @@

namespace robot_trajectory
{
RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model)
: robot_model_(robot_model), group_(nullptr)
{
}

RobotTrajectory::RobotTrajectory(const moveit::core::RobotModelConstPtr& robot_model, const std::string& group)
: robot_model_(robot_model), group_(group.empty() ? nullptr : robot_model->getJointModelGroup(group))
{
Expand All @@ -71,11 +76,6 @@ RobotTrajectory::RobotTrajectory(const RobotTrajectory& other, bool deepcopy)
}
}

void RobotTrajectory::setGroupName(const std::string& group_name)
{
group_ = robot_model_->getJointModelGroup(group_name);
}

const std::string& RobotTrajectory::getGroupName() const
{
if (group_)
Expand Down Expand Up @@ -105,11 +105,11 @@ void RobotTrajectory::swap(RobotTrajectory& other)
duration_from_previous_.swap(other.duration_from_previous_);
}

void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t start_index, size_t end_index)
RobotTrajectory& RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t start_index, size_t end_index)
{
end_index = std::min(end_index, source.waypoints_.size());
if (start_index >= end_index)
return;
return *this;
waypoints_.insert(waypoints_.end(), std::next(source.waypoints_.begin(), start_index),
std::next(source.waypoints_.begin(), end_index));
std::size_t index = duration_from_previous_.size();
Expand All @@ -118,9 +118,11 @@ void RobotTrajectory::append(const RobotTrajectory& source, double dt, size_t st
std::next(source.duration_from_previous_.begin(), end_index));
if (duration_from_previous_.size() > index)
duration_from_previous_[index] += dt;

return *this;
}

void RobotTrajectory::reverse()
RobotTrajectory& RobotTrajectory::reverse()
{
std::reverse(waypoints_.begin(), waypoints_.end());
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
Expand All @@ -134,12 +136,14 @@ void RobotTrajectory::reverse()
std::reverse(duration_from_previous_.begin(), duration_from_previous_.end());
duration_from_previous_.pop_back();
}

return *this;
}

void RobotTrajectory::unwind()
RobotTrajectory& RobotTrajectory::unwind()
{
if (waypoints_.empty())
return;
return *this;

const std::vector<const moveit::core::JointModel*>& cont_joints =
group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
Expand Down Expand Up @@ -169,12 +173,14 @@ void RobotTrajectory::unwind()
}
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
waypoint->update();

return *this;
}

void RobotTrajectory::unwind(const moveit::core::RobotState& state)
RobotTrajectory& RobotTrajectory::unwind(const moveit::core::RobotState& state)
{
if (waypoints_.empty())
return;
return *this;

const std::vector<const moveit::core::JointModel*>& cont_joints =
group_ ? group_->getContinuousJointModels() : robot_model_->getContinuousJointModels();
Expand Down Expand Up @@ -215,12 +221,8 @@ void RobotTrajectory::unwind(const moveit::core::RobotState& state)
}
for (moveit::core::RobotStatePtr& waypoint : waypoints_)
waypoint->update();
}

void RobotTrajectory::clear()
{
waypoints_.clear();
duration_from_previous_.clear();
return *this;
}

void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory,
Expand Down Expand Up @@ -357,11 +359,11 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
}
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::msg::JointTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const trajectory_msgs::msg::JointTrajectory& trajectory)
{
// make a copy just in case the next clear() removes the memory for the reference passed in
const moveit::core::RobotState& copy = reference_state;
const moveit::core::RobotState copy(reference_state); // NOLINT
clear();
std::size_t state_count = trajectory.points.size();
rclcpp::Time last_time_stamp = trajectory.header.stamp;
Expand All @@ -381,10 +383,12 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).seconds());
last_time_stamp = this_time_stamp;
}

return *this;
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotTrajectory& trajectory)
{
// make a copy just in case the next clear() removes the memory for the reference passed in
const moveit::core::RobotState& copy = reference_state;
Expand Down Expand Up @@ -428,15 +432,16 @@ void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& refe
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).seconds());
last_time_stamp = this_time_stamp;
}
return *this;
}

void RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::RobotTrajectory& trajectory)
RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::RobotState& reference_state,
const moveit_msgs::msg::RobotState& state,
const moveit_msgs::msg::RobotTrajectory& trajectory)
{
moveit::core::RobotState st(reference_state);
moveit::core::robotStateMsgToRobotState(state, st);
setRobotTrajectoryMsg(st, trajectory);
return setRobotTrajectoryMsg(st, trajectory);
}

void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double& duration, int& before, int& after,
Expand Down
Loading

0 comments on commit e45df23

Please sign in to comment.