Merge pull request #118 from bluesat/add-urdf

LR-23 + Add numbat urdf

.gitmodules

@@ -1,6 +1,3 @@
 [submodule "rover/src/mtig_driver"]
 	path = rover/src/mtig_driver
 	url = https://github.com/lukscasanova/mtig_driver.git
-[submodule "rover/src/xsens_mti_ros_node"]
-	path = rover/src/xsens_mti_ros_node
-	url = https://github.com/bluesat/mtig_driver_fork.git

rover/src/owr_camera_control/package.xml

@@ -51,6 +51,7 @@
   <run_depend>message_runtime</run_depend>
   <run_depend>gscam</run_depend>
   <run_depend>owr_messages</run_depend>
+
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- You can specify that this package is a metapackage here: -->

rover/src/owr_drive_controls/CMakeLists.txt

@@ -120,6 +120,8 @@ target_link_libraries(cmd_vel_2_joints ${catkin_LIBRARIES})
 add_executable(arm_joints src/cmdVelToArmJoints.cpp src/armControl.cpp)
 add_dependencies(arm_joints owr_messages)
 target_link_libraries(arm_joints ${catkin_LIBRARIES})
+add_executable(cmd_vel_conversion src/Cmd_Vel_Conversion.cpp)
+target_link_libraries(cmd_vel_conversion ${catkin_LIBRARIES})
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes

rover/src/owr_drive_controls/README.md

@@ -0,0 +1,13 @@
+# Owr_Drive_Controls
+
+This package provides core driving functionality for the NUMBAT and BLUEtongue Mar's Rovers.
+
+## Nodes
+
+- `owr_drive_board` - module for communicating with the original PIC based PCB for BLUEtongue, as well as its replacement arduino. 
+Currently provides functionality for the controlling the NUMBAT rover's arm. 
+
+- `cmd_vel_2_joints` - converts the `linear` component of a ROS `Twist` message into movements of each of the individual joints in the drive system.
+Currently provides two modes of steering for NUMBAT.
+
+- `cmd_vel_conversion` - converts a ROS `Twist` message providing a linear velocity on the X axis and a rotation around the Z axis into the format used by `owr_joint_controls`.

rover/src/owr_drive_controls/include/Cmd_Vel_Conversion.hpp

@@ -0,0 +1,46 @@
+/*
+ * @date: 27/08/18
+ * @author: (original author) Harry J.E Day <[email protected]>
+ * @authors: (editors) 
+ * @details: Provides a conversion from a Twist message the uses linear.x and angular.z to one that uses linear.x and linear.y
+ * @copydetails: This code is released under the MIT License
+ * @copyright: Copyright BLUEsat UNSW 2018
+ * ros_package: owr_drive_controls
+ * ros_node: owr_cmd_vel_conversion
+ */
+
+#ifndef PROJECT_CMD_VEL_CONVERSION_HPP
+#define PROJECT_CMD_VEL_CONVERSION_HPP
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+
+#define OUT_TOPIC "/cmd_vel"
+#define IN_TOPIC "/cmd_vel/twist"
+
+class Cmd_Vel_Conversion {
+
+    public:
+        Cmd_Vel_Conversion();
+        /**
+         * Starts the nodes spin loop
+         */
+        void run();
+
+    protected:
+        /**
+         * Callback for the velocity message.
+         *
+         * Converts and republishes the message.
+         *
+         * @param vel_msg the message to convert.
+         */
+        void receive_vel_msg(const geometry_msgs::Twist::ConstPtr & vel_msg);
+
+    private:
+        ros::NodeHandle nh;
+        ros::Subscriber cmd_vel_sub;
+        ros::Publisher cmd_vel_pub;
+};
+
+#endif //PROJECT_CMD_VEL_CONVERSION_HPP

rover/src/owr_drive_controls/src/Cmd_Vel_Conversion.cpp

@@ -0,0 +1,49 @@
+/*
+ * @date: 27/08/18
+ * @author: (original author) Harry J.E Day <[email protected]>
+ * @authors: (editors) 
+ * @details: Provides a conversion from a Twist message the uses linear.x and angular.z to one that uses linear.x and linear.y
+ * @copydetails: This code is released under the MIT License
+ * @copyright: Copyright BLUEsat UNSW 2018
+ * ros_package: owr_drive_controls
+ * ros_node: owr_cmd_vel_conversion
+ */
+
+#include "Cmd_Vel_Conversion.hpp"
+
+
+int main(int argc, char ** argv) {
+    ros::init(argc, argv, "owr_cmd_vel_conversion");
+
+    Cmd_Vel_Conversion conversion_node;
+    conversion_node.run();
+}
+
+Cmd_Vel_Conversion::Cmd_Vel_Conversion() {
+    ros::TransportHints transportHints = ros::TransportHints().tcpNoDelay();
+    cmd_vel_sub = nh.subscribe<geometry_msgs::Twist>(IN_TOPIC, 1, &Cmd_Vel_Conversion::receive_vel_msg, this, transportHints);
+    cmd_vel_pub = nh.advertise<geometry_msgs::Twist>(OUT_TOPIC, 1, true);
+}
+
+void Cmd_Vel_Conversion::receive_vel_msg(const geometry_msgs::Twist::ConstPtr & vel_msg) {
+
+    // if we're not turning, retransmit the message
+    if(vel_msg->angular.z == 0.0L) {
+        cmd_vel_pub.publish(vel_msg);
+        return;
+    }
+
+    geometry_msgs::Twist out_msg;
+
+    // y = tan(theta)/x
+    out_msg.linear.y = tan(vel_msg->angular.z)/vel_msg->linear.x;
+    out_msg.linear.x = vel_msg->linear.x;
+    cmd_vel_pub.publish(out_msg);
+
+}
+
+void Cmd_Vel_Conversion::run() {
+    while(ros::ok()) {
+        ros::spin();
+    }
+}

rover/src/owr_numbat_tf/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.8.3)
+
+project(owr_numbat_tf)
+
+find_package(catkin REQUIRED)
+
+catkin_package()
+
+find_package(roslaunch)
+
+foreach(dir config launch meshes urdf)
+	install(DIRECTORY ${dir}/
+		DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir})
+endforeach(dir)

rover/src/owr_numbat_tf/config/joint_names_Assembly.SLDASM.yaml

@@ -0,0 +1 @@
+controller_joint_names: ['front_left_swerve', 'front_left_rotate', 'back_left_swerve', 'back_left_Rotate', 'back_right_swerve', 'back_RIght_wheel_axel', 'front_right_swerve', 'front_right_Rotate', ]

rover/src/owr_numbat_tf/config/numbat_control.yaml

@@ -0,0 +1,44 @@
+
+# Publish all joint states -----------------------------------
+joint_state_controller:
+    type: joint_state_controller/JointStateController
+    publish_rate: 50
+
+front_left_wheel_axel_controller:
+    type: velocity_controllers/JointVelocityController
+    joint: front_left_wheel_axel
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+back_left_wheel_axel_controller:
+    type: velocity_controllers/JointVelocityController
+    joint: back_left_wheel_axel
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+front_right_wheel_axel_controller:
+    type: velocity_controllers/JointVelocityController
+    joint: front_right_wheel_axel
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+back_right_wheel_axel_controller:
+    type: velocity_controllers/JointVelocityController
+    joint: back_right_wheel_axel
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+
+front_left_swerve_controller:
+    type: position_controllers/JointPositionController
+    joint: front_left_swerve
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+back_left_swerve_controller:
+    type: position_controllers/JointPositionController
+    joint: back_left_swerve
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+front_right_swerve_controller:
+    type: position_controllers/JointPositionController
+    joint: front_right_swerve
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+back_right_swerve_controller:
+    type: position_controllers/JointPositionController
+    joint: back_right_swerve
+    pid: {p: 100.0, i: 0.01, d: 10.0}
+
+laser_tilt_joint_controller:
+    type: position_controllers/JointPositionController
+    joint: laser_tilt_joint
+    pid: {p: 100.0, i: 0.01, d: 10.0}

rover/src/owr_numbat_tf/launch/display.launch

@@ -0,0 +1,26 @@
+<launch>
+  <arg
+    name="model" />
+  <arg
+    name="gui"
+    default="False" />
+  <param
+    name="robot_description"
+    textfile="$(find owr_numbat_tf)/robots/owr_numbat_tf.urdf" />
+  <param
+    name="use_gui"
+    value="$(arg gui)" />
+  <node
+    name="joint_state_publisher"
+    pkg="joint_state_publisher"
+    type="joint_state_publisher" />
+  <node
+    name="robot_state_publisher"
+    pkg="robot_state_publisher"
+    type="state_publisher" />
+  <node
+    name="rviz"
+    pkg="rviz"
+    type="rviz"
+    args="-d $(find owr_numbat_tf)/urdf.rviz" />
+</launch>

rover/src/owr_numbat_tf/launch/gazebo.launch

@@ -0,0 +1,29 @@
+<launch>
+  <rosparam file="$(find owr_bluetounge2_tf)/config/bluetounge_control.yaml" command="load"/>
+  <include
+    file="$(find gazebo_ros)/launch/empty_world.launch" />
+  <node
+    name="tf_footprint_base"
+    pkg="tf"
+    type="static_transform_publisher"
+    args="0 0 0 0 0 0 base_link base_footprint 40" />
+  <node
+    name="spawn_model"
+    pkg="gazebo_ros"
+    type="spawn_model"
+    args="-file $(find owr_numbat_tf)/robots/owr_numbat_tf.urdf -urdf -model owr_numbat_tf"
+    output="screen" />
+  <node
+    name="fake_joint_calibration"
+    pkg="rostopic"
+    type="rostopic"
+    args="pub /calibrated std_msgs/Bool true" />
+  <!-- load the controllers -->
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
+    output="screen" ns="/" 
+    args="joint_state_controller laser_tilt_joint_controller front_left_wheel_axel_controller back_left_wheel_axel_controller front_right_wheel_axel_controller  back_right_wheel_axel_controller front_left_swerve_controller back_left_swerve_controller front_right_swerve_controller  back_right_swerve_controller "/>
+
+  <!-- convert joint states to TF transforms for rviz, etc -->
+  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
+    respawn="false" output="screen"/>
+</launch>

rover/src/owr_numbat_tf/launch/joint_guess.launch

@@ -0,0 +1,55 @@
+<launch>
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="laser_tilt" output="screen">
+        <param name="joint_name" value="laser_tilt_joint"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/laser_tilt_joint_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="front_left_wheel_axel" output="screen">
+        <param name="joint_name" value="front_left_wheel_axel"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/front_left_wheel_axel_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="front_right_wheel_axel" output="screen">
+        <param name="joint_name" value="front_right_wheel_axel"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/front_right_wheel_axel_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="back_left_wheel_axel" output="screen">
+        <param name="joint_name" value="back_left_wheel_axel"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/back_left_wheel_axel_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="back_right_wheel_axel" output="screen">
+        <param name="joint_name" value="back_right_wheel_axel"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/back_right_wheel_axel_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="front_left_swerve" output="screen">
+        <param name="joint_name" value="front_left_swerve"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/front_left_swerve_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="front_right_swerve" output="screen">
+        <param name="joint_name" value="front_right_swerve"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/front_right_swerve_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="back_left_swerve" output="screen">
+        <param name="joint_name" value="back_left_swerve"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/back_left_swerve_controller/command"/>
+    </node>
+
+    <node pkg="owr_numbat_tf" type="publish_joint_guess.py" name="back_right_swerve" output="screen">
+        <param name="joint_name" value="back_right_swerve"/>
+        <param name="/joint_guesser/joint_delay_ns" value="100000000.0"/>
+        <remap from="/input/command" to="/back_right_swerve_controller/command"/>
+    </node>
+</launch>

rover/src/owr_numbat_tf/launch/tf.launch

@@ -0,0 +1,12 @@
+<launch>
+    <param
+            name="robot_description"
+            command="$(find xacro)/xacro '$(find owr_numbat_tf)/robots/owr_numbat_tf.urdf'"/>
+
+    <node
+            name="robot_state_publisher"
+            pkg="robot_state_publisher"
+            type="state_publisher"/>
+    <include file="$(find owr_numbat_tf)/launch/joint_guess.launch" />
+
+</launch>

rover/src/owr_numbat_tf/package.xml

@@ -0,0 +1,21 @@
+<package>
+  <name>owr_numbat_tf</name>
+  <version>1.0.0</version>
+  <description>
+    <p>URDF Description package for owr_numbat_tf</p>
+    <p>This package contains configuration data, 3D models and launch files
+for owr_numbat_tf robot</p>
+  </description>
+  <author>me</author>
+  <maintainer email="[email protected]" />
+  <license>MIT</license>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roslaunch</build_depend>
+  <run_depend>robot_state_publisher</run_depend>
+  <run_depend>rviz</run_depend>
+  <run_depend>joint_state_publisher</run_depend>
+  <run_depend>gazebo</run_depend>
+  <export>
+    <architecture_independent />
+  </export>
+</package>