diff --git a/cfg/DynamicTransformPublisher.cfg b/cfg/DynamicTransformPublisher.cfg index 39091bccbd3655b441ffa574e1e8f19e240ee558..5328fae2fa9a7f8fb5b648a867a609786fc742f6 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 b97563db2a546e89ecd48b61f3f98b757c0abf6f..84f01080fffe7c1fd9ca8e73b4173d39af3065d2 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) {