-
Notifications
You must be signed in to change notification settings - Fork 20
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Introduce ROS<->M+ concept conversion functions
Only support MP_COORD -> geometry_msgs/{Pose,Transform} for now, but others can be added later.
- Loading branch information
1 parent
9492838
commit 5236faf
Showing
5 changed files
with
79 additions
and
0 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
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,27 @@ | ||
//RosMotoPlusConversionUtils.c | ||
|
||
// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. | ||
// SPDX-FileCopyrightText: 2023, Delft University of Technology | ||
// | ||
// SPDX-License-Identifier: Apache-2.0 | ||
|
||
#include "MotoROS.h" | ||
|
||
|
||
void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose) | ||
{ | ||
ros_pose->position.x = MICROMETERS_TO_METERS(mp_coord->x); | ||
ros_pose->position.y = MICROMETERS_TO_METERS(mp_coord->y); | ||
ros_pose->position.z = MICROMETERS_TO_METERS(mp_coord->z); | ||
QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion( | ||
mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_pose->orientation); | ||
} | ||
|
||
void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform) | ||
{ | ||
ros_transform->translation.x = MICROMETERS_TO_METERS(mp_coord->x); | ||
ros_transform->translation.y = MICROMETERS_TO_METERS(mp_coord->y); | ||
ros_transform->translation.z = MICROMETERS_TO_METERS(mp_coord->z); | ||
QuatConversion_MpCoordOrient_To_GeomMsgsQuaternion( | ||
mp_coord->rx, mp_coord->ry, mp_coord->rz, &ros_transform->rotation); | ||
} |
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,42 @@ | ||
//RosMotoPlusConversionUtils.h | ||
|
||
// SPDX-FileCopyrightText: 2023, Yaskawa America, Inc. | ||
// SPDX-FileCopyrightText: 2023, Delft University of Technology | ||
// | ||
// SPDX-License-Identifier: Apache-2.0 | ||
|
||
#ifndef MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H | ||
#define MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H | ||
|
||
|
||
/** | ||
* Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Pose`. | ||
* | ||
* Unit conversion is also performed by this function: | ||
* | ||
* - `MP_COORD` positions are converted from micro-meters to meters | ||
* - `MP_COORD` orientations are converted from milli-degrees to radians (and | ||
* subsequently to a `Quaternion`) | ||
* | ||
* @param mp_coord Input M+ `MP_COORD` instance | ||
* @param ros_pose Output `geometry_msgs/msg/Pose` | ||
*/ | ||
extern void Ros_MpCoord_To_GeomMsgsPose(MP_COORD const* const mp_coord, geometry_msgs__msg__Pose* const ros_pose); | ||
|
||
|
||
/** | ||
* Converts an M+ `MP_COORD` struct to a ROS `geometry_msgs/msg/Transform`. | ||
* | ||
* Unit conversion is also performed by this function: | ||
* | ||
* - `MP_COORD` positions are converted from micro-meters to meters | ||
* - `MP_COORD` orientations are converted from milli-degrees to radians (and | ||
* subsequently to a `Quaternion`) | ||
* | ||
* @param mp_coord Input M+ `MP_COORD` instance | ||
* @param ros_transform Output `geometry_msgs/msg/Pose` | ||
*/ | ||
extern void Ros_MpCoord_To_GeomMsgsTransform(MP_COORD const* const mp_coord, geometry_msgs__msg__Transform* const ros_transform); | ||
|
||
|
||
#endif // MOTOROS2_ROS_MOTOPLUS_CONVERSION_UTILS_H |