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

Humble support and removal of undesired transformation between y and z axis #82

Open
wants to merge 2 commits into
base: foxy-devel
Choose a base branch
from
Open

Humble support and removal of undesired transformation between y and z axis #82

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e6787ee
Removed undesired transformation of Y and Z axis
Jul 12, 2024
18cb07a
Added catching new QOS parameters for ROS Humble during start up
Jul 12, 2024
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
12 changes: 12 additions & 0 deletions include/mocap_optitrack/mocap_config.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,10 @@ namespace rosparam
const std::string EnableTfPublisher = "tf";
const std::string ChildFrameId = "child_frame_id";
const std::string ParentFrameId = "parent_frame_id";
const std::string QosOverrideDurability = "qos_overrides./tf.publisher.durability";
const std::string QosOverrideHistory = "qos_overrides./tf.publisher.history";
const std::string QosOverrideDepth = "qos_overrides./tf.publisher.depth";
const std::string QosOverrideReliability = "qos_overrides./tf.publisher.reliability";
}
}

Expand All @@ -66,6 +70,10 @@ struct ServerDescription
static const int DataPort;
static const std::string MulticastIpAddress;
static const bool EnableOptitrack;
static const std::string QosOverrideDurability;
static const std::string QosOverrideHistory;
static const int QosOverrideDepth;
static const std::string QosOverrideReliability;
};

ServerDescription();
Expand All @@ -74,6 +82,10 @@ struct ServerDescription
std::string multicastIpAddress;
bool enableOptitrack;
std::vector<int64_t> version;
std::string QosOverrideDurability;
std::string QosOverrideHistory;
int QosOverrideDepth;
std::string QosOverrideReliability;
};

/// \brief ROS publisher configuration
Expand Down
18 changes: 17 additions & 1 deletion src/mocap_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -163,7 +163,6 @@ namespace mocap_optitrack
RCLCPP_INFO(node->get_logger(), "Got parameter: '%s'", ss.str().c_str());

std::string rigid_bodies_prefix = "rigid_bodies.";

if (!param.get_name().compare(rosparam::keys::CommandPort))
{
serverDescription.commandPort = param.as_int();
Expand All @@ -180,6 +179,23 @@ namespace mocap_optitrack
{
serverDescription.enableOptitrack = param.as_bool();
}
//In ROS humble qos is added as parameter, these lines add this to catch the exception, but overriding QOS is not implemented
else if (!param.get_name().compare(rosparam::keys::QosOverrideDurability))
{
serverDescription.QosOverrideDurability = param.as_string();
}
else if (!param.get_name().compare(rosparam::keys::QosOverrideHistory))
{
serverDescription.QosOverrideHistory = param.as_string();
}
else if (!param.get_name().compare(rosparam::keys::QosOverrideDepth))
{
serverDescription.QosOverrideDepth = param.as_int();
}
else if (!param.get_name().compare(rosparam::keys::QosOverrideReliability))
{
serverDescription.QosOverrideReliability = param.as_string();
}
else
{
result.successful = false;
Expand Down
32 changes: 16 additions & 16 deletions src/rigid_body_publisher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,25 +46,25 @@ namespace utilities
{
// Motive 1.7+ and < Motive 2.0 coordinate system
poseStampedMsg.pose.position.x = -body.pose.position.x;
poseStampedMsg.pose.position.y = body.pose.position.z;
poseStampedMsg.pose.position.z = body.pose.position.y;
poseStampedMsg.pose.position.y = body.pose.position.y;
poseStampedMsg.pose.position.z = body.pose.position.z;

poseStampedMsg.pose.orientation.x = -body.pose.orientation.x;
poseStampedMsg.pose.orientation.y = body.pose.orientation.z;
poseStampedMsg.pose.orientation.z = body.pose.orientation.y;
poseStampedMsg.pose.orientation.y = body.pose.orientation.y;
poseStampedMsg.pose.orientation.z = body.pose.orientation.z;
poseStampedMsg.pose.orientation.w = body.pose.orientation.w;
}
else
{
// y & z axes are swapped in the Optitrack coordinate system
// Also compatible with versions > Motive 2.0
poseStampedMsg.pose.position.x = body.pose.position.x;
poseStampedMsg.pose.position.y = -body.pose.position.z;
poseStampedMsg.pose.position.z = body.pose.position.y;
poseStampedMsg.pose.position.y = -body.pose.position.y;
poseStampedMsg.pose.position.z = body.pose.position.z;

poseStampedMsg.pose.orientation.x = body.pose.orientation.x;
poseStampedMsg.pose.orientation.y = -body.pose.orientation.z;
poseStampedMsg.pose.orientation.z = body.pose.orientation.y;
poseStampedMsg.pose.orientation.y = -body.pose.orientation.y;
poseStampedMsg.pose.orientation.z = body.pose.orientation.z;
poseStampedMsg.pose.orientation.w = body.pose.orientation.w;
}
return poseStampedMsg;
Expand All @@ -77,25 +77,25 @@ nav_msgs::msg::Odometry getRosOdom(RigidBody const& body, const Version& coordin
{
// Motive 1.7+ and < Motive 2.0 coordinate system
OdometryMsg.pose.pose.position.x = -body.pose.position.x;
OdometryMsg.pose.pose.position.y = body.pose.position.z;
OdometryMsg.pose.pose.position.z = body.pose.position.y;
OdometryMsg.pose.pose.position.y = body.pose.position.y;
OdometryMsg.pose.pose.position.z = body.pose.position.z;

OdometryMsg.pose.pose.orientation.x = -body.pose.orientation.x;
OdometryMsg.pose.pose.orientation.y = body.pose.orientation.z;
OdometryMsg.pose.pose.orientation.z = body.pose.orientation.y;
OdometryMsg.pose.pose.orientation.y = body.pose.orientation.y;
OdometryMsg.pose.pose.orientation.z = body.pose.orientation.z;
OdometryMsg.pose.pose.orientation.w = body.pose.orientation.w;
}
else
{
// y & z axes are swapped in the Optitrack coordinate system
// Also compatible with versions > Motive 2.0
OdometryMsg.pose.pose.position.x = body.pose.position.x;
OdometryMsg.pose.pose.position.y = -body.pose.position.z;
OdometryMsg.pose.pose.position.z = body.pose.position.y;
OdometryMsg.pose.pose.position.y = -body.pose.position.y;
OdometryMsg.pose.pose.position.z = body.pose.position.z;

OdometryMsg.pose.pose.orientation.x = body.pose.orientation.x;
OdometryMsg.pose.pose.orientation.y = -body.pose.orientation.z;
OdometryMsg.pose.pose.orientation.z = body.pose.orientation.y;
OdometryMsg.pose.pose.orientation.y = -body.pose.orientation.y;
OdometryMsg.pose.pose.orientation.z = body.pose.orientation.z;
OdometryMsg.pose.pose.orientation.w = body.pose.orientation.w;
}
return OdometryMsg;
Expand Down