-
Notifications
You must be signed in to change notification settings - Fork 90
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
8 changed files
with
310 additions
and
8 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
87 changes: 87 additions & 0 deletions
87
include/ur_client_library/control/trajectory_point_interface.h
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,87 @@ | ||
| // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- | ||
|
|
||
| // -- BEGIN LICENSE BLOCK ---------------------------------------------- | ||
| // Copyright 2021 FZI Forschungszentrum Informatik | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // -- END LICENSE BLOCK ------------------------------------------------ | ||
|
|
||
| //---------------------------------------------------------------------- | ||
| /*!\file | ||
| * | ||
| * \author Felix Exner mauch@fzi.de | ||
| * \date 2021-06-01 | ||
| * | ||
| */ | ||
| //---------------------------------------------------------------------- | ||
|
|
||
| #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED | ||
| #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED | ||
|
|
||
| #include "ur_client_library/control/reverse_interface.h" | ||
| #include "ur_client_library/comm/control_mode.h" | ||
| #include "ur_client_library/types.h" | ||
| #include "ur_client_library/log.h" | ||
|
|
||
| namespace urcl | ||
| { | ||
| namespace control | ||
| { | ||
| /*! | ||
| * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full | ||
| * trajectories are forwarded to the robot controller and are executed there. | ||
| */ | ||
| class TrajectoryPointInterface : public ReverseInterface | ||
| { | ||
| public: | ||
| static const int32_t MULT_TIME = 1000; | ||
| static const int32_t JOINT_POINT = 0; | ||
| static const int32_t CARTESIAN_POINT = 1; | ||
|
|
||
| TrajectoryPointInterface() = delete; | ||
| /*! | ||
| * \brief Creates a TrajectoryPointInterface object including a TCPServer. | ||
| * | ||
| * \param port Port the Server is started on | ||
| */ | ||
| TrajectoryPointInterface(uint32_t port); | ||
|
|
||
| /*! | ||
| * \brief Disconnects possible clients so the reverse interface object can be safely destroyed. | ||
| */ | ||
| virtual ~TrajectoryPointInterface() = default; | ||
|
|
||
|
|
||
| /*! | ||
| * \brief Writes needed information to the robot to be read by the URScript program. | ||
| * | ||
| * \param positions A vector of joint or cartesian targets for the robot | ||
| * \param time The goal time to reach the target | ||
| * | ||
| * \returns True, if the write was performed successfully, false otherwise. | ||
| */ | ||
| bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius, | ||
| const bool cartesian); | ||
|
|
||
| protected: | ||
| virtual void connectionCallback(const int filedescriptor) override; | ||
|
|
||
| virtual void disconnectionCallback(const int filedescriptor) override; | ||
|
|
||
| virtual void messageCallback(const int filedescriptor, char* buffer) override; | ||
| }; | ||
|
|
||
| } // namespace control | ||
| } // namespace urcl | ||
|
|
||
| #endif // UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,113 @@ | ||
| // this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- | ||
|
|
||
| // -- BEGIN LICENSE BLOCK ---------------------------------------------- | ||
| // Copyright 2021 FZI Forschungszentrum Informatik | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
| // -- END LICENSE BLOCK ------------------------------------------------ | ||
|
|
||
| //---------------------------------------------------------------------- | ||
| /*!\file | ||
| * | ||
| * \author Felix Exner mauch@fzi.de | ||
| * \date 2021-06-01 | ||
| * | ||
| */ | ||
| //---------------------------------------------------------------------- | ||
|
|
||
| #include <ur_client_library/control/trajectory_point_interface.h> | ||
|
|
||
| namespace urcl | ||
| { | ||
| namespace control | ||
| { | ||
| TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInterface(port, [](bool foo) { return foo; }) | ||
| { | ||
| } | ||
|
|
||
| bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, | ||
| const float blend_radius, const bool cartesian) | ||
| { | ||
| if (client_fd_ == -1) | ||
| { | ||
| return false; | ||
| } | ||
| uint8_t buffer[sizeof(int32_t) * 9]; | ||
| uint8_t* b_pos = buffer; | ||
|
|
||
| if (positions != nullptr) | ||
| { | ||
| for (auto const& pos : *positions) | ||
| { | ||
| int32_t val = static_cast<int32_t>(pos * MULT_JOINTSTATE); | ||
| val = htobe32(val); | ||
| b_pos += append(b_pos, val); | ||
| } | ||
| } | ||
| else | ||
| { | ||
| b_pos += 6 * sizeof(int32_t); | ||
| } | ||
|
|
||
| int32_t val = static_cast<int32_t>(goal_time * MULT_TIME); | ||
| val = htobe32(val); | ||
| b_pos += append(b_pos, val); | ||
|
|
||
| val = static_cast<int32_t>(blend_radius * MULT_TIME); | ||
| val = htobe32(val); | ||
| b_pos += append(b_pos, val); | ||
|
|
||
| if (cartesian) | ||
| { | ||
| val = CARTESIAN_POINT; | ||
| } | ||
| else | ||
| { | ||
| val = JOINT_POINT; | ||
| } | ||
|
|
||
| val = htobe32(val); | ||
| b_pos += append(b_pos, val); | ||
|
|
||
| size_t written; | ||
|
|
||
| return server_.write(client_fd_, buffer, sizeof(buffer), written); | ||
| } | ||
|
|
||
| void TrajectoryPointInterface::connectionCallback(const int filedescriptor) | ||
| { | ||
| if (client_fd_ < 0) | ||
| { | ||
| URCL_LOG_DEBUG("Robot connected to trajectory interface."); | ||
| client_fd_ = filedescriptor; | ||
| } | ||
| else | ||
| { | ||
| URCL_LOG_ERROR("Connection request to TrajectoryPointInterface received while connection already established. Only one " | ||
| "connection is allowed at a time. Ignoring this request."); | ||
| } | ||
| } | ||
|
|
||
| void TrajectoryPointInterface::disconnectionCallback(const int filedescriptor) | ||
| { | ||
| URCL_LOG_DEBUG("Connection to trajectory interface dropped.", filedescriptor); | ||
| client_fd_ = -1; | ||
| } | ||
|
|
||
| void TrajectoryPointInterface::messageCallback(const int filedescriptor, char* buffer) | ||
| { | ||
| // TODO: Send up. This will be things like "finished trajectory" | ||
| URCL_LOG_INFO("Received message %s on TrajectoryPointInterface", buffer); | ||
| } | ||
| } // namespace control | ||
| } // namespace urcl |
Oops, something went wrong.