Polonium19 · Polonium19 · May 9, 2019 · Feb 17, 2019 · Feb 17, 2019 · Feb 22, 2019
diff --git a/magni_bringup/launch/core.launch b/magni_bringup/launch/core.launch
@@ -48,6 +48,11 @@
           <node pkg="pi_sonar" type="pi_sonar" name="pi_sonar"/>
     </group>
 
+    <node pkg="diagnostic_aggregator" type="aggregator_node" name="diagnostics_agg">
+        <!-- Load the file you made above -->
+        <rosparam command="load" file="$(find magni_bringup)/param/diagnostics_agg.yaml"/>
+    </node>
+
     <!-- Load the parameters used by the following nodes -->
     <rosparam command="load" file="$(find magni_bringup)/param/base.yaml" />
     <!-- Launch the roscontrol controllers needed -->

diff --git a/magni_bringup/param/base.yaml b/magni_bringup/param/base.yaml
@@ -27,7 +27,7 @@ ubiquity_velocity_controller:
   wheel_radius : 0.1015
 
 
-  base_frame_id: base_link
+  base_frame_id: base_footprint
 
   # Wheel separation and radius multipliers
   wheel_separation_multiplier: 1.0 # default: 1.0

diff --git a/magni_bringup/param/diagnostics_agg.yaml b/magni_bringup/param/diagnostics_agg.yaml
@@ -0,0 +1,6 @@
+analyzers:
+      Raspicam:
+        type: GenericAnalyzer
+        path: Motors
+        startswith: 'motor_node:'
+
diff --git a/magni_description/meshes/sensors/raspi_camera.dae b/magni_description/meshes/sensors/raspi_camera.dae
diff --git a/magni_description/meshes/sensors/sonar_hc-sr04.dae b/magni_description/meshes/sensors/sonar_hc-sr04.dae
diff --git a/magni_description/urdf/magni.urdf.xacro b/magni_description/urdf/magni.urdf.xacro
@@ -14,6 +14,8 @@
   <xacro:include filename="$(find magni_description)/urdf/magni.transmission.xacro" />
   <xacro:include filename="$(find magni_description)/urdf/magni.gazebo.xacro" />
   <xacro:include filename="$(find magni_description)/urdf/inertial.xacro" />
+  <xacro:include filename="$(find magni_description)/urdf/sensors/sonar_hc-sr04.xacro" />
+  <xacro:include filename="$(find magni_description)/urdf/sensors/raspi_camera.xacro" />
 
 
   <!-- Robot base_link -->
@@ -132,55 +134,36 @@
 
 
 
-  <!-- Macro for defining sonars -->
-  <xacro:macro name="sonar" params="name *origin">
-    <link name="${name}"/>
-    <joint name="${name}" type="fixed">
-      <parent link="base_link"/>
-      <child link="${name}"/>
-      <xacro:insert_block name="origin"/>
-    </joint>
-  </xacro:macro>
-
   <!-- Define all sonars, if sonars_installed set to 1 -->
   <xacro:if value="${sonars_installed}">
-    <xacro:sonar name="sonar_0">
+    <xacro:sonar_hc-sr04 name="sonar_0" connected_to="base_link">
       <origin xyz="0.01 -0.14 0.15" rpy="0 0 ${-pi/2}"/>
-    </xacro:sonar>
-    <xacro:sonar name="sonar_1">
+    </xacro:sonar_hc-sr04>
+    <xacro:sonar_hc-sr04 name="sonar_1" connected_to="base_link">
       <origin xyz="0.08 -0.07 0.15" rpy="0 0 0.785"/>
-    </xacro:sonar>
-    <xacro:sonar name="sonar_2">
+    </xacro:sonar_hc-sr04>
+    <xacro:sonar_hc-sr04 name="sonar_2" connected_to="base_link">
       <origin xyz="0.08 -0.01 0.15" rpy="0 0 -0.785"/>
-    </xacro:sonar>
-    <xacro:sonar name="sonar_3">
+    </xacro:sonar_hc-sr04>
+    <xacro:sonar_hc-sr04 name="sonar_3" connected_to="base_link">
       <origin xyz="0.08 0.04 0.15" rpy="0 0 0"/>
-    </xacro:sonar>
-    <xacro:sonar name="sonar_4">
+    </xacro:sonar_hc-sr04>
+    <xacro:sonar_hc-sr04 name="sonar_4" connected_to="base_link">
       <origin xyz="0.01 0.14 0.15" rpy="0 0 ${pi/2}"/>
-    </xacro:sonar>
+    </xacro:sonar_hc-sr04>
   </xacro:if>
 
-  <!-- Macro for defining raspicam -->
-  <xacro:macro name="raspicam" params="*origin">
-    <link name="raspicam"/>
-    <joint name="raspicam" type="fixed">
-      <parent link="base_link"/>
-      <child link="raspicam"/>
-      <xacro:insert_block name="origin"/>
-    </joint>
-  </xacro:macro>
 
   <!-- Define raspicam based on the raspicam_mount argument -->
   <xacro:if value="${raspicam_mount == 'forward'}">
-    <xacro:raspicam>
+    <xacro:raspi_camera name="raspicam" connected_to="base_link">
       <origin xyz="0.035 0.085 0.14" rpy="1.15191731 0 1.5707"/>
-    </xacro:raspicam>
+    </xacro:raspi_camera>
   </xacro:if>
   <xacro:if value="${raspicam_mount == 'upward'}">
-    <xacro:raspicam>
+    <xacro:raspi_camera name="raspicam" connected_to="base_link">
       <origin xyz="0.035 0.15 0.14" rpy="0 0 ${pi}"/>
-    </xacro:raspicam>
+    </xacro:raspi_camera>
   </xacro:if>
 
 

diff --git a/magni_description/urdf/sensors/raspi_camera.xacro b/magni_description/urdf/sensors/raspi_camera.xacro
@@ -0,0 +1,76 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <xacro:macro name="raspi_camera" params="name:='' connected_to:='' *origin">
+
+    <xacro:unless value="${connected_to == ''}">
+      <joint name="${name}_joint" type="fixed">
+        <parent link="${connected_to}"/>
+        <child link="${name}"/>
+        <xacro:insert_block name="origin"/>
+      </joint>
+    </xacro:unless>
+
+    <link name="${name}">
+      <visual>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+        <geometry>
+          <mesh filename="package://magni_description/meshes/sensors/raspi_camera.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 ${pi/2}" />
+        <geometry>
+          <box size="0.03 0.026 0.016"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <!-- Additional link for camera sensor to orient camera direction -->
+    <link name="${name}_sensor"/>
+    <joint name="${name}_sensor_joint" type="fixed">
+      <parent link="${name}"/>
+      <child link="${name}_sensor"/>
+      <origin xyz="0 0 0.012" rpy="${pi/2} ${-pi/2} 0" />
+    </joint>
+
+    <!-- Gazebo sensor plugin -->
+    <gazebo reference="${name}_sensor">
+      <sensor type="camera" name="camera1">
+        <visualize>false</visualize>
+        <update_rate>30.0</update_rate>
+        <camera name="raspi_camera">
+          <horizontal_fov>1.086</horizontal_fov>
+          <image>
+            <width>640</width>
+            <height>480</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+        </camera>
+        <!-- Gazebo-ROS interface plugin -->
+        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+          <alwaysOn>true</alwaysOn>
+          <updateRate>0.0</updateRate>
+          <cameraName>${name}</cameraName>
+          <imageTopicName>/${name}_node/image/compressed</imageTopicName>
+          <cameraInfoTopicName>/${name}_node/camera_info</cameraInfoTopicName>
+          <frameName>${name}</frameName>
+          <hackBaseline>0.07</hackBaseline>
+          <distortionK1>0.0</distortionK1>
+          <distortionK2>0.0</distortionK2>
+          <distortionK3>0.0</distortionK3>
+          <distortionT1>0.0</distortionT1>
+          <distortionT2>0.0</distortionT2>
+        </plugin>
+      </sensor>
+    </gazebo>
+
+
+
+  </xacro:macro>
+
+</robot>
diff --git a/magni_description/urdf/sensors/sonar_hc-sr04.xacro b/magni_description/urdf/sensors/sonar_hc-sr04.xacro
@@ -0,0 +1,72 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot xmlns:xacro="http://wiki.ros.org/xacro">
+
+  <xacro:macro name="sonar_hc-sr04" params="name:='' connected_to:='' *origin">
+
+    <xacro:unless value="${connected_to == ''}">
+      <joint name="${name}_joint" type="fixed">
+        <parent link="${connected_to}"/>
+        <child link="${name}"/>
+        <xacro:insert_block name="origin"/>
+      </joint>
+    </xacro:unless>
+
+    <link name="${name}">
+      <visual>
+        <origin xyz="-0.006 0 0" rpy="${pi/2} 0 ${pi/2}" />
+        <geometry>
+          <mesh filename="package://magni_description/meshes/sensors/sonar_hc-sr04.dae" />
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <box size="0.02 0.044 0.021"/>
+        </geometry>
+      </collision>
+    </link>
+
+
+    <!-- Gazebo sensor plugin -->
+    <gazebo reference="${name}">
+      <sensor type="ray" name="${name}_sensor">
+        <visualize>false</visualize>
+        <update_rate>5</update_rate>
+        <ray>
+          <scan>
+            <horizontal>
+              <samples>5</samples>
+              <resolution>1.0</resolution>
+              <min_angle>-0.24</min_angle>
+              <max_angle>0.24</max_angle>
+            </horizontal>
+            <vertical>
+              <samples>5</samples>
+              <resolution>1.0</resolution>
+              <min_angle>-0.24</min_angle>
+              <max_angle>0.24</max_angle>
+            </vertical>
+          </scan>
+          <range>
+            <min>0.02</min>
+            <max>3.2</max>
+            <resolution>0.01</resolution>
+          </range>
+        </ray>
+        <!-- Gazebo-ROS interface plugin -->
+        <plugin filename="libgazebo_ros_range.so" name="gazebo_ros_range">
+          <gaussianNoise>0.015</gaussianNoise>
+          <alwaysOn>true</alwaysOn>
+          <updateRate>5</updateRate>
+          <topicName>/ubiquity_sonar/${name}</topicName>
+          <frameName>${name}</frameName>
+          <fov>0.5</fov>
+          <radiation>ultrasound</radiation>
+        </plugin>
+      </sensor>
+    </gazebo>
+
+
+  </xacro:macro>
+
+</robot>
diff --git a/magni_desktop/CMakeLists.txt b/magni_desktop/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(magni_desktop)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/magni_desktop/README.md b/magni_desktop/README.md
@@ -0,0 +1,5 @@
+# magni_desktop
+
+This is a metapackage that has everything needed for a workstation to control magni.
+
+Install with `sudo apt install ros-kinetic-magni-desktop`.
diff --git a/magni_desktop/package.xml b/magni_desktop/package.xml
@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>magni_desktop</name>
+  <version>0.4.3</version>
+  <description>Meta package for all the desktop side utilities for magni</description>
+
+  <maintainer email="send2arohan@gmail.com">Rohan Agrawal</maintainer>
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>magni_description</exec_depend>
+  <exec_depend>magni_teleop</exec_depend>
+  <exec_depend>magni_gazebo</exec_depend>
+  <exec_depend>magni_viz</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+    <metapackage />
+  </export>
+</package>
diff --git a/magni_gazebo/config/magni_controllers.yaml b/magni_gazebo/config/magni_controllers.yaml
@@ -29,15 +29,3 @@ ubiquity_velocity_controller:
       max_velocity           : 2.0   # rad/s
       has_acceleration_limits: true
       max_acceleration       : 5.0   # rad/s^2
-
-
-#/gazebo_ros_control:
-#  pid_gains:
-#    left_wheel_joint:
-#      p: 100.0
-#      i: 0.01
-#      d: 10.0
-#    right_wheel_joint:
-#      p: 100.0
-#      i: 0.01
-#      d: 10.0
diff --git a/magni_gazebo/launch/empty_world.launch b/magni_gazebo/launch/empty_world.launch
@@ -1,6 +1,9 @@
 <launch>
 
-    <arg name="rviz_config" default="false" />
+    <arg name="raspicam_mount" default="forward"/>
+    <arg name="sonars_installed" default="true"/>
+
+    <arg name="rviz_config" default="true" />
 
     <!-- load empty world -->
     <arg name="gui" default="true"/>
@@ -18,7 +21,11 @@
     </include>
 
     <!-- start robot -->
-    <include file="$(find magni_gazebo)/launch/magni.launch"/>
+    <include file="$(find magni_gazebo)/launch/magni.launch">
+        <arg name="raspicam_mount" value="$(arg raspicam_mount)"/>
+        <arg name="sonars_installed" value="$(arg sonars_installed)"/>
+    </include>
+