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

Add RosPositionCommandExecutor #173

Merged

gilwoolee merged 50 commits into master from rosBarrettHandExecutor

Apr 22, 2017

Contributor

Shushman commented Apr 15, 2017 •

edited

Loading

This PR introduces a ROS executor for commands to the Barrett Hand through aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp-cpp.

A point to note is that there is a lot of code (and logic) reuse between this and RosTrajectoryExecutor. Any way to make this more succinct?

gilwoolee and others added 17 commits

April 12, 2017 15:24


          Renaming, adding an abstract BarretHandPositionCommandExecutor class

8feb924


          Adding -override

811a63b


          Merge branch 'master' into SimBarrettHandCommandExecutor

dd96c10


          Exposing step

83f69dd


          Merge branch 'SimBarrettHandCommandExecutor' of https://github.com/pe…

cf8f104

…rsonalrobotics/aikido into SimBarrettHandCommandExecutor


          Renaiming PositionCommandExecutor

9d9fb3d


          Changing header files

648c2bf


          Changing names and step function.

a38c65a


          Merge branch 'master' into SimBarrettHandCommandExecutor

06dad13


          Update simulation speed constants and matrix->vector methods.

c6462db


          Add collisionDetector argument. Make collideWith an optional argument.

e16880e


          Don't reset mCollideWith every time.

ce86f93


          Simplify logic of optional collision detector and collision groups.

c90cd8d

Duplicate changes in BarrettHandKinematicSimulationPositionCommandExecutor.


          Add ShapeFrames of original collision group to new collision group.

2d3a5bc


          Adding RosBarrettHandPositionCommandExecutor to CMakeLists of aikido/…

c7c2040

…control/ros


          Adding conversion from goal positions and joint names to sensor_msgs/…

ea130a6

…JointState


          Ros executor for hand commands

eaff64c

Shushman requested review from mkoval and gilwoolee

April 15, 2017 21:04

mkoval requested changes

View reviewed changes

Member

mkoval left a comment

Major requests:

Eliminate the "duration" template arguments on the constructor (see my comment for a nuance here).
Inherit from the PositionCommandExecutor interface added in Adding an abstract BarrettHandPositionCommandExecutor class #166.
Eliminate the execute() overload that accepts a ros::Time unless I am wrong about SetPositionGoal lacking a Header.
Make jointNames a constructor argument, instead of an argument to execute().
Move waitForServer to a helper function (this probably should have been done when we first implemented, to be honest).

I also left a few nitpick comments.

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp Outdated

+                  const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20}
+                );
+                ~RosBarrettHandPositionCommandExecutor();

Member

mkoval Apr 15, 2017

Either make this virtual or mark the class as final. In this case, I'd prefer the former.

Contributor Author

Shushman Apr 17, 2017

Made virtual

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp Outdated

+                  const std::string& serverName,
+                  const DurationA& connectionTimeout = std::chrono::milliseconds{1000},
+                  const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20}
+                );

Member

mkoval Apr 15, 2017

Nit: I don't think there is any value in templateing DurationA and DurationB here, since we immediately store these values in std::chrono::milliseconds member variables. To make this work, we would have to template the entire class on these two types - which seems like it is distinctly not worth it.

I believe @jslee02, @gilwoolee, and I discussed this in a previous PR. Tagging them to see if they have anything to add.

Contributor Author

Shushman Apr 17, 2017

Removed template params and changed to std::chrono::milliseconds

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp Outdated

+                /// Sends positions to ROS server for execution.
+                /// \param[in] goalPositions Vector of 3 finger positions and 1 spread position
+                /// \param[in] jointNames Vector of 4 joint names for fingers
+                std::future<void> execute(Eigen::VectorXd& goalPositions,std::vector<std::string>& jointNames);

Member

mkoval Apr 15, 2017

Take goalPositions by const &.
Make jointNames a constructor argument, instead of making it an argument here. This is necessary to comply with the PositionCommandExecutor interface that @gilwoolee added in Adding an abstract BarrettHandPositionCommandExecutor class #166.

Contributor Author

Shushman Apr 17, 2017

Done both

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp Outdated

+              namespace ros {
+              /// This class sends commands for the hand position to the ROS server
+              class RosBarrettHandPositionCommandExecutor

Member

mkoval Apr 15, 2017

This should inherit from PositionCommandExecutor that is added in Adding an abstract BarrettHandPositionCommandExecutor class #166.
This class is not specific to the BarrettHand, so it needs to be renamed. I suggest RosPositionCommandExecutor to mimic RosTrajectoryExecutor.

Contributor Author

Shushman Apr 17, 2017

Changed name and is now inheriting

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp Outdated

+                std::future<void> execute(
+                  Eigen::VectorXd& goalPositions,
+                  std::vector<std::string>& jointNames,
+                  const ::ros::Time& startTime);

Member

mkoval Apr 15, 2017

The pr_control_msgs/SetPosition action doesn't have a time parameter, so this argument is not necessary. We should remove this function and roll everything into the execute(const Eigen::VectorXd&) overload.

Contributor Author

Shushman Apr 17, 2017

Removed - only a single execute method now

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp Outdated

+                , mConnectionTimeout{
+                  std::chrono::duration_cast<milliseconds>(connectionTimeout)}
+                , mConnectionPollingPeriod{
+                  std::chrono::duration_cast<milliseconds>(connectionPollingPeriod)}

Member

mkoval Apr 15, 2017

See my comment above about this cast. I'd prefer to eliminate the template parameters entirely and force the user to perform this conversion, if necessary.

If we do decide to keep the template parameters, then this implementation needs to be moved into a header file. By convention, it would go into a file named src/control/ros/detail/RosBarrettHandPositionCommandExecutor-impl.cpp.

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp Outdated

+                std::vector<std::string>& jointNames,
+                const ::ros::Time& startTime)
+              {
+                if (goalPositions.size() != 4)

Member

mkoval Apr 15, 2017

Replace the hard-coded value 4 with jointNames.size().

Contributor Author

Shushman Apr 17, 2017 •

edited

Loading

Actually delegated the check to the toJointState message in conversions

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp Outdated

+                // Convert goal positions and joint names to jointstate
+                goal.command = toJointState(goalPositions,jointNames);
+                goal.command.header.stamp = startTime;

Member

mkoval Apr 15, 2017

I don't think SetPositionGoal has a header. Are you sure this compiles?

Contributor Author

Shushman Apr 17, 2017

Removed as we no longer need startTime. But in general the command which is JointState does have a header.

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp Outdated

+              //=============================================================================
+              // TODO : This is directly reused from RosTrajectoryExecutor.cpp
+              // Anyway to reuse this? (assuming we want this here)

Member

mkoval Apr 15, 2017

I agree! Code duplication sucks. 😁

It should be straightforward to move this to a helper function:

template <class ActionSpec, class TimeoutDuration, class PeriodDuration>
bool waitForActionServer(
  actionlib::ActionClient<ActionSpec>& actionClient,
  ros::CallbackQueue* callbackQueue,
  TimeoutDuration timeoutDuration = std::chrono::seconds{ 1 },
  PeriodDuration periodDuration = std::chronos::milliseconds{ 10 }
);

Contributor Author

Shushman Apr 17, 2017

I've moved this to aikido/include/control/ros/util.hpp and am calling this directly with templates in execute.

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp Outdated

+              //=============================================================================
+              // TODO : This is also basically copied from RosTrajectoryExecutor
+              void RosBarrettHandPositionCommandExecutor::step()

Member

mkoval Apr 15, 2017

I think that's fine in for now. We may want to write a base class for actionnlib-based executors in the future, but there really isn't all that much duplicated code once we move waitForServer to a helper function.

brianhou added 8 commits

April 15, 2017 17:59


          Make collisionDetector, collideWith optional.

d9d36fc


          Instantiate FingerPositionCommandExecutors and FingerSpreadCommandExe…

3af8671

…cutor in HandPositionCommandExecutor.

Adapted from https://github.com/personalrobotics/libherb/pull/20.


          Clean up some documentation.


          Remove parameter from step.

6ecb30e


          Set mCollisionDetector when setting mCollideWith.

c3eb6f3


          Make FingerCommandExecutors implement PositionCommandExecutor.

47a9852


          Swap argument order in HandPositionExecutor.

bebb761


          Partial test fixes.

f1d805f

Make test_BarrettFinger* compile (although actual execution seems to fail).
Make part of test_BarrettHand compile, although introducing `robot` as a
parameter seems like it will make testing this difficult.

mkoval changed the title ~~Ros barrett hand executor~~ Add RosPositionCommandExecutor


          Adding time-reset when a new execute command is given.

fc7cd1c

Shushman Choudhury added 8 commits

April 17, 2017 11:09


          Basing this branch off SimBarrettHandCommandExecutor as it depends on it

cc709a2

Merge remote-tracking branch 'origin/SimBarrettHandCommandExecutor' into rosBarrettHandExecutor


          Renamed RosBarrettHandPositionCommandExecutor to RosPositionCommandEx…

90bb1c9

…ecutor; moved jointNames to constructor, made it inherit from PositionCommandExecutor


          Changed methods to match header file changes, addressed Mike's other …

db9bd59

…comments:


          Edited CMakeLists of control/ros to reflect new filename

4fc36d6


          Edited the toJointState method

5a1059a


          Created waitForActionServer helper function

dc53d76


          Deleted renamed files

d3b1647


          No longer blocked by #166

1bd2834

Merge branch 'master' into rosBarrettHandExecutor

jslee02 added this to the Aikido 0.1.0 milestone

mkoval requested changes

View reviewed changes

Member

mkoval left a comment

This generally looks great! One last round of very minor comments and I am happy to merge this:

Write a docstring for waitForActionServer.
Move the implementation of waitForActionServer to a separate header file.
Fix the bug I commented on in waitForActionServer.
Improve the docstring for RosPositionCommandExecutor.

👍

include/aikido/control/ros/Conversions.hpp

               trajectory_msgs::JointTrajectory toRosJointTrajectory(
                 const aikido::trajectory::TrajectoryPtr& trajectory, double timestep);
+              /// Converts Eigen VectorXd and joint names to JointState
+              /// \param[in] goalPositions The required positions for the fingers

Member

mkoval Apr 18, 2017

Nit: This is not specific to finger joints! 😉

Also, we should add "position" to the name of this function, since we may very well need equivalent functions for velocities and/or efforts. Perhaps positionsToJointState?

include/aikido/control/ros/RosPositionCommandExecutor.hpp Outdated

+              namespace control {
+              namespace ros {
+              /// This class sends commands for the hand position to the ROS server

Member

mkoval Apr 18, 2017

Nit: This comment is not very helpful. At a minimum, we should specify that this class uses actionlib to command a pr_control_msgs/SetPosition action.

Member

mkoval Apr 21, 2017

This is not resolved.

include/aikido/control/ros/RosPositionCommandExecutor.hpp Outdated

+              public:
+                /// Constructor
+                /// \param[in] node ROS node handle for action client.
+                /// \param[in] serverName Name of the server to send traejctory to.

Member

mkoval Apr 18, 2017

Typo: "traejctory "

include/aikido/control/ros/RosPositionCommandExecutor.hpp Outdated

+                  const std::string& serverName,
+                  const std::vector<std::string>& jointNames,
+                  const std::chrono::milliseconds connectionTimeout = std::chrono::milliseconds{1000},
+                  const std::chrono::milliseconds connectionPollingPeriod = std::chrono::milliseconds{20}

Member

mkoval Apr 18, 2017

Nit: Const is meaningless on a parameter passed by value in a function declaration.

include/aikido/control/ros/util.hpp Outdated

+                const auto startTime = Clock::now();
+                const auto endTime = startTime + timeoutDuration;
+                auto currentTime = startTime + periodDuration;

Member

mkoval Apr 18, 2017

I think this should be initialized to startTime.

Contributor Author

Shushman Apr 18, 2017

I based it off this :
https://github.com/personalrobotics/aikido/blob/master/src/control/ros/RosTrajectoryExecutor.cpp#L157
Can you help me figure it out? @gilwoolee any comments?

include/aikido/control/ros/util.hpp Outdated

+                ::ros::CallbackQueue& callbackQueue,
+                TimeoutDuration timeoutDuration = std::chrono::milliseconds{ 1000 },
+                PeriodDuration periodDuration = std::chrono::milliseconds{ 10 }
+              )

Member

mkoval Apr 18, 2017

This function needs a docstring. Also, it should only be declared in the util.hpp file. The implementation should be #included from a detail/util-impl.hpp file to match the convention we follow inn the rest of Aikido.

src/control/ros/Conversions.cpp Outdated

+                {
+                  std::stringstream message;
+                  message << "The size of goalPositions ("<<goalPositions.size()<<
+                    ") must be the same as jointNames ("<<jointNames.size()<<"!" ;

Member

mkoval Apr 18, 2017

There are mismatched parens in this error message.

src/control/ros/Conversions.cpp


		sensor_msgs::JointState jointState;

		jointState.name = std::move(jointNames);

Member

mkoval Apr 18, 2017

Nit: It is not possible to std::move out of jointNames because it was passed by const reference. You should either remove the std::move or modify the function to take jointNames by value. I'd slightly prefer the latter.

Member

mkoval Apr 21, 2017

This is now resolved.

src/control/ros/RosPositionCommandExecutor.cpp Outdated

+              RosPositionCommandExecutor::RosPositionCommandExecutor(
+                    ::ros::NodeHandle node,
+                    const std::string& serverName,
+                    const std::vector<std::string>& jointNames,

Member

mkoval Apr 18, 2017

Nit: Take jointNames by value and std::move it below.

Contributor Author

Shushman Apr 18, 2017

I removed the const as well after looking up std::move

Member

mkoval Apr 21, 2017

It looks like this is still not resolved.

src/control/ros/RosPositionCommandExecutor.cpp Outdated

+              RosPositionCommandExecutor::~RosPositionCommandExecutor()
+              {
+                // Do nothing.
+                // TODO: Should we wait for the current trajectory to finish executing?

Member

mkoval Apr 18, 2017

Nit: This comment was copied and pasted from RosTrajectoryExecutor. There are no trajectories in this file! 😁

Shushman Choudhury and others added 4 commits

April 18, 2017 11:38


          Adding detail/util-impl in the necessary convention

5e18e73


          Taking joint names by non-const value

971bb6a


          Addressed rest of Mike

35d8144

's nits


          Merge branch 'master' into rosBarrettHandExecutor

571bc22

mkoval requested changes

View reviewed changes

include/aikido/control/ros/detail/util-impl.hpp Outdated

+                const auto startTime = Clock::now();
+                const auto endTime = startTime + timeoutDuration;
+                auto currentTime = startTime + periodDuration;

Member

mkoval Apr 21, 2017

This should not have periodDuration added onto it. Otherwise, the first iteration will have twice the expected period.

src/control/ros/Conversions.cpp


		sensor_msgs::JointState jointState;

		jointState.name = std::move(jointNames);

Member

mkoval Apr 21, 2017

This is now resolved.

src/control/ros/RosPositionCommandExecutor.cpp Outdated

+              RosPositionCommandExecutor::RosPositionCommandExecutor(
+                    ::ros::NodeHandle node,
+                    const std::string& serverName,
+                    const std::vector<std::string>& jointNames,

Member

mkoval Apr 21, 2017

It looks like this is still not resolved.

include/aikido/control/ros/RosPositionCommandExecutor.hpp Outdated

+              namespace control {
+              namespace ros {
+              /// This class sends commands for the hand position to the ROS server

Member

mkoval Apr 21, 2017

This is not resolved.

Shushman Choudhury added 5 commits

April 20, 2017 23:14


          Better docstring for class

b89672b


          Fixed incorrect currentTime


          Taking joint names by non-const value in constructor

45d632b


          Merge branch 'rosBarrettHandExecutor' of https://github.com/personalr…

5e38013

…obotics/aikido into rosBarrettHandExecutor


          Renaming toJointState to positionsToJointState

83a41fd

mkoval approved these changes

View reviewed changes

Member

mkoval left a comment

LGTM. Thanks for making those changes @Shushman!

Shushman Choudhury and others added 5 commits

April 21, 2017 00:02


          Fixing signed with unsigned check

e18ab71


          Resolve compilation issues:

0ecb2b9

- Use find module for pr_control_msgs
- Fix unmatched function signature in Conversions
- Fix function termination without return value
- Fix unordered initialization list


          Remove debug code

cd516a2


          Add pr_control_msgs to package.xml

061e676


          Clean up ros/CMakeLists.txt

7474bea

coveralls commented Apr 22, 2017

Coverage decreased (-1.2%) to 79.163% when pulling 7474bea on rosBarrettHandExecutor into 2af5c55 on master.

gilwoolee merged commit 5d19317 into master

gilwoolee deleted the rosBarrettHandExecutor branch

April 22, 2017 18:37

Shushman mentioned this pull request

Using helper function for waiting for action server #192

Merged

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet