Add parameters to increase/decrease a coord or angle by increments on rqt

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"))
 
 

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)
   {