Merge branch 'topic_tools_transfomer' into 'indigo'

Topic tools transfomer

See merge request !178

mcr_common/mcr_topic_tools/CMakeLists.txt

@@ -5,15 +5,24 @@ project(mcr_topic_tools)
 find_package(catkin REQUIRED
   COMPONENTS
     roscpp
+    rospy
     std_msgs
     nodelet
     pluginlib
     topic_tools
+    roslint
 )
 
+catkin_python_setup()
+
 catkin_package(
   CATKIN_DEPENDS
+    roscpp
+    rospy
+    sensor_msgs
+    brics_actuator
     std_msgs
+    nodelet
 )
 
 include_directories(
@@ -31,9 +40,18 @@ target_link_libraries(topic_mux
 )
 
 ### TESTS
+roslint_cpp()
+roslint_python(
+  ros/scripts/brics_joints_to_joint_states_node
+)
+
 if(CATKIN_ENABLE_TESTING)
   find_package(roslaunch REQUIRED)
+  find_package(rostest REQUIRED)
+
   roslaunch_add_file_check(ros/launch)
+  add_rostest(ros/launch/brics_joints_to_joint_states.test)
+  catkin_add_nosetests(ros/test/test_transform_topic_utils.py)
 endif()
 
 ### INSTALLS
@@ -54,3 +72,8 @@ install(DIRECTORY ros/launch
 
 install(FILES nodelet_plugins.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(PROGRAMS
+  ros/scripts/brics_joints_to_joint_states_node
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)

mcr_common/mcr_topic_tools/package.xml

@@ -7,25 +7,33 @@
   </description>
 
   <maintainer email="santosh.thoduka@smail.inf.h-brs.de">Santosh Thoduka</maintainer>
+  <maintainer email="jose.sanchez.loza@gmail.com">Jose Sanchez</maintainer>
+  <maintainer email="shehzad.ahmed@smail.inf.h-brs.de">Shehzad Ahmed</maintainer>
 
   <author email="santosh.thoduka@smail.inf.h-brs.de">Santosh Thoduka</author>
+  <author email="jose.sanchez.loza@gmail.com">Jose Sanchez</author>
 
   <license>GPLv3</license>
 
   <buildtool_depend>catkin</buildtool_depend>
 
   <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>nodelet</build_depend>
   <build_depend>pluginlib</build_depend>
   <build_depend>topic_tools</build_depend>
+  <build_depend>roslint</build_depend>
 
   <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
   <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>brics_actuator</run_depend>
   <run_depend>nodelet</run_depend>
 
   <test_depend>roslaunch</test_depend>
-
+  <test_depend>rostest</test_depend>
   <export>
     <nodelet plugin="${prefix}/nodelet_plugins.xml"/>
   </export>

mcr_common/mcr_topic_tools/ros/doc/brics_joints_to_joint_states.graphml

@@ -0,0 +1,115 @@
+<?xml version="1.0" encoding="UTF-8" standalone="no"?>
+<graphml xmlns="http://graphml.graphdrawing.org/xmlns" xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xmlns:y="http://www.yworks.com/xml/graphml" xmlns:yed="http://www.yworks.com/xml/yed/3" xsi:schemaLocation="http://graphml.graphdrawing.org/xmlns http://www.yworks.com/xml/schema/graphml/1.1/ygraphml.xsd">
+  <!--Created by yFiles for Java 2.11-->
+  <key for="graphml" id="d0" yfiles.type="resources"/>
+  <key for="port" id="d1" yfiles.type="portgraphics"/>
+  <key for="port" id="d2" yfiles.type="portgeometry"/>
+  <key for="port" id="d3" yfiles.type="portuserdata"/>
+  <key attr.name="url" attr.type="string" for="node" id="d4"/>
+  <key attr.name="description" attr.type="string" for="node" id="d5"/>
+  <key for="node" id="d6" yfiles.type="nodegraphics"/>
+  <key attr.name="Description" attr.type="string" for="graph" id="d7"/>
+  <key attr.name="url" attr.type="string" for="edge" id="d8"/>
+  <key attr.name="description" attr.type="string" for="edge" id="d9"/>
+  <key for="edge" id="d10" yfiles.type="edgegraphics"/>
+  <graph edgedefault="directed" id="G">
+    <data key="d7"/>
+    <node id="n0">
+      <data key="d6">
+        <y:ShapeNode>
+          <y:Geometry height="57.0" width="16.267266560000053" x="206.73273343999995" y="286.9319999999999"/>
+          <y:Fill color="#FFFFFF" transparent="false"/>
+          <y:BorderStyle hasColor="false" type="line" width="1.0"/>
+          <y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" hasText="false" height="4.0" modelName="custom" textColor="#000000" visible="true" width="4.0" x="6.133633280000026" y="26.5">
+            <y:LabelModel>
+              <y:SmartNodeLabelModel distance="4.0"/>
+            </y:LabelModel>
+            <y:ModelParameter>
+              <y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
+            </y:ModelParameter>
+          </y:NodeLabel>
+          <y:Shape type="rectangle"/>
+        </y:ShapeNode>
+      </data>
+    </node>
+    <node id="n1">
+      <data key="d6">
+        <y:ShapeNode>
+          <y:Geometry height="57.0" width="16.267266560000053" x="608.7423667200021" y="286.9319999999999"/>
+          <y:Fill color="#FFFFFF" transparent="false"/>
+          <y:BorderStyle hasColor="false" type="line" width="1.0"/>
+          <y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" hasText="false" height="4.0" modelName="custom" textColor="#000000" visible="true" width="4.0" x="6.133633280000026" y="26.5">
+            <y:LabelModel>
+              <y:SmartNodeLabelModel distance="4.0"/>
+            </y:LabelModel>
+            <y:ModelParameter>
+              <y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
+            </y:ModelParameter>
+          </y:NodeLabel>
+          <y:Shape type="rectangle"/>
+        </y:ShapeNode>
+      </data>
+    </node>
+    <node id="n2">
+      <data key="d6">
+        <y:ShapeNode>
+          <y:Geometry height="57.0" width="133.9999999999999" x="340.3560000000024" y="286.9319999999999"/>
+          <y:Fill color="#993300" transparent="false"/>
+          <y:BorderStyle color="#993300" type="line" width="1.0"/>
+          <y:NodeLabel alignment="center" autoSizePolicy="content" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="31.9375" modelName="custom" textColor="#FFFFFF" visible="true" width="85.59765625" x="24.201171874999943" y="12.53125">Brics joints to
+joint states<y:LabelModel>
+              <y:SmartNodeLabelModel distance="4.0"/>
+            </y:LabelModel>
+            <y:ModelParameter>
+              <y:SmartNodeLabelModelParameter labelRatioX="0.0" labelRatioY="0.0" nodeRatioX="0.0" nodeRatioY="0.0" offsetX="0.0" offsetY="0.0" upX="0.0" upY="-1.0"/>
+            </y:ModelParameter>
+          </y:NodeLabel>
+          <y:Shape type="rectangle"/>
+        </y:ShapeNode>
+      </data>
+    </node>
+    <edge id="e0" source="n0" target="n2">
+      <data key="d10">
+        <y:PolyLineEdge>
+          <y:Path sx="8.131393280000111" sy="1.6028800000000274" tx="-52.74864000000224" ty="1.6028800000000274"/>
+          <y:LineStyle color="#000000" type="line" width="1.0"/>
+          <y:Arrows source="none" target="standard"/>
+          <y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="45.90625" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" visible="true" width="100.5390625" x="4.9701080322265625" y="-16.90887752769129">configuration_in
+(brics_actuator/
+ JointPositions)<y:LabelModel>
+              <y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/>
+            </y:LabelModel>
+            <y:ModelParameter>
+              <y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="6.044249064105637" distanceToCenter="true" position="right" ratio="0.0" segment="0"/>
+            </y:ModelParameter>
+            <y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/>
+          </y:EdgeLabel>
+          <y:BendStyle smoothed="false"/>
+        </y:PolyLineEdge>
+      </data>
+    </edge>
+    <edge id="e1" source="n2" target="n1">
+      <data key="d10">
+        <y:PolyLineEdge>
+          <y:Path sx="0.0" sy="0.0" tx="0.0" ty="0.0"/>
+          <y:LineStyle color="#000000" type="line" width="1.0"/>
+          <y:Arrows source="none" target="standard"/>
+          <y:EdgeLabel alignment="center" configuration="AutoFlippingLabel" distance="2.0" fontFamily="Dialog" fontSize="12" fontStyle="plain" hasBackgroundColor="false" hasLineColor="false" height="45.90625" modelName="custom" preferredPlacement="anywhere" ratio="0.5" textColor="#000000" visible="true" width="109.251953125" x="5.000012451174143" y="-16.940177621537714">configuration_out
+(sensor_msgs/
+ JointState)<y:LabelModel>
+              <y:SmartEdgeLabelModel autoRotationEnabled="false" defaultAngle="0.0" defaultDistance="10.0"/>
+            </y:LabelModel>
+            <y:ModelParameter>
+              <y:SmartEdgeLabelModelParameter angle="6.283185307179586" distance="6.012954214399904" distanceToCenter="true" position="right" ratio="0.0" segment="0"/>
+            </y:ModelParameter>
+            <y:PreferredPlacementDescriptor angle="0.0" angleOffsetOnRightSide="0" angleReference="absolute" angleRotationOnRightSide="co" distance="-1.0" frozen="true" placement="anywhere" side="anywhere" sideReference="relative_to_edge_flow"/>
+          </y:EdgeLabel>
+          <y:BendStyle smoothed="false"/>
+        </y:PolyLineEdge>
+      </data>
+    </edge>
+  </graph>
+  <data key="d0">
+    <y:Resources/>
+  </data>
+</graphml>

mcr_common/mcr_topic_tools/ros/include/mcr_topic_tools/topic_mux.h

@@ -30,13 +30,15 @@
 //          publisher/subcriber to select input topic.
 /////////////////////////////////////////////////////////////////////////////
 
-#ifndef TOPIC_MUX_H_
-#define TOPIC_MUX_H_
+#ifndef MCR_TOPIC_TOOLS_TOPIC_MUX_H_
+#define MCR_TOPIC_TOOLS_TOPIC_MUX_H_
 
 #include <nodelet/nodelet.h>
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 #include <std_msgs/String.h>
+#include <string>
+#include <vector>
 
 #include <topic_tools/shape_shifter.h>
 #include <topic_tools/parse.h>
@@ -129,7 +131,7 @@ class TopicMux : public nodelet::Nodelet
     ros::Publisher publisher_;
     ros::Subscriber sub_select_topic_;
 };
-
 PLUGINLIB_DECLARE_CLASS(mcr_topic_tools, TopicMux, mcr_topic_tools::TopicMux, nodelet::Nodelet);
-}
-#endif
+} /* namespace mcr_topic_tools */
+
+#endif  // MCR_TOPIC_TOOLS_TOPIC_MUX_H_

mcr_common/mcr_topic_tools/ros/launch/brics_joints_to_joint_states.test

@@ -0,0 +1,11 @@
+<?xml version="1.0"?>
+<launch>
+  <include file="$(find mcr_topic_tools)/ros/launch/brics_joints_to_joint_states_example.launch"/>
+
+  <test pkg="mcr_topic_tools" type="test_brics_joints_to_joint_states.py"
+        test-name="test_brics_joints_to_joint_states" ns="mcr_common">
+
+    <remap from="~joint_configuration" to="/mcr_common/brics_joints_to_joint_states/configuration_in"/>
+    <remap from="~component_output" to="/mcr_common/brics_joints_to_joint_states/configuration_out"/>
+  </test>
+</launch>

mcr_common/mcr_topic_tools/ros/launch/brics_joints_to_joint_states_example.launch

@@ -0,0 +1,10 @@
+<?xml version="1.0"?>
+<!-- Note: example launch file. Do not modify! -->
+<launch>
+  <node pkg="mcr_topic_tools" type="brics_joints_to_joint_states_node"
+        name="brics_joints_to_joint_states" output="screen" ns="mcr_common">
+
+    <!-- Node cycle rate (in hz) -->
+    <param name="loop_rate" type="double" value="10.0"/>
+  </node>
+</launch>

mcr_common/mcr_topic_tools/ros/scripts/brics_joints_to_joint_states_node

@@ -0,0 +1,6 @@
+#!/usr/bin/env python
+
+import mcr_topic_tools_ros.brics_joints_to_joint_states
+
+if __name__ == '__main__':
+    mcr_topic_tools_ros.brics_joints_to_joint_states.main()

mcr_common/mcr_topic_tools/ros/src/mcr_topic_tools_ros/brics_joints_to_joint_states.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python
+"""
+This module transforms a brics_actuator.msg.JointPositions message
+into a sensor_msgs.msg.JointState message.
+
+"""
+#-*- encoding: utf-8 -*-
+
+import rospy
+import sensor_msgs.msg
+import brics_actuator.msg
+import mcr_topic_tools_ros.transformer_utils as topic_utils
+
+
+class BricsJointsToJointStates(object):
+    """
+    Transforms a brics_actuator.msg.JointPositions message
+    into a sensor_msgs.msg.JointState message.
+
+    """
+    def __init__(self):
+        # Params
+        self.configuration_in = None
+
+        # Node cycle rate (in hz)
+        self.loop_rate = rospy.Rate(rospy.get_param('~loop_rate', 10.0))
+
+        # Publishers
+        self.configuration_out = rospy.Publisher(
+            '~configuration_out', sensor_msgs.msg.JointState, queue_size=1
+        )
+
+        # Subscribers
+        rospy.Subscriber(
+            '~configuration_in', brics_actuator.msg.JointPositions,
+            self.configuration_in_cb
+        )
+
+    def start(self):
+        """
+        Starts the component-wise pose error calculator.
+
+        """
+        rospy.loginfo("Ready to start...")
+
+        while not rospy.is_shutdown():
+            if self.configuration_in:
+                configuration_out = topic_utils.brics_joints_to_joint_state(
+                    self.configuration_in
+                )
+                self.configuration_out.publish(configuration_out)
+            else:
+                rospy.logdebug("Waiting for data...")
+
+            self.loop_rate.sleep()
+
+    def configuration_in_cb(self, msg):
+        """
+        Obtains the joint configuration.
+
+        """
+        self.configuration_in = msg
+
+
+def main():
+    rospy.init_node('brics_joints_to_joint_states', anonymous=True)
+    brics_joints_to_joint_states = BricsJointsToJointStates()
+    brics_joints_to_joint_states.start()

mcr_common/mcr_topic_tools/ros/src/mcr_topic_tools_ros/transformer_utils.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""
+This module contains a collection of functions to transform topics, e.g. to convert
+a 'brics_actuator/JointPosition' message into a 'sensor_msgs/JointState'.
+
+"""
+#-*- encoding: utf-8 -*-
+
+import sensor_msgs.msg
+
+
+def brics_joints_to_joint_state(m):
+    """
+    Transforms a PoseStamped message to a Pose, and if rounding is enable
+    then the message elements are rounded.
+
+    :param m: The message to be rounded.
+    :type m: brics_actuator.msg.JointPosition
+
+    :return: The transformed message.
+    :rtype : sensor_msgs.msg.JointState
+
+    """
+    message_out = sensor_msgs.msg.JointState()
+    # Assumption: The time stamp of the first joint is used as the general time stamp
+    message_out.header.stamp = m.positions[0].timeStamp
+    message_out.name = [i.joint_uri for i in m.positions]
+    message_out.position = [i.value for i in m.positions]
+
+    return message_out