Skip to content

Commit ab401ac

Browse files
Alexey-KamenevAlexey-Kamenev
authored
Merge pull request #26 from NVIDIA-Jetson/alexeyk/rover
Rover fixes and samples
2 parents 37ba895 + 8e7d008 commit ab401ac

File tree

5 files changed

+71
-4
lines changed

5 files changed

+71
-4
lines changed

‎ros/packages/px4_controller/include/px4_controller/px4_controller.h

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,8 @@ class PX4Controller
8888
void executeCommand(const PX4Controller& ctl, const geometry_msgs::PoseStamped& goto_pose,
8989
float linear_control_val, float angular_control_val, bool has_command) override;
9090
private:
91-
float turn_angle_scale_ = 1;
91+
float linear_speed_scale_ = 1;
92+
float turn_angle_scale_ = 1;
9293
ros::Publisher rc_pub_;
9394
int rc_steer_trim_ = 1500;
9495
int rc_steer_dz_ = 30;

‎ros/packages/px4_controller/launch/aion_ctl.launch

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<launch>
2+
<arg name="vehicle_type" default="apmrover" />
3+
<arg name="altitude_gain" default="0" />
4+
<arg name="linear_speed" default="2.0" />
5+
<arg name="linear_speed_scale" default="90" />
6+
<arg name="turn_angle_scale" default="-250" />
7+
<arg name="dnn_turn_angle" default="45.0" />
8+
<arg name="dnn_lateralcorr_angle" default="45.0" />
9+
<arg name="joy_type" default="shield" />
10+
11+
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller">
12+
<param name="vehicle_type" value="$(arg vehicle_type)" />
13+
<param name="altitude_gain" value="$(arg altitude_gain)" />
14+
<param name="linear_speed" value="$(arg linear_speed)" />
15+
<param name="linear_speed_scale" value="$(arg linear_speed_scale)" />
16+
<param name="turn_angle_scale" value="$(arg turn_angle_scale)" />
17+
<param name="dnn_turn_angle" value="$(arg dnn_turn_angle)" />
18+
<param name="dnn_lateralcorr_angle" value="$(arg dnn_lateralcorr_angle)" />
19+
<param name="joy_type" value="$(arg joy_type)" />
20+
</node>
21+
</launch>
22+

‎ros/packages/px4_controller/launch/mavros_ctl.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller">
2525
<param name="linear_speed" value="$(arg linear_speed)" />
2626
<param name="altitude_gain" value="$(arg altitude_gain)" />
27-
<param name="dnn_turn_angle" value="$(arg dnn_turn_angle)" />
27+
<param name="dnn_turn_angle" value="$(arg dnn_turn_angle)" />
2828
<param name="joy_type" value="$(arg joy_type)" />
2929
</node>
3030
</launch>

‎ros/packages/px4_controller/launch/nomad_ctl.launch

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
<launch>
2+
<arg name="vehicle_type" default="apmrover" />
3+
<arg name="altitude_gain" default="0" />
4+
<arg name="linear_speed" default="2.0" />
5+
<arg name="linear_speed_scale" default="120" />
6+
<arg name="turn_angle_scale" default="-200" />
7+
<arg name="dnn_turn_angle" default="15.0" />
8+
<arg name="dnn_lateralcorr_angle" default="15.0" />
9+
<arg name="joy_type" default="shield" />
10+
11+
<node pkg="px4_controller" type="px4_controller_node" name="px4_controller">
12+
<param name="vehicle_type" value="$(arg vehicle_type)" />
13+
<param name="altitude_gain" value="$(arg altitude_gain)" />
14+
<param name="linear_speed" value="$(arg linear_speed)" />
15+
<param name="linear_speed_scale" value="$(arg linear_speed_scale)" />
16+
<param name="turn_angle_scale" value="$(arg turn_angle_scale)" />
17+
<param name="dnn_turn_angle" value="$(arg dnn_turn_angle)" />
18+
<param name="dnn_lateralcorr_angle" value="$(arg dnn_lateralcorr_angle)" />
19+
<param name="joy_type" value="$(arg joy_type)" />
20+
</node>
21+
</launch>
22+

‎ros/packages/px4_controller/src/px4_controller.cpp

Lines changed: 24 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
#include "px4_controller/px4_controller.h"
1313
#include <boost/algorithm/string.hpp>
1414
#include <mavros_msgs/ParamGet.h>
15+
#include <mavros_msgs/ParamSet.h>
1516

1617
namespace px4_control
1718
{
@@ -41,7 +42,8 @@ void PX4Controller::Drone::executeCommand(const PX4Controller& ctl, const geomet
4142

4243
bool PX4Controller::APMRover::init(ros::NodeHandle& nh)
4344
{
44-
nh.param("turn_angle_scale", turn_angle_scale_, 1.0f);
45+
nh.param("linear_speed_scale", linear_speed_scale_, 1.0f);
46+
nh.param("turn_angle_scale", turn_angle_scale_, 1.0f);
4547

4648
const int QUEUE_SIZE = 1;
4749
rc_pub_ = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", QUEUE_SIZE);
@@ -73,11 +75,26 @@ bool PX4Controller::APMRover::init(ros::NodeHandle& nh)
7375
rc_throttle_dz_ = (int)param_get.response.value.integer;
7476
nh.param("rc_throttle_dz", rc_throttle_dz_, rc_throttle_dz_);
7577

78+
// APM requires setting SYSID_MYGCS to enable RC override.
79+
auto set_param_client = nh.serviceClient<mavros_msgs::ParamSet>("/mavros/param/set");
80+
mavros_msgs::ParamSet param_set;
81+
82+
param_set.request.param_id = "SYSID_MYGCS";
83+
// REVIEW alexeyk: make a parameter?
84+
const int gcs_id = 1;
85+
param_set.request.value.integer = gcs_id;
86+
if (set_param_client.call(param_set) && param_set.response.success)
87+
ROS_INFO("(%s) Set SYSID_MYGCS to %d", getName().c_str(), gcs_id);
88+
else
89+
ROS_WARN("(%s) Failed to set SYSID_MYGCS to %d", getName().c_str(), gcs_id);
90+
91+
is_initialized_ = true;
7692
return true;
7793
}
7894

7995
void PX4Controller::APMRover::printArgs()
8096
{
97+
ROS_INFO("(%s) Speed scale : %.1f", getName().c_str(), linear_speed_scale_);
8198
ROS_INFO("(%s) Turn angle scale : %.1f", getName().c_str(), turn_angle_scale_);
8299
ROS_INFO("(%s) Steer trim : %d", getName().c_str(), rc_steer_trim_);
83100
ROS_INFO("(%s) Steer deadzone : %d", getName().c_str(), rc_steer_dz_);
@@ -101,7 +118,7 @@ void PX4Controller::APMRover::executeCommand(const PX4Controller& ctl, const geo
101118
int steer_delta = turn_angle_scale_ * angular_control_val;
102119
int steer_dz = steer_delta != 0 ? copysign(rc_steer_dz_, steer_delta) : 0;
103120
rc_override.channels[0] = rc_steer_trim_ + steer_dz + steer_delta;
104-
int throttle_delta = ctl.linear_speed_ * linear_control_val;
121+
int throttle_delta = linear_speed_scale_ * ctl.linear_speed_ * linear_control_val;
105122
int throttle_dz = throttle_delta != 0 ? copysign(rc_throttle_dz_, throttle_delta) : 0;
106123
rc_override.channels[2] = rc_throttle_trim_ + throttle_dz + throttle_delta;
107124
if(has_command)
@@ -765,7 +782,12 @@ void PX4Controller::spin()
765782
dnn_commands_count_++;
766783
}
767784
else
785+
{
786+
// We get here only when there is no commands from DNN or no joystick movements outside of deadzone.
787+
// Clearing the flag is currently requried only for rover.
788+
has_command = false;
768789
break;
790+
}
769791
}
770792

771793
// Log

0 commit comments

Comments
 (0)