Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Port robot_trajectory to ROS2 #39

Merged
merged 9 commits into from
May 21, 2019
Merged

Port robot_trajectory to ROS2 #39

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
80ee295
Port robot_trajectory to ROS2
vmayoral Mar 10, 2019
60a8e9c
robot_trajectory, use static_cast
vmayoral Mar 16, 2019
e6e7a0b
Compress the use of .seconds()
vmayoral Mar 16, 2019
9cb2a18
robot_trajectory, clang format
vmayoral Mar 16, 2019
c08749e
udpating robot_trajectory CMakeLists.txt
ahcorde May 6, 2019
3293633
Restoring accidently removed code and fix time convertions
ahcorde May 6, 2019
f0c522b
Robot_trajectory - constructor of rclcpp::Duration more readable
ahcorde May 16, 2019
ff4b888
Update moveit_core/robot_trajectory/src/robot_trajectory.cpp
May 16, 2019
5936b7a
Update moveit_core/robot_trajectory/src/robot_trajectory.cpp
May 16, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 13 additions & 7 deletions moveit_core/robot_trajectory/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,19 @@
set(MOVEIT_LIB_NAME moveit_robot_trajectory)

add_library(${MOVEIT_LIB_NAME} src/robot_trajectory.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
add_library(${MOVEIT_LIB_NAME} SHARED src/robot_trajectory.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})

target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_robot_state ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
target_link_libraries(${MOVEIT_LIB_NAME}
moveit_robot_model
moveit_robot_state
${rclcpp_LIBRARIES}
${rmw_implementation_LIBRARIES}
${urdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
${Boost_LIBRARIES})

install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
ARCHIVE DESTINATION lib
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
LIBRARY DESTINATION lib)

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/ DESTINATION include)
18 changes: 13 additions & 5 deletions moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,17 @@
#include <moveit/macros/class_forward.h>
#include <moveit/macros/deprecation.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit_msgs/RobotTrajectory.h>
#include <moveit_msgs/RobotState.h>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
#include <deque>

#include "rcl/error_handling.h"
#include "rcl/time.h"
#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/time.hpp"
#include "rclcpp/utilities.hpp"

namespace robot_trajectory
{
MOVEIT_CLASS_FORWARD(RobotTrajectory);
Expand Down Expand Up @@ -204,7 +211,7 @@ class RobotTrajectory

double getAverageSegmentDuration() const;

void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory) const;
void getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory);
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
Expand All @@ -213,7 +220,7 @@ class RobotTrajectory
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 robot_state::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory);
const trajectory_msgs::msg::JointTrajectory& trajectory);
vmayoral marked this conversation as resolved.
Show resolved Hide resolved

/** \brief Copy the content of the trajectory message into this class. The trajectory message itself is not required
to contain the values
Expand Down Expand Up @@ -261,7 +268,8 @@ class RobotTrajectory
const robot_model::JointModelGroup* group_;
std::deque<robot_state::RobotStatePtr> waypoints_;
std::deque<double> duration_from_previous_;
rclcpp::Clock clock_ros_;
};
}
} // namespace robot_trajectory

#endif
55 changes: 34 additions & 21 deletions moveit_core/robot_trajectory/src/robot_trajectory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <tf2_eigen/tf2_eigen.h>
#include <boost/math/constants/constants.hpp>
#include <numeric>
#include "rclcpp/rclcpp.hpp"

namespace robot_trajectory
{
Expand Down Expand Up @@ -201,9 +202,12 @@ void RobotTrajectory::clear()
duration_from_previous_.clear();
}

void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory) const
vmayoral marked this conversation as resolved.
Show resolved Hide resolved
void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& trajectory)
{
trajectory = moveit_msgs::msg::RobotTrajectory();

builtin_interfaces::msg::Time stamp = clock_ros_.now();

if (waypoints_.empty())
return;
const std::vector<const robot_model::JointModel*>& jnt =
Expand All @@ -228,24 +232,27 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
if (!onedof.empty())
{
trajectory.joint_trajectory.header.frame_id = robot_model_->getModelFrame();
trajectory.joint_trajectory.header.stamp = ros::Time(0);
trajectory.joint_trajectory.header.stamp = stamp;
trajectory.joint_trajectory.points.resize(waypoints_.size());
}

if (!mdof.empty())
{
trajectory.multi_dof_joint_trajectory.header.frame_id = robot_model_->getModelFrame();
trajectory.multi_dof_joint_trajectory.header.stamp = ros::Time(0);
trajectory.multi_dof_joint_trajectory.header.stamp = stamp;
trajectory.multi_dof_joint_trajectory.points.resize(waypoints_.size());
}

static const ros::Duration ZERO_DURATION(0.0);
static const rclcpp::Duration ZERO_DURATION(0.0);
double total_time = 0.0;
rclcpp::Duration dur_total(0, 0);
for (std::size_t i = 0; i < waypoints_.size(); ++i)
{
if (duration_from_previous_.size() > i)
total_time += duration_from_previous_[i];

dur_total = rclcpp::Duration(1, 0) * total_time;

if (!onedof.empty())
{
trajectory.joint_trajectory.points[i].positions.resize(onedof.size());
Expand Down Expand Up @@ -277,7 +284,8 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
trajectory.joint_trajectory.points[i].effort.clear();

if (duration_from_previous_.size() > i)
trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(total_time);

trajectory.joint_trajectory.points[i].time_from_start = dur_total;
else
trajectory.joint_trajectory.points[i].time_from_start = ZERO_DURATION;
}
Expand All @@ -286,15 +294,15 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
trajectory.multi_dof_joint_trajectory.points[i].transforms.resize(mdof.size());
for (std::size_t j = 0; j < mdof.size(); ++j)
{
geometry_msgs::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
geometry_msgs::msg::TransformStamped ts = tf2::eigenToTransform(waypoints_[i]->getJointTransform(mdof[j]));
trajectory.multi_dof_joint_trajectory.points[i].transforms[j] = ts.transform;
// TODO: currently only checking for planar multi DOF joints / need to add check for floating
if (waypoints_[i]->hasVelocities() && (mdof[j]->getType() == robot_model::JointModel::JointType::PLANAR))
{
const std::vector<std::string> names = mdof[j]->getVariableNames();
const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]);

geometry_msgs::Twist point_velocity;
geometry_msgs::msg::Twist point_velocity;

for (std::size_t k = 0; k < names.size(); ++k)
{
Expand All @@ -319,26 +327,30 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t
}
}
if (duration_from_previous_.size() > i)
trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(total_time);
trajectory.multi_dof_joint_trajectory.points[i].time_from_start = dur_total;
else
trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ZERO_DURATION;
}
}
}

void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
const trajectory_msgs::JointTrajectory& trajectory)
const trajectory_msgs::msg::JointTrajectory& trajectory)
{
// make a copy just in case the next clear() removes the memory for the reference passed in

const robot_state::RobotState& copy = reference_state;
clear();
std::size_t state_count = trajectory.points.size();
ros::Time last_time_stamp = trajectory.header.stamp;
ros::Time this_time_stamp = last_time_stamp;
rclcpp::Time last_time_stamp = trajectory.header.stamp;
rclcpp::Time this_time_stamp = last_time_stamp;

rclcpp::Time traj_stamp = trajectory.header.stamp;
rclcpp::Duration dur_from_start(0, 0);

for (std::size_t i = 0; i < state_count; ++i)
{
this_time_stamp = trajectory.header.stamp + trajectory.points[i].time_from_start;
this_time_stamp = traj_stamp + trajectory.points[i].time_from_start;
robot_state::RobotStatePtr st(new robot_state::RobotState(copy));
st->setVariablePositions(trajectory.joint_names, trajectory.points[i].positions);
if (!trajectory.points[i].velocities.empty())
Expand All @@ -347,24 +359,25 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer
st->setVariableAccelerations(trajectory.joint_names, trajectory.points[i].accelerations);
if (!trajectory.points[i].effort.empty())
st->setVariableEffort(trajectory.joint_names, trajectory.points[i].effort);
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).seconds());
last_time_stamp = this_time_stamp;
}
}

void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& reference_state,
const moveit_msgs::msg::RobotTrajectory& trajectory)
{
geometry_msgs::msg::TransformStamped tf_stamped;
// make a copy just in case the next clear() removes the memory for the reference passed in
const robot_state::RobotState& copy = reference_state;
clear();

std::size_t state_count =
std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
ros::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
trajectory.multi_dof_joint_trajectory.header.stamp :
trajectory.joint_trajectory.header.stamp;
ros::Time this_time_stamp = last_time_stamp;
rclcpp::Time last_time_stamp = trajectory.joint_trajectory.points.empty() ?
trajectory.multi_dof_joint_trajectory.header.stamp :
trajectory.joint_trajectory.header.stamp;
rclcpp::Time this_time_stamp = last_time_stamp;

for (std::size_t i = 0; i < state_count; ++i)
{
Expand All @@ -381,8 +394,8 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer
trajectory.joint_trajectory.points[i].accelerations);
if (!trajectory.joint_trajectory.points[i].effort.empty())
st->setVariableEffort(trajectory.joint_trajectory.joint_names, trajectory.joint_trajectory.points[i].effort);
this_time_stamp =
trajectory.joint_trajectory.header.stamp + trajectory.joint_trajectory.points[i].time_from_start;
this_time_stamp = rclcpp::Time(trajectory.joint_trajectory.header.stamp) +
trajectory.joint_trajectory.points[i].time_from_start;
}
if (trajectory.multi_dof_joint_trajectory.points.size() > i)
{
Expand All @@ -391,11 +404,11 @@ void RobotTrajectory::setRobotTrajectoryMsg(const robot_state::RobotState& refer
Eigen::Isometry3d t = tf2::transformToEigen(trajectory.multi_dof_joint_trajectory.points[i].transforms[j]);
st->setJointPositions(trajectory.multi_dof_joint_trajectory.joint_names[j], t);
}
this_time_stamp = trajectory.multi_dof_joint_trajectory.header.stamp +
this_time_stamp = rclcpp::Time(trajectory.multi_dof_joint_trajectory.header.stamp) +
trajectory.multi_dof_joint_trajectory.points[i].time_from_start;
}

addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).toSec());
addSuffixWayPoint(st, (this_time_stamp - last_time_stamp).seconds());
last_time_stamp = this_time_stamp;
}
}
Expand Down