Swarm in Blocks: code adaptation to swarms

clover/examples/leds.py

@@ -29,3 +29,5 @@
 
 print('Rainbow')
 set_effect(effect='rainbow')  # show rainbow
+
+# effects_list = ['fill', 'fade', 'flash', 'blink', 'blink_fast', 'wipe', 'rainbow', 'rainbow_fill']

clover/launch/led.launch

@@ -21,7 +21,7 @@
     </node>
 
     <!-- high level led effects control, events notification with leds -->
-    <node pkg="clover" name="led_effect" type="led" ns="led" clear_params="true" output="screen" if="$(arg led_effect)">
+    <node pkg="clover" name="led_effect" type="led" clear_params="true" output="screen" if="$(arg led_effect)">
         <param name="blink_rate" value="2"/>
         <param name="fade_period" value="0.5"/>
         <param name="rainbow_period" value="5"/>

clover/src/led.cpp

@@ -310,19 +310,19 @@ int main(int argc, char **argv)
 	nh_priv.param("notify/low_battery/threshold", low_battery_threshold, 3.7);
 	nh_priv.param("notify/error/ignore", error_ignore, {});
 
-	ros::service::waitForService("set_leds"); // cannot work without set_leds service
-	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("set_leds", true);
+	ros::service::waitForService("led/set_leds"); // cannot work without set_leds service
+	set_leds_srv = nh.serviceClient<led_msgs::SetLEDs>("led/set_leds", true);
 
 	// wait for leds count info
-	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("state", nh));
+	handleState(*ros::topic::waitForMessage<led_msgs::LEDStateArray>("led/state", nh));
 
-	auto state_sub = nh.subscribe("state", 1, &handleState);
+	auto state_sub = nh.subscribe("led/state", 1, &handleState);
 
-	auto set_effect = nh.advertiseService("set_effect", &setEffect);
+	auto set_effect = nh.advertiseService("led/set_effect", &setEffect);
 
-	auto mavros_state_sub = nh.subscribe("/mavros/state", 1, &handleMavrosState);
-	auto battery_sub = nh.subscribe("/mavros/battery", 1, &handleBattery);
-	auto rosout_sub = nh.subscribe("/rosout_agg", 1, &handleLog);
+	auto mavros_state_sub = nh.subscribe("mavros/state", 1, &handleMavrosState);
+	auto battery_sub = nh.subscribe("mavros/battery", 1, &handleBattery);
+	auto rosout_sub = nh.subscribe("rosout_agg", 1, &handleLog);
 
 	timer = nh.createTimer(ros::Duration(0), &proceed, false, false);
 

clover/src/simple_offboard.cpp

@@ -95,7 +95,7 @@ AttitudeTarget att_raw_msg;
 Thrust thrust_msg;
 TwistStamped rates_msg;
 TransformStamped target, setpoint;
-geometry_msgs::TransformStamped body;
+geometry_msgs::TransformStamped body, initial_pose;
 
 // State
 PoseStamped nav_start;
@@ -155,9 +155,9 @@ inline void publishBodyFrame()
 	q.setRPY(0, 0, tf::getYaw(local_position.pose.orientation));
 	tf::quaternionTFToMsg(q, body.transform.rotation);
 
-	body.transform.translation.x = local_position.pose.position.x;
-	body.transform.translation.y = local_position.pose.position.y;
-	body.transform.translation.z = local_position.pose.position.z;
+	body.transform.translation.x = local_position.pose.position.x + initial_pose.transform.translation.x;
+	body.transform.translation.y = local_position.pose.position.y + initial_pose.transform.translation.y;
+	body.transform.translation.z = local_position.pose.position.z + initial_pose.transform.translation.z;
 	body.header.frame_id = local_position.header.frame_id;
 	body.header.stamp = local_position.header.stamp;
 	transform_broadcaster->sendTransform(body);
@@ -874,6 +874,9 @@ int main(int argc, char **argv)
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.param<string>("body_frame", body.child_frame_id, "body");
 	nh_priv.getParam("reference_frames", reference_frames);
+	nh.param("initial_pose/x", initial_pose.transform.translation.x, 0.0);
+	nh.param("initial_pose/y", initial_pose.transform.translation.y, 0.0);
+	nh.param("initial_pose/z", initial_pose.transform.translation.z, 0.0);
 
 	// Default reference frames
 	std::map<string, string> default_reference_frames;

clover_blocks/www/index.html

@@ -488,4 +488,4 @@
 	</xml>
 </body>
 
-</html>
+</html>

clover_blocks/www/main.css

@@ -133,4 +133,4 @@ body[data-tab="python"] #python { display: block !important; }
 	margin: 0px;
 	padding: 10px;
 	z-index: 999999; /* overlap some Blockly's popups */
-}
+}

clover_blocks/www/main.js

@@ -323,4 +323,4 @@ programSelect.addEventListener('change', function(e) {
 		}
 		updateChanged();
 	}
-});
+});

clover_description/urdf/clover/clover4.xacro

@@ -7,6 +7,7 @@
   <xacro:arg name="gps" default="true"/>
   <xacro:arg name="maintain_camera_rate" default="false"/>
   <xacro:arg name="use_clover_physics" default="false"/>
+  <xacro:arg name="mavlink_tcp_port" default="4560"/>
 
   <xacro:include filename="clover4_base.xacro" />
   <xacro:include filename="../sensors/rpi_cam.urdf.xacro"/>
@@ -15,26 +16,26 @@
 
   <!-- Create camera plugin -->
   <xacro:if value="$(arg main_camera)">
-    <xacro:rpi_cam name="main_camera" parent="base_link" x="0.055" y="0.0" z="-0.03" roll="0" pitch="${pi / 2}" yaw="0" width="320" height="240" rate="40" do_throttling="$(arg maintain_camera_rate)"/>
-  </xacro:if>
-
-  <!-- Create rangefinder plugin -->
-  <xacro:if value="$(arg rangefinder)">
-    <xacro:distance_sensor parent="base_link" x="0.0" y="0.0" z="-0.04" roll="0" pitch="${pi / 2}" yaw="0"/>
+    <xacro:rpi_cam name="main_camera" parent="base_link" x="0.055" y="0.0" z="-0.03" roll="0" pitch="${pi/2}" yaw="0" width="320" height="240" rate="40" do_throttling="$(arg maintain_camera_rate)"/>
   </xacro:if>
 
   <!-- Instantiate LED strip -->
   <xacro:if value="$(arg led)">
     <xacro:led_strip
       name="led"
       parent="base_link"
-      radius="0.08"
-      bulb_radius="0.006"
-      led_count="58"
+      radius="0.1"
+      bulb_radius="0.01"
+      led_count="72"
       use_plugin="true"
       z="-0.002"/>
   </xacro:if>
 
+  <!-- Create rangefinder plugin -->
+  <xacro:if value="$(arg rangefinder)">
+    <xacro:distance_sensor parent="base_link" x="0.0" y="0.0" z="-0.04" roll="0" pitch="${pi / 2}" yaw="0"/>
+  </xacro:if>
+
   <xacro:if value="$(arg gps)">
     <gazebo>
       <include>

clover_description/urdf/clover/clover4_base.xacro

@@ -25,7 +25,7 @@
   </xacro:if>
 
   <!-- Gazebo plugins -->
-  <xacro:include filename="$(find clover_description)/urdf/clover/clover4_gazebo.xacro" />
+  <xacro:include filename="clover4_gazebo.xacro" />
 
   <xacro:if value="$(arg enable_logging)">
     <!-- Instantiate a logger -->

clover_description/urdf/clover/clover4_gazebo.xacro

@@ -51,13 +51,13 @@
       <magSubTopic>/mag</magSubTopic>
       <baroSubTopic>/baro</baroSubTopic>
       <mavlink_addr>INADDR_ANY</mavlink_addr>
-      <mavlink_tcp_port>4560</mavlink_tcp_port>
+      <mavlink_tcp_port>$(arg mavlink_tcp_port)</mavlink_tcp_port>
       <mavlink_udp_port>14560</mavlink_udp_port>
       <serialEnabled>false</serialEnabled>
       <serialDevice>/dev/ttyACM0</serialDevice>
       <baudRate>921600</baudRate>
       <qgc_addr>INADDR_ANY</qgc_addr>
-      <qgc_udp_port>14550</qgc_udp_port>
+      <qgc_udp_port>14560</qgc_udp_port>
       <sdk_addr>INADDR_ANY</sdk_addr>
       <sdk_udp_port>14540</sdk_udp_port>
       <hil_mode>false</hil_mode>

clover_description/urdf/leds/led_strip.xacro

@@ -30,7 +30,7 @@
                     <origin xyz="${radius*cos(2 * pi * link_i / links_total)} ${radius*sin(2 * pi * link_i / links_total)} 0"
                             rpy="0 0 ${2 * pi * link_i / links_total}"/>
                     <geometry>
-                        <box size="${bulb_radius} ${bulb_radius} ${bulb_radius}"/>
+                        <box size="${bulb_radius} ${bulb_radius} ${2 * bulb_radius}"/>
                     </geometry>
                 </visual>
                 <xacro:unless value="${link_i + 1 == links_total}">
@@ -39,12 +39,13 @@
             </xacro:macro>
             <xacro:loop_links link_i="0" links_total="${led_count}"/>
         </link>
-
+
+
         <xacro:if value="${use_plugin}">
         <gazebo reference="${name}_link">
             <visual>
                 <plugin name="${name}_controller" filename="libsim_leds.so">
-                    <robotNamespace></robotNamespace>
+                    <!-- <robotNamespace></robotNamespace> -->
                     <ledCount>${led_count}</ledCount>
                 </plugin>
                 <cast_shadows>false</cast_shadows>
@@ -55,6 +56,17 @@
                     <emissive>0 0 0 1</emissive>
                 </material>
             </visual>
+            <!-- <light name="${name}_light" type="point">
+                <attenuation>
+                    <range>20</range>
+                    <linear>0.2</linear>
+                    <constant>0.8</constant>
+                    <quadratic>0.01</quadratic>
+                </attenuation>
+                <diffuse>1.0 1.0 1.0 1</diffuse>
+                <specular>0.1 0.1 0.1 1</specular> 
+                <cast_shadows>false</cast_shadows>
+            </light>     -->
         </gazebo>
         </xacro:if>
         <gazebo reference="${name}_joint">

clover_description/urdf/sensors/distance_sensor.urdf.xacro

@@ -54,7 +54,7 @@
           </range>
         </ray>
         <plugin name="laser" filename="libgazebo_ros_range.so">
-          <robotNamespace></robotNamespace> <!-- FIXME: fill namespace? -->
+          <!-- <robotNamespace></robotNamespace> --> <!-- FIXME: fill namespace? -->
           <topicName>/rangefinder/range</topicName>
           <frameName>rangefinder</frameName>
           <radiation>infrared</radiation>

clover_simulation/launch/simulator.launch

@@ -3,7 +3,7 @@
     <arg name="mav_id" default="0"/>
     <arg name="est" default="ekf2"/> <!-- PX4 estimator: lpe, ekf2 -->
     <arg name="vehicle" default="clover"/> <!-- PX4 vehicle configuration: clover, clover_vpe -->
-    <arg name="main_camera" default="true"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
+    <arg name="main_camera" default="false"/> <!-- Simulated vision position estimation camera (optical flow, ArUco) -->
     <arg name="maintain_camera_rate" default="false"/> <!-- Slow simulation down to maintain camera rate -->
     <arg name="rangefinder" default="true"/> <!-- Simulated downward-facing rangefinder, vl53l1x-like -->
     <arg name="led" default="true"/> <!-- Simulated LED strip, ws281x-like -->

clover_simulation/resources/worlds/clover.world

@@ -11,8 +11,8 @@
     </include>
 
     <scene>
-      <ambient>0.8 0.8 0.8 1</ambient>
-      <background>0.8 0.9 1 1</background>
+      <ambient>0.1 0.1 0.1 1</ambient>
+      <background>0.1 0.1 0.1 1</background>
       <shadows>false</shadows>
       <grid>false</grid>
       <origin_visual>false</origin_visual>

clover_simulation/resources/worlds/clover_aruco.world

@@ -15,8 +15,8 @@
     </include>
 
     <scene>
-      <ambient>0.8 0.8 0.8 1</ambient>
-      <background>0.8 0.9 1 1</background>
+      <ambient>0.1 0.1 0.1 1</ambient>
+      <background>0.1 0.1 0.1 1</background>
       <shadows>false</shadows>
       <grid>false</grid>
       <origin_visual>false</origin_visual>

clover_simulation/resources/worlds/clover_light.world

@@ -0,0 +1,62 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="default">
+    <!-- A global light source -->
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <include>
+      <uri>model://parquet_plane</uri>
+      <pose>0 0 -0.01 0 0 0</pose>
+    </include>
+
+    <!-- <include>
+      <uri>model://aruco_cmit_txt</uri>
+    </include> -->
+
+    <scene>
+      <ambient>0.8 0.8 0.8 1</ambient>
+      <background>0.8 0.9 1 1</background>
+      <shadows>false</shadows>
+      <grid>false</grid>
+      <origin_visual>false</origin_visual>
+    </scene>
+
+    <light type="point" name="point">
+        <pose>0 2 2 0 0 0</pose>
+        <diffuse>1 1 1 1</diffuse>
+        <specular>.1 .1 .1 1</specular>
+        <attenuation>
+          <range>20</range>
+          <linear>0.2</linear>
+          <constant>0.8</constant>
+          <quadratic>0.01</quadratic>
+        </attenuation>
+        <cast_shadows>false</cast_shadows>
+      </light>
+
+
+
+    <physics name='default_physics' default='0' type='ode'>
+      <gravity>0 0 -9.8066</gravity>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>10</iters>
+          <sor>1.3</sor>
+          <use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
+        </solver>
+        <constraints>
+          <cfm>0</cfm>
+          <erp>0.2</erp>
+          <contact_max_correcting_vel>100</contact_max_correcting_vel>
+          <contact_surface_layer>0.001</contact_surface_layer>
+        </constraints>
+      </ode>
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>250</real_time_update_rate>
+      <magnetic_field>6.0e-6 2.3e-5 -4.2e-5</magnetic_field>
+    </physics>
+  </world>
+</sdf>

clover_simulation/src/sim_leds.cpp

@@ -65,6 +65,7 @@ class LedController
 		}
 
 		role = (ros::this_node::getName() == "/gazebo") ? Role::Server : Role::Client;
+		ROS_INFO("Led node name: %s", ros::this_node::getName().c_str());
 		ROS_INFO_NAMED(("LedController_" + robotNamespace).c_str(), "LedController has started (as %s)", role == Role::Client ? "client" : "server");
 
 		nh.reset(new ros::NodeHandle(robotNamespace));