modify readme

This commit is contained in:
robin_shaun 2021-02-27 08:21:30 +08:00
parent e4becb504f
commit 3dcec7b6d3
5 changed files with 59 additions and 22 deletions

Binary file not shown.

Before

Width:  |  Height:  |  Size: 60 KiB

After

Width:  |  Height:  |  Size: 61 KiB

0
image/architecture_2.png Normal file → Executable file
View File

Before

Width:  |  Height:  |  Size: 178 KiB

After

Width:  |  Height:  |  Size: 178 KiB

View File

@ -1,21 +0,0 @@
import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import sys
import tf
local_pose = PoseStamped()
def odm_groundtruth_callback(msg):
local_pose.header = msg.header
local_pose.pose = msg.pose.pose
if __name__ == '__main__':
rospy.init_node('get_pose_groundtruth')
odom_groundtruth_sub = rospy.Subscriber('/xtdrone/ground_truth/odom', Odometry, odm_groundtruth_callback)
pose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=2)
rate = rospy.Rate(100)
while True:
pose_pub.publish(local_pose)
rate.sleep()

View File

@ -0,0 +1,59 @@
<?xml version="1.0"?>
<launch>
<!-- MAVROS posix SITL environment launch script -->
<!-- launches Gazebo environment and 2x: MAVROS, PX4 SITL, and spawns vehicle -->
<!-- vehicle model and world -->
<arg name="est" default="ekf2"/>
<arg name="vehicle" default="iris"/>
<arg name="world" default="worlds/empty.world"/>
<!-- gazebo configs -->
<arg name="gui" default="true"/>
<arg name="debug" default="false"/>
<arg name="verbose" default="false"/>
<arg name="paused" default="false"/>
<!-- Gazebo sim -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="gui" value="$(arg gui)"/>
<arg name="world_name" value="$(arg world)"/>
<arg name="debug" value="$(arg debug)"/>
<arg name="verbose" value="$(arg verbose)"/>
<arg name="paused" value="$(arg paused)"/>
</include>
<!-- UAV0 -->
<group ns="iris_0">
<!-- MAVROS and vehicle configs -->
<arg name="ID" value="0"/>
<arg name="fcu_url" default="udp://:24540@localhost:34580"/>
<!-- PX4 SITL and vehicle spawn -->
<include file="$(find px4)/launch/single_vehicle_spawn.launch">
<arg name="x" value="0.5"/>
<arg name="y" value="0"/>
<arg name="z" value="0"/>
<arg name="R" value="0"/>
<arg name="P" value="0"/>
<arg name="Y" value="0"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="mavlink_udp_port" value="18570"/>
<arg name="mavlink_tcp_port" value="4560"/>
<arg name="ID" value="$(arg ID)"/>
</include>
<!-- MAVROS -->
<include file="$(find mavros)/launch/px4.launch">
<arg name="fcu_url" value="$(arg fcu_url)"/>
<arg name="gcs_url" value=""/>
<arg name="tgt_system" value="$(eval 1 + arg('ID'))"/>
<arg name="tgt_component" value="1"/>
</include>
</group>
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find le_arm)/urdf/le_arm.xacro'" />
<include file="$(find le_arm)/launch/controller_utils.launch"/>
<rosparam file="$(find le_arm)/controller/le_arm_controller.yaml" command="load"/>
<node name="le_arm_controller_spawner" pkg="controller_manager" type="spawner" args="le_arm_controller" />
<rosparam file="$(find le_arm)/controller/gripper_controller.yaml" command="load"/>
<node name="gripper_controller_spawner" pkg="controller_manager" type="spawner" args="gripper_controller" />
</launch>
<!-- to add more UAVs (up to 10):
Increase the id
Change the name space
Set the FCU to default="udp://:14540+id@localhost:14550+id"
Set the malink_udp_port to 14560+id) -->

@ -1 +0,0 @@
Subproject commit 9ad2fbf962e7ac46991ced5ce81a6536895bed6c