Commit 9bffcd19 authored by srees's avatar srees
Browse files

Import JD's changes to F450 branch

parent 259e4cbd
- git:
local-name: mavros
uri: https://github.com/cpsvo/mavros.git
version: master
- git:
local-name: mavlink
uri: https://github.com/mavlink/mavlink-gbp-release.git
version: release/indigo/mavlink/2016.2.5-0
<launch>
<!-- vim: set ft=xml noet : -->
<!-- example launch script for PX4 based FCU's -->
<!-- these are the arguments you can pass this launch file, for example paused:=true -->
<arg name="paused" default="false"/>
<arg name="use_sim_time" default="true"/>
<arg name="extra_gazebo_args" default=""/>
<arg name="gui" default="false"/>
<arg name="headless" default="false"/>
<arg name="debug" default="false"/>
<arg name="physics" default="ode"/>
<arg name="verbose" default="false"/>
<arg name="world_name" default="$(find mavros)/launch/nsf-uav-fron-down-cam.world"/> <!-- Note: the world_name is with respect to GAZEBO_RESOURCE_PATH environmental variable -->
<arg name="fcu_url" default="/dev/ttyACM0:57600" />
<arg name="gcs_url" default="" />
<arg name="tgt_system" default="1" />
<arg name="tgt_component" default="1" />
<arg name="log_output" default="screen" />
<include file="$(find mavros)/launch/node.launch">
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
<arg name="fcu_url" value="$(arg fcu_url)" />
<arg name="gcs_url" value="$(arg gcs_url)" />
<arg name="tgt_system" value="$(arg tgt_system)" />
<arg name="tgt_component" value="$(arg tgt_component)" />
<arg name="log_output" value="$(arg log_output)" />
</include>
<!-- set use_sim_time flag -->
<group if="$(arg use_sim_time)">
<param name="/use_sim_time" value="true" />
</group>
<!-- set command arguments -->
<arg unless="$(arg paused)" name="command_arg1" value=""/>
<arg if="$(arg paused)" name="command_arg1" value="-u"/>
<arg unless="$(arg headless)" name="command_arg2" value=""/>
<arg if="$(arg headless)" name="command_arg2" value="-r"/>
<arg unless="$(arg verbose)" name="command_arg3" value=""/>
<arg if="$(arg verbose)" name="command_arg3" value="--verbose"/>
<arg unless="$(arg debug)" name="script_type" value="gzserver"/>
<arg if="$(arg debug)" name="script_type" value="debug"/>
<!-- start gazebo server-->
<node name="gazebo" pkg="gazebo_ros" type="$(arg script_type)" respawn="false" output="screen"
args="$(arg command_arg1) $(arg command_arg2) $(arg command_arg3) -e $(arg physics) $(arg extra_gazebo_args) $(arg world_name)" />
<!-- start gazebo client -->
<group if="$(arg gui)">
<node name="gazebo_gui" pkg="gazebo_ros" type="gzclient" respawn="false" output="screen"/>
</group>
</launch>
......@@ -15,7 +15,7 @@
<param name="gcs_url" value="$(arg gcs_url)" />
<param name="target_system_id" value="$(arg tgt_system)" />
<param name="target_component_id" value="$(arg tgt_component)" />
<param name="system_id" value="34" />
<!-- load blacklist, config -->
<rosparam command="load" file="$(arg pluginlists_yaml)" />
<rosparam command="load" file="$(arg config_yaml)" />
......
{
"keys": {},
"groups": {
"pluginmanager": {
"keys": {
"running-plugins": {
"type": "repr",
"repr": "{u'rqt_image_view/ImageView': [2, 1]}"
}
},
"groups": {
"plugin__rqt_image_view__ImageView__2": {
"keys": {},
"groups": {
"dock_widget__ImageViewWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"plugin": {
"keys": {
"dynamic_range": {
"type": "repr",
"repr": "False"
},
"max_range": {
"type": "repr",
"repr": "10.0"
},
"topic": {
"type": "repr",
"repr": "u'/uav_camera/image_raw_down'"
},
"zoom1": {
"type": "repr",
"repr": "False"
}
},
"groups": {}
}
}
},
"plugin__rqt_image_view__ImageView__1": {
"keys": {},
"groups": {
"dock_widget__ImageViewWidget": {
"keys": {
"dockable": {
"type": "repr",
"repr": "True"
},
"parent": {
"type": "repr",
"repr": "None"
}
},
"groups": {}
},
"plugin": {
"keys": {
"dynamic_range": {
"type": "repr",
"repr": "False"
},
"max_range": {
"type": "repr",
"repr": "10.0"
},
"topic": {
"type": "repr",
"repr": "u'/uav_camera/image_raw_front'"
},
"zoom1": {
"type": "repr",
"repr": "False"
}
},
"groups": {}
}
}
}
}
},
"mainwindow": {
"keys": {
"geometry": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('01d9d0cb000100000000005a0000005a0000033c00000200000000640000008000000332000001f6000000000000')",
"pretty-print": " Z Z < d 2 "
},
"state": {
"type": "repr(QByteArray.hex)",
"repr(QByteArray.hex)": "QtCore.QByteArray('000000ff00000000fd0000000100000003000002cf00000148fc0100000002fb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0031005f005f0049006d0061006700650056006900650077005700690064006700650074010000000000000165000000c800fffffffb0000005a007200710074005f0069006d006100670065005f0076006900650077005f005f0049006d0061006700650056006900650077005f005f0032005f005f0049006d0061006700650056006900650077005700690064006700650074010000016b00000164000000db00ffffff000002cf0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')",
"pretty-print": " Zrqt_image_view__ImageView__1__ImageViewWidget Zrqt_image_view__ImageView__2__ImageViewWidget 6MinimizedDockWidgetsToolbar "
}
},
"groups": {
"toolbar_areas": {
"keys": {
"MinimizedDockWidgetsToolbar": {
"type": "repr",
"repr": "8"
}
},
"groups": {}
}
}
}
}
}
\ No newline at end of file
This diff is collapsed.
......@@ -673,7 +673,7 @@ private:
void heartbeat_cb(const ros::TimerEvent &event) {
mavlink_message_t msg;
mavlink_msg_heartbeat_pack_chan(UAS_PACK_CHAN(uas), &msg,
MAV_TYPE_ONBOARD_CONTROLLER,
MAV_TYPE_GCS,
MAV_AUTOPILOT_INVALID,
MAV_MODE_MANUAL_ARMED,
0,
......
import rospy
from mavros_msgs.srv import WaypointPush
from mavros_msgs.srv import WaypointPushRequest
from mavros_msgs.srv import WaypointSetCurrent
from mavros_msgs.srv import WaypointSetCurrentRequest
from mavros_msgs.msg import Waypoint
from mavros_msgs.srv import SetMode
from mavros_msgs.srv import CommandBool
#+---+------+------+---------+---------------+----------------+-------------------+------+-----+---------------+---------------+-------+
#| # | Curr | Auto | Frame | Command | P1 | P2 | P3 | P4 | X Lat | Y Long | Z Alt |
#+---+------+------+---------+---------------+----------------+-------------------+------+-----+---------------+---------------+-------+
#| 0 | Yes | Yes | GRA (3) | TAKEOFF (22) | 0.261799395084 | 1.65544985316e-24 | -0.0 | 0.0 | 47.3669242859 | 8.54999923706 | 25.0 |
#| 1 | No | Yes | GRA (3) | WAYPOINT (16) | 0.0 | 3.0 | -0.0 | 0.0 | 47.3667449951 | 8.55048179626 | 25.0 |
#| 2 | No | Yes | GRA (3) | LAND (21) | 25.0 | 3.0 | -0.0 | 0.0 | 47.3665084839 | 8.55013847351 | 25.0 |
#+---+------+------+---------+---------------+----------------+-------------------+------+-----+---------------+---------------+-------+
# HOME latitude: 47.3667049
# longitude: 8.5499866
def createTakeoffCurr():
# 22 MAV_CMD_NAV_TAKEOFF Takeoff from ground / hand
# Mission Param #1 Minimum pitch (if airspeed sensor present), desired pitch without sensor
# Mission Param #2 Empty
# Mission Param #3 Empty
# Mission Param #4 Yaw angle (if magnetometer present), ignored without magnetometer
# Mission Param #5 Latitude
# Mission Param #6 Longitude
# Mission Param #7 Altitude
wp = Waypoint()
wp.frame = 3
wp.command = 22
wp.autocontinue = True
wp.param1 = 0.261799395084
wp.param2 = 0.0
wp.param3 = 0.0
wp.param4 = 0.0
wp.x_lat = 47.3669242859
wp.y_long = 8.54999923706
wp.z_alt = 10.0
return wp
def createWaypoint(_visitationRadius, _lat, _lon, _alt):
wp = Waypoint()
wp.frame = 3
wp.command = 16
wp.autocontinue = True
wp.param1 = 0
wp.param2 = _visitationRadius
wp.param3 = 0
wp.param4 = 0
wp.x_lat = _lat
wp.y_long = _lon
wp.z_alt = _alt
return wp
def createLand():
wp = Waypoint()
wp.frame = 3
wp.command = 21
wp.autocontinue = True
wp.param1 = 25.0
wp.param2 = 3.0
wp.param3 = -0.0
wp.param4 = 0.0
wp.x_lat = 47.3667049
wp.y_long = 8.5499866
wp.z_alt = 10.0
return wp
if __name__ == "__main__":
rospy.wait_for_service('/mavros/mission/push')
rospy.wait_for_service('/mavros/mission/set_current')
rospy.wait_for_service('/mavros/set_mode')
waypointPushService = rospy.ServiceProxy('/mavros/mission/push', WaypointPush)
wpPushRequest = WaypointPushRequest()
waypointSetCurrService = rospy.ServiceProxy('/mavros/mission/set_current', WaypointSetCurrent)
wpPushRequest.waypoints.append(createTakeoffCurr())
wpPushRequest.waypoints.append(createWaypoint(10.0, 47.3670138753861707, 8.55059266090393066, 10.0))
wpPushRequest.waypoints.append(createWaypoint(10.0, 47.3667049, 8.5499866, 10.0))
wpPushRequest.waypoints.append(createLand())
print(waypointPushService.call(wpPushRequest))
rospy.sleep(1)
arm = rospy.ServiceProxy('mavros/cmd/arming', CommandBool)
print(arm(True))
rospy.sleep(1)
setmode = rospy.ServiceProxy('mavros/set_mode', SetMode)
print(setmode(0, 'AUTO.MISSION'))
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment