diff --git a/cfg/TiagoArmClient.cfg b/cfg/TiagoArmClient.cfg
index a9c95e5bc16d9a0db1ce83acef44dbbd7982ee9e..46eb135d32cf4297e007e37c4cd60f58698905e6 100755
--- a/cfg/TiagoArmClient.cfg
+++ b/cfg/TiagoArmClient.cfg
@@ -61,10 +61,13 @@ move_pose.add("plan_frame_id", str_t, 0, "
move_pose.add("x_pos", double_t, 0, "Target X position", 0.5, -2.0, 2.0)
move_pose.add("y_pos", double_t, 0, "Target Y position", 0.0, -2.0, 2.0)
move_pose.add("z_pos", double_t, 0, "Target Z position", 0.6, -2.0, 2.0)
-move_pose.add("x_orientation", double_t, 0, "Target X orientation", 0.0, -3.14159,3.14159)
-move_pose.add("y_orientation", double_t, 0, "Target Y orientation", 0.0, -3.14159,3.14159)
-move_pose.add("z_orientation", double_t, 0, "Target Z orientation", 0.0, -3.14159,3.14159)
-move_pose.add("w_orientation", double_t, 0, "Target W orientation", 1.0, -3.14159,3.14159)
+move_pose.add("roll", double_t, 0, "Target roll angle", 0.0, -3.14159,3.14159)
+move_pose.add("pitch", double_t, 0, "Target pitch angle", 0.0, -3.14159,3.14159)
+move_pose.add("yaw", double_t, 0, "Target yaw angle", 0.0, -3.14159,3.14159)
+#move_pose.add("x_orientation", double_t, 0, "Target X orientation", 0.0, -3.14159,3.14159)
+#move_pose.add("y_orientation", double_t, 0, "Target Y orientation", 0.0, -3.14159,3.14159)
+#move_pose.add("z_orientation", double_t, 0, "Target Z orientation", 0.0, -3.14159,3.14159)
+#move_pose.add("w_orientation", double_t, 0, "Target W orientation", 1.0, -3.14159,3.14159)
move_pose.add("position_tol", double_t, 0, "Target position tolerance", 0.1, 0, 0.5)
move_pose.add("orientation_tol", double_t, 0, "Target orientation tolerance", 0.1, 0, 3.14159)
@@ -73,10 +76,13 @@ objects.add("object_frame_id", str_t, 0, "
objects.add("obj_x_pos", double_t, 0, "Object X position", 0.0, -2.0, 2.0)
objects.add("obj_y_pos", double_t, 0, "Object Y position", 0.0, -2.0, 2.0)
objects.add("obj_z_pos", double_t, 0, "Object Y position", 0.0, -2.0, 2.0)
-objects.add("obj_x_orientation", double_t, 0, "Object X orientation", 0.0, -3.14159,3.14159)
-objects.add("obj_y_orientation", double_t, 0, "Object Y orientation", 0.0, -3.14159,3.14159)
-objects.add("obj_z_orientation", double_t, 0, "Object Z orientation", 0.0, -3.14159,3.14159)
-objects.add("obj_w_orientation", double_t, 0, "Object W orientation", 0.0, -3.14159,3.14159)
+objects.add("obj_roll", double_t, 0, "Object roll angle", 0.0, -3.14159,3.14159)
+objects.add("obj_pitch", double_t, 0, "Object pitch angle", 0.0, -3.14159,3.14159)
+objects.add("obj_yaw", double_t, 0, "Object yaw angle", 0.0, -3.14159,3.14159)
+#objects.add("obj_x_orientation", double_t, 0, "Object X orientation", 0.0, -3.14159,3.14159)
+#objects.add("obj_y_orientation", double_t, 0, "Object Y orientation", 0.0, -3.14159,3.14159)
+#objects.add("obj_z_orientation", double_t, 0, "Object Z orientation", 0.0, -3.14159,3.14159)
+#objects.add("obj_w_orientation", double_t, 0, "Object W orientation", 0.0, -3.14159,3.14159)
objects.add("obj_width", double_t, 0, "Object width", 0.0, -2.0, 2.0)
objects.add("obj_length", double_t, 0, "Object length", 0.0, -2.0, 2.0)
objects.add("obj_height", double_t, 0, "Object height", 0.0, -2.0, 2.0)
@@ -86,10 +92,13 @@ objects.add("dis_col", bool_t, 0, "Disable collision of this obje
objects.add("remove_env", bool_t, 0, "Remove object from environment", False)
const.add("const_frame_id", str_t, 0, "Constraint reference frame", "/base_footprint")
-const.add("const_x_orientation", double_t, 0, "Constraint X orientation", 0.0, -3.14159,3.14159)
-const.add("const_y_orientation", double_t, 0, "Constraint Y orientation", 0.0, -3.14159,3.14159)
-const.add("const_z_orientation", double_t, 0, "Constraint Z orientation", 0.0, -3.14159,3.14159)
-const.add("const_w_orientation", double_t, 0, "Constraint W orientation", 0.0, -3.14159,3.14159)
+const.add("const_roll", double_t, 0, "Contrainst roll angle", 0.0, -3.14159,3.14159)
+const.add("const_pitch", double_t, 0, "Contrainst pitch angle", 0.0, -3.14159,3.14159)
+const.add("const_yaw", double_t, 0, "Contrainst yaw angle", 0.0, -3.14159,3.14159)
+#const.add("const_x_orientation", double_t, 0, "Constraint X orientation", 0.0, -3.14159,3.14159)
+#const.add("const_y_orientation", double_t, 0, "Constraint Y orientation", 0.0, -3.14159,3.14159)
+#const.add("const_z_orientation", double_t, 0, "Constraint Z orientation", 0.0, -3.14159,3.14159)
+#const.add("const_w_orientation", double_t, 0, "Constraint W orientation", 0.0, -3.14159,3.14159)
const.add("add_orientation_const", bool_t, 0, "Add orientation constraint", False)
const.add("x_plane_const", bool_t, 0, "Plane on the X axis", False)
const.add("y_plane_const", bool_t, 0, "Plane on the Y axis", False)
diff --git a/cfg/TiagoArmModule.cfg b/cfg/TiagoArmModule.cfg
index b8bfbb2d2c551fdd530c8e200db51464af9d5b65..db5110e7ad05fd9e8f3762446d6457d2d50fc1bc 100755
--- a/cfg/TiagoArmModule.cfg
+++ b/cfg/TiagoArmModule.cfg
@@ -44,7 +44,7 @@ workspace = gen.add_group("Planner workspace")
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
-add_module_params(gen,"arm_module")
+add_module_params(gen,"arm")
play_motion_action=add_module_action_params(gen,"move")
joints_subs.add("js_watchdog_time_s",double_t,0, "Maximum time between joint state messages",1,0.01,50)
diff --git a/include/tiago_arm_client_alg_node.h b/include/tiago_arm_client_alg_node.h
index fd558ff075e98ed4961c9c579843c979c3c44584..584bb0168fd336a565942ecd55719ba360c13f5c 100644
--- a/include/tiago_arm_client_alg_node.h
+++ b/include/tiago_arm_client_alg_node.h
@@ -35,6 +35,9 @@
// [arm server client headers]
#include <tiago_arm_module/tiago_arm_module.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+//#include <tf2_ros/transform_listener.h>
+
/**
* \brief IRI ROS Specific Algorithm Class
*
diff --git a/launch/ari_arm_bt_client.launch b/launch/ari_arm_bt_client.launch
deleted file mode 100644
index 989a98214f6e6abc74ab603ce31524b795c3ea52..0000000000000000000000000000000000000000
--- a/launch/ari_arm_bt_client.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ari_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ari_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_arm_module"
- type="tiago_arm_bt_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/arm_module/joint_states"
- to="$(arg joint_states_topic)"/>
- <remap from="~/arm_module/arm"
- to="$(arg arm_action)"/>
- <remap from="~/arm_module/planning_scene"
- to="$(arg planning_scene_topic)"/>
- <remap from="~/arm_module/get_planning_scene"
- to="$(arg get_planning_scene_service)"/>
- <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
- <rosparam file="$(arg move_groups_file)" command="load" ns="arm_module" />
- <param name="path" value="$(arg tree_path)"/>
- <param name="tree_xml_file" value="$(arg tree_file)"/>
- <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/ari_arm_client.launch b/launch/ari_arm_client.launch
deleted file mode 100644
index 04f117840ad172b1e5045d4a1fae2712e772633a..0000000000000000000000000000000000000000
--- a/launch/ari_arm_client.launch
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ari_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ari_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_arm_module"
- type="tiago_arm_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/arm_module/joint_states"
- to="$(arg joint_states_topic)"/>
- <remap from="~/arm_module/arm"
- to="$(arg arm_action)"/>
- <remap from="~/arm_module/planning_scene"
- to="$(arg planning_scene_topic)"/>
- <remap from="~/arm_module/get_planning_scene"
- to="$(arg get_planning_scene_service)"/>
- <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
- <rosparam file="$(arg move_groups_file)" command="load" ns="arm_module" />
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/arm_bt_client.launch b/launch/arm_bt_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..0e98c5a04ddf543a6dc057598742999667219456
--- /dev/null
+++ b/launch/arm_bt_client.launch
@@ -0,0 +1,45 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="robot_type" doc="Choose robot type: ari, tiago, tiago_dual, ivo"/>
+ <arg name="dr" default="false"/>
+ <arg name="node_name" default="arm_bt_client" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find tiago_arm_module)/config/$(arg robot_type)_arm_module_default.yaml" />
+ <arg name="move_groups_file" default="$(find tiago_arm_module)/config/$(arg robot_type)_move_groups.yaml" />
+ <arg name="joint_states_topic" default="/joint_states"/>
+ <arg name="arm_action" default="/move_group"/>
+ <arg name="planning_scene_topic" default="/planning_scene"/>
+ <arg name="get_planning_scene_service" default="/get_planning_scene"/>
+ <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
+ <arg name="tree_file" default="bt_test" />
+ <arg name="bt_client_rate" default="10" />
+
+ <node name="$(arg node_name)"
+ pkg="tiago_arm_module"
+ type="tiago_arm_bt_client"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~/arm/joint_states" to="$(arg joint_states_topic)"/>
+ <remap from="~/arm/arm" to="$(arg arm_action)"/>
+ <remap from="~/arm/planning_scene" to="$(arg planning_scene_topic)"/>
+ <remap from="~/arm/get_planning_scene" to="$(arg get_planning_scene_service)"/>
+ <rosparam file="$(arg config_file)" command="load" ns="arm" />
+ <rosparam file="$(arg move_groups_file)" command="load" ns="arm" />
+ <param name="path" value="$(arg tree_path)"/>
+ <param name="tree_xml_file" value="$(arg tree_file)"/>
+ <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
+ </node>
+
+ <node if="$(arg dr)"
+ name="$(anon rqt)"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ respawn="false"
+ output="$(arg output)"
+ args="$(arg node_name)">
+ </node>
+
+
+</launch>
+
diff --git a/launch/arm_client.launch b/launch/arm_client.launch
new file mode 100644
index 0000000000000000000000000000000000000000..82c39e25d3133fe48faa86964b2f24720fc7152f
--- /dev/null
+++ b/launch/arm_client.launch
@@ -0,0 +1,38 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<launch>
+ <arg name="robot_type" doc="Choose robot type: ari, tiago, tiago_dual, ivo"/>
+ <arg name="dr" default="false"/>
+ <arg name="node_name" default="arm_client" />
+ <arg name="output" default="screen" />
+ <arg name="launch_prefix" default="" />
+ <arg name="config_file" default="$(find tiago_arm_module)/config/$(arg robot_type)_arm_module_default.yaml" />
+ <arg name="move_groups_file" default="$(find tiago_arm_module)/config/$(arg robot_type)_move_groups.yaml" />
+ <arg name="joint_states_topic" default="/joint_states"/>
+ <arg name="arm_action" default="/move_group"/>
+ <arg name="planning_scene_topic" default="/planning_scene"/>
+ <arg name="get_planning_scene_service" default="/get_planning_scene"/>
+
+ <node name="$(arg node_name)"
+ pkg="tiago_arm_module"
+ type="tiago_arm_client"
+ output="$(arg output)"
+ launch-prefix="$(arg launch_prefix)">
+ <remap from="~/arm/joint_states" to="$(arg joint_states_topic)"/>
+ <remap from="~/arm/arm" to="$(arg arm_action)"/>
+ <remap from="~/arm/planning_scene" to="$(arg planning_scene_topic)"/>
+ <remap from="~/arm/get_planning_scene" to="$(arg get_planning_scene_service)"/>
+ <rosparam file="$(arg config_file)" command="load" ns="arm" />
+ <rosparam file="$(arg move_groups_file)" command="load" ns="arm" />
+ </node>
+
+ <node if="$(arg dr)"
+ name="$(anon rqt)"
+ pkg="rqt_reconfigure"
+ type="rqt_reconfigure"
+ respawn="false"
+ output="$(arg output)"
+ args="$(arg node_name)">
+ </node>
+
+</launch>
+
diff --git a/launch/ivo_arm_bt_client.launch b/launch/ivo_arm_bt_client.launch
deleted file mode 100644
index 7beadbd7e1862e66f3fc13cd520e5449b43a56ca..0000000000000000000000000000000000000000
--- a/launch/ivo_arm_bt_client.launch
+++ /dev/null
@@ -1,33 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ivo_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ivo_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_arm_bt_client_sim.launch b/launch/ivo_arm_bt_client_sim.launch
deleted file mode 100644
index fc36816493f5361be040482d9b372b6aeb7faf95..0000000000000000000000000000000000000000
--- a/launch/ivo_arm_bt_client_sim.launch
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ivo_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ivo_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find ivo_1_gazebo)/launch/ivo_gazebo.launch">
- </include>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_arm_client.launch b/launch/ivo_arm_client.launch
deleted file mode 100644
index d28910b0f3e884498e0173c5dcc750d5f734416d..0000000000000000000000000000000000000000
--- a/launch/ivo_arm_client.launch
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ivo_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ivo_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- </include>
-
-</launch>
-
diff --git a/launch/ivo_arm_client_sim.launch b/launch/ivo_arm_client_sim.launch
deleted file mode 100644
index b5463ea1df3ed978632dee66603134e0feba3163..0000000000000000000000000000000000000000
--- a/launch/ivo_arm_client_sim.launch
+++ /dev/null
@@ -1,29 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/ivo_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/ivo_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <include file="$(find ivo_1_gazebo)/launch/ivo_gazebo.launch">
- </include>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_arm_bt_client.launch b/launch/tiago_arm_bt_client.launch
deleted file mode 100644
index 27f0572c6593f423d75caed0717bdf21830be0d8..0000000000000000000000000000000000000000
--- a/launch/tiago_arm_bt_client.launch
+++ /dev/null
@@ -1,43 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_arm_module"
- type="tiago_arm_bt_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/arm_module/joint_states"
- to="$(arg joint_states_topic)"/>
- <remap from="~/arm_module/arm"
- to="$(arg arm_action)"/>
- <remap from="~/arm_module/planning_scene"
- to="$(arg planning_scene_topic)"/>
- <remap from="~/arm_module/get_planning_scene"
- to="$(arg get_planning_scene_service)"/>
- <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
- <rosparam file="$(arg move_groups_file)" command="load" ns="arm_module" />
- <param name="path" value="$(arg tree_path)"/>
- <param name="tree_xml_file" value="$(arg tree_file)"/>
- <param name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/tiago_arm_bt_client_sim.launch b/launch/tiago_arm_bt_client_sim.launch
deleted file mode 100644
index 3ec6a9796d9a661f8366e411afe2349a561e569c..0000000000000000000000000000000000000000
--- a/launch/tiago_arm_bt_client_sim.launch
+++ /dev/null
@@ -1,40 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- <arg name="world" value="empty" />
- <arg name="use_moveit_camera" value="true"/>
- </include>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_arm_client.launch b/launch/tiago_arm_client.launch
deleted file mode 100644
index 844addc82cca9425fbf04b956421030eb4f4cda8..0000000000000000000000000000000000000000
--- a/launch/tiago_arm_client.launch
+++ /dev/null
@@ -1,36 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <!-- launch the play motion client node -->
- <node name="$(arg node_name)"
- pkg="tiago_arm_module"
- type="tiago_arm_client"
- output="$(arg output)"
- launch-prefix="$(arg launch_prefix)">
- <remap from="~/arm_module/joint_states"
- to="$(arg joint_states_topic)"/>
- <remap from="~/arm_module/arm"
- to="$(arg arm_action)"/>
- <remap from="~/arm_module/planning_scene"
- to="$(arg planning_scene_topic)"/>
- <remap from="~/arm_module/get_planning_scene"
- to="$(arg get_planning_scene_service)"/>
- <rosparam file="$(arg config_file)" command="load" ns="arm_module" />
- <rosparam file="$(arg move_groups_file)" command="load" ns="arm_module" />
- </node>
-
- <!-- launch dynamic reconfigure -->
- <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
- output="screen"/>
-
-</launch>
-
diff --git a/launch/tiago_arm_client_sim.launch b/launch/tiago_arm_client_sim.launch
deleted file mode 100644
index 05c9ab2a588192836fbd91c5aed69f59b5b3c0a4..0000000000000000000000000000000000000000
--- a/launch/tiago_arm_client_sim.launch
+++ /dev/null
@@ -1,33 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <include file="$(find tiago_gazebo)/launch/tiago_gazebo.launch">
- <arg name="public_sim" value="true" />
- <arg name="robot" value="steel" />
- <arg name="world" value="empty" />
- <arg name="use_moveit_camera" value="true"/>
- </include>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_dual_arm_bt_client.launch b/launch/tiago_dual_arm_bt_client.launch
deleted file mode 100644
index c2d57cbcdba66ad864eab99f528337c54420f80d..0000000000000000000000000000000000000000
--- a/launch/tiago_dual_arm_bt_client.launch
+++ /dev/null
@@ -1,33 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_dual_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_dual_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <arg name="tree_path" default="$(find tiago_arm_module)/src/xml" />
- <arg name="tree_file" default="bt_test" />
- <arg name="bt_client_rate" default="10" />
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_bt_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- <arg name="tree_path" value="$(arg tree_path)"/>
- <arg name="tree_file" value="$(arg tree_file)"/>
- <arg name="bt_client_rate" value="$(arg bt_client_rate)"/>
- </include>
-
-</launch>
-
diff --git a/launch/tiago_dual_arm_client.launch b/launch/tiago_dual_arm_client.launch
deleted file mode 100644
index 7979f3ea8dcb37e527570d11ec333a3c9a6707a4..0000000000000000000000000000000000000000
--- a/launch/tiago_dual_arm_client.launch
+++ /dev/null
@@ -1,26 +0,0 @@
-<?xml version="1.0" encoding="UTF-8"?>
-<launch>
- <arg name="node_name" default="arm_client" />
- <arg name="output" default="screen" />
- <arg name="launch_prefix" default="" />
- <arg name="config_file" default="$(find tiago_arm_module)/config/tiago_dual_arm_module_default.yaml" />
- <arg name="move_groups_file" default="$(find tiago_arm_module)/config/tiago_dual_move_groups.yaml" />
- <arg name="joint_states_topic" default="/joint_states"/>
- <arg name="arm_action" default="/move_group"/>
- <arg name="planning_scene_topic" default="/planning_scene"/>
- <arg name="get_planning_scene_service" default="/get_planning_scene"/>
-
- <include file="$(find tiago_arm_module)/launch/tiago_arm_client.launch">
- <arg name="node_name" value="$(arg node_name)" />
- <arg name="output" value="$(arg output)" />
- <arg name="launch_prefix" value="$(arg launch_prefix)" />
- <arg name="config_file" value="$(arg config_file)" />
- <arg name="move_groups_file" value="$(arg move_groups_file)" />
- <arg name="joint_states_topic" value="$(arg joint_states_topic)"/>
- <arg name="arm_action" value="$(arg arm_action)"/>
- <arg name="planning_scene_topic" value="$(arg planning_scene_topic)"/>
- <arg name="get_planning_scene_service" value="$(arg get_planning_scene_service)"/>
- </include>
-
-</launch>
-
diff --git a/src/tiago_arm_client_alg_node.cpp b/src/tiago_arm_client_alg_node.cpp
index 7a6dcb848a44d65817fa08114d7d8c815a882383..2951cbdaab93e67421111a97e9e20b4c00f2a871 100644
--- a/src/tiago_arm_client_alg_node.cpp
+++ b/src/tiago_arm_client_alg_node.cpp
@@ -2,7 +2,7 @@
TiagoArmClientAlgNode::TiagoArmClientAlgNode(void) :
algorithm_base::IriBaseAlgorithm<TiagoArmClientAlgorithm>(),
- arm("arm_module",ros::this_node::getName())
+ arm("arm",ros::this_node::getName())
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
@@ -88,7 +88,7 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
joint_states.position[3]=config.joint4;
}
else
- ROS_ERROR("TiagoArmClientAlgNode -> Number of joints not recognized");
+ ROS_ERROR("TiagoArmClientAlgNode -> Number of joints not recognized: %lu", joint_states.position.size());
this->arm.move_to(joint_states,config.joint_tol);
config.start_joints=false;
}
@@ -104,10 +104,14 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
pose.pose.position.x=config.x_pos;
pose.pose.position.y=config.y_pos;
pose.pose.position.z=config.z_pos;
- pose.pose.orientation.x=config.x_orientation;
- pose.pose.orientation.y=config.y_orientation;
- pose.pose.orientation.z=config.z_orientation;
- pose.pose.orientation.w=config.w_orientation;
+
+ tf2::Quaternion quat_tf;
+ quat_tf.setRPY(config.roll,config.pitch,config.yaw);
+ pose.pose.orientation = tf2::toMsg(quat_tf);
+ //pose.pose.orientation.x=config.x_orientation;
+ //pose.pose.orientation.y=config.y_orientation;
+ //pose.pose.orientation.z=config.z_orientation;
+ //pose.pose.orientation.w=config.w_orientation;
this->arm.move_to(pose,config.position_tol,config.orientation_tol);
config.start_pose=false;
}
@@ -122,10 +126,13 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
pose.pose.position.x=config.obj_x_pos;
pose.pose.position.y=config.obj_y_pos;
pose.pose.position.z=config.obj_z_pos;
- pose.pose.orientation.x=config.obj_x_orientation;
- pose.pose.orientation.y=config.obj_y_orientation;
- pose.pose.orientation.z=config.obj_z_orientation;
- pose.pose.orientation.w=config.obj_w_orientation;
+ tf2::Quaternion quat_tf;
+ quat_tf.setRPY(config.obj_roll,config.obj_pitch,config.obj_yaw);
+ pose.pose.orientation = tf2::toMsg(quat_tf);
+ //pose.pose.orientation.x=config.obj_x_orientation;
+ //pose.pose.orientation.y=config.obj_y_orientation;
+ //pose.pose.orientation.z=config.obj_z_orientation;
+ //pose.pose.orientation.w=config.obj_w_orientation;
this->arm.add_object(config.obj_name,pose,config.obj_width,config.obj_length,config.obj_height);
config.add_object=false;
}
@@ -143,10 +150,15 @@ void TiagoArmClientAlgNode::node_config_update(Config &config, uint32_t level)
{
quat.header.frame_id=config.const_frame_id;
quat.header.stamp=ros::Time::now();
- quat.quaternion.x=config.const_x_orientation;
- quat.quaternion.y=config.const_y_orientation;
- quat.quaternion.z=config.const_z_orientation;
- quat.quaternion.w=config.const_w_orientation;
+
+ tf2::Quaternion quat_tf;
+ quat_tf.setRPY(config.const_roll,config.const_pitch,config.const_yaw);
+ quat.quaternion = tf2::toMsg(quat_tf);
+
+ //quat.quaternion.x=config.const_x_orientation;
+ //quat.quaternion.y=config.const_y_orientation;
+ //quat.quaternion.z=config.const_z_orientation;
+ //quat.quaternion.w=config.const_w_orientation;
this->arm.add_orientation_path_constraint(quat);
config.add_orientation_const=false;
}