From dab97c40e0ab7e11bec3e5edfdf68f9b21be90f8 Mon Sep 17 00:00:00 2001
From: fherrero <fherrero@iri.upc.edu>
Date: Mon, 11 Nov 2024 13:32:58 +0100
Subject: [PATCH] Add parameters to increase/decrease a coord or angle by
increments on rqt
---
cfg/DynamicTransformPublisher.cfg | 26 +++++++++--
src/dynamic_transform_publisher_alg_node.cpp | 45 ++++++++++++++++++++
2 files changed, 67 insertions(+), 4 deletions(-)
diff --git a/cfg/DynamicTransformPublisher.cfg b/cfg/DynamicTransformPublisher.cfg
index 39091bc..5328fae 100755
--- a/cfg/DynamicTransformPublisher.cfg
+++ b/cfg/DynamicTransformPublisher.cfg
@@ -42,17 +42,35 @@ gen.add("rate", double_t, 0, "Main loop rate (Hz)",
gen.add("broadcast", bool_t, 0, "enable broadcasting", True)
gen.add("parent_id", str_t, 0, "parent frame id", "parent")
gen.add("frame_id", str_t, 0, "child frame id", "child")
-gen.add("time_offset", double_t, 0, "Seconds offset to add to time now", 0.0, -10000.0, 10000.0)
+gen.add("time_offset", double_t, 0, "Seconds offset to add to time now", 0.0, -100.0, 100.0)
gen.add("reset_translation", bool_t, 0, "reset x y z offsets to zero", False)
-gen.add("x", double_t, 0, "x offset in meters", 0.0, -10000.0, 10000.0)
-gen.add("y", double_t, 0, "y offset in meters", 0.0, -10000.0, 10000.0)
-gen.add("z", double_t, 0, "z offset in meters", 0.0, -10000.0, 10000.0)
+gen.add("x", double_t, 0, "x offset in meters", 0.0, -1000.0, 1000.0)
+gen.add("y", double_t, 0, "y offset in meters", 0.0, -1000.0, 1000.0)
+gen.add("z", double_t, 0, "z offset in meters", 0.0, -1000.0, 1000.0)
gen.add("reset_rotation", bool_t, 0, "reset roll pitch yaw to zero", False)
gen.add("roll", double_t, 0, "roll rotation in radians", 0.0, -3.14159, 3.14159)
gen.add("pitch", double_t, 0, "pitch rotation in radians", 0.0, -3.14159, 3.14159)
gen.add("yaw", double_t, 0, "yaw rotation in radians", 0.0, -3.14159, 3.14159)
gen.add("show_rosrun", bool_t, 0, "outputs ROS_INFO with rosrun equivalent", False)
+group_increment = gen.add_group("Increment")
+
+variable_enum = gen.enum([ gen.const("x", int_t, 0, "x coord"),
+ gen.const("y", int_t, 1, "y coord"),
+ gen.const("z", int_t, 2, "z coord"),
+ gen.const("roll", int_t, 3, "roll angle"),
+ gen.const("pitch", int_t, 4, "pitch angle"),
+ gen.const("yaw", int_t, 5, "yaw angle")
+ ],
+ "An enum to choose variable")
+
+group_increment.add("variable", int_t, 0, "Choose variable to apply increment to", 0, 0, 5, edit_method=variable_enum)
+group_increment.add("coord_increment", double_t, 0, "increment in m", 1.0, 0, 10.0)
+group_increment.add("angle_increment", double_t, 0, "increment in rad", 0.1, 0, 3.14159)
+group_increment.add("add_increment", bool_t, 0, "add increment to chosen variable", False)
+group_increment.add("rm_increment", bool_t, 0, "remove increment to chosen variable", False)
+
+
exit(gen.generate(PACKAGE, "DynamicTransformPublisherAlgorithm", "DynamicTransformPublisher"))
diff --git a/src/dynamic_transform_publisher_alg_node.cpp b/src/dynamic_transform_publisher_alg_node.cpp
index b97563d..84f0108 100644
--- a/src/dynamic_transform_publisher_alg_node.cpp
+++ b/src/dynamic_transform_publisher_alg_node.cpp
@@ -63,6 +63,51 @@ void DynamicTransformPublisherAlgNode::node_config_update(Config &config, uint32
this->transform_msg.header.stamp = ros::Time::now() + ros::Duration(this->config_.time_offset);
this->transform_msg.header.frame_id = config.parent_id;
this->transform_msg.child_frame_id = config.frame_id;
+
+ if(config.add_increment || config.rm_increment)
+ {
+ switch(config.variable)
+ {
+ case 0:
+ if(config.add_increment)
+ config.x += config.coord_increment;
+ else
+ config.x -= config.coord_increment;
+ break;
+ case 1:
+ if(config.add_increment)
+ config.y += config.coord_increment;
+ else
+ config.y -= config.coord_increment;
+ break;
+ case 2:
+ if(config.add_increment)
+ config.z += config.coord_increment;
+ else
+ config.z -= config.coord_increment;
+ break;
+ case 3:
+ if(config.add_increment)
+ config.roll += config.angle_increment;
+ else
+ config.roll -= config.angle_increment;
+ break;
+ case 4:
+ if(config.add_increment)
+ config.pitch += config.angle_increment;
+ else
+ config.pitch -= config.angle_increment;
+ break;
+ case 5:
+ if(config.add_increment)
+ config.yaw += config.angle_increment;
+ else
+ config.yaw -= config.angle_increment;
+ break;
+ }
+ config.add_increment=false;
+ config.rm_increment=false;
+ }
if(config.reset_translation)
{
--
GitLab