Merge remote-tracking branch 'origin/devel' into devel

robot_high_level_control/scripts/state_machine.py

@@ -28,10 +28,13 @@ def __init__(self, starting_state="drive_to_p0"):
         # Variables to use for
         self.state_vars = {
             "outbound": True,
-            "P0": (2.0, 1.0),
+            "P0": (1.75, 1.0),
             "P1": (1.5, 2.0),
             "P2": (1.5, 4.0),
             "P3": (1.5, 5.0),
+            "PA1": (1.0, 1.0),
+            "PA2": (0.5, 1.0),
+            "PA3": (0.4, 1.0),
             "drivability_map": None,
             "max_x_offset": 1.5,
             "P1_y": 2.5,

robot_misc_tools/scripts/tf_configs_static/full_system.json

@@ -4,7 +4,7 @@
             "frame_id" : "camera_mount_base",
             "child_id" : "robot_frame_BL",
             "trans_x" : 0,
-            "trans_y" : 0,
+            "trans_y" : 0.235,
             "trans_z" : 0,
             "rot_x" : 0,
             "rot_y" : 0,
@@ -26,6 +26,17 @@
             "frame_id" : "robot_frame_BL",
             "child_id" : "robot_frame_BR",
             "trans_x" : 0,
+            "trans_y" : -0.45,
+            "trans_z" : 0,
+            "rot_x" : 0,
+            "rot_y" : 0,
+            "rot_z" : 0,
+            "rot_w" : 1
+        },
+        {
+            "frame_id" : "robot_frame_BL",
+            "child_id" : "robot_frame_FLJ",
+            "trans_x" : 0.845,
             "trans_y" : 0,
             "trans_z" : 0,
             "rot_x" : 0,
@@ -36,7 +47,7 @@
         {
             "frame_id" : "robot_frame_BL",
             "child_id" : "robot_frame_FL",
-            "trans_x" : 0,
+            "trans_x" : 1.07,
             "trans_y" : 0,
             "trans_z" : 0,
             "rot_x" : 0,
@@ -47,7 +58,7 @@
         {
             "frame_id" : "robot_frame_BR",
             "child_id" : "robot_frame_FR",
-            "trans_x" : 0,
+            "trans_x" : 0.845,
             "trans_y" : 0,
             "trans_z" : 0,
             "rot_x" : 0,

robot_mission_control/launch/localization.launch

@@ -1,10 +1,13 @@
 <launch>
 
     <machine name="jetson" address="192.168.0.103" user="vur" password="theswapbot" env-loader="~/catkin_ws/devel_isolated/robot_ros_env.sh"/>
+    <machine name="rpi" address="192.168.0.105" user="vur" password="theswapbot" env-loader="~/catkin_ws/devel/robot_ros_env.sh"/>
+
+    <!-- JETSON NODES -->
 
     <!--Transform publisher -->
-    <param name="tf_file_static" value="full_system_test.json"/>
-    <param name="tf_file_dynamic" value="full_system_test.json"/>
+    <param name="tf_file_static" value="full_system.json"/>
+    <param name="tf_file_dynamic" value="full_system.json"/>
     <node machine="jetson" name="transform_publisher" pkg="robot_misc_tools" type="transform_publisher.py"/>
 
     <!-- Image publisher -->
@@ -26,6 +29,13 @@
     <param name="unfiltered_pose_topic" value="aruco/pose_raw"/>
     <node machine="jetson" name="robot_pose_ekf" pkg="robot_slam" type="robot_pose_ekf.py"/>
 
+    <!-- RASPBERRY PI NODES -->
+
+    <!-- Camera mount servo control node -->
+    <node machine="rpi" name="camera_motor_control" pkg="robot_motor_control" type="camera_motor_control.py"/>
+
+    <!-- LAPTOP NODES -->
+
     <!-- Field Visualization -->
     <node name="field_visualization" pkg="robot_simulation" type="field_visualization.py"/>
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find robot_simulation)/rviz/competition_config.rviz"/>

robot_motor_control/cfg/Talon.cfg

@@ -8,6 +8,7 @@ gen = ParameterGenerator()
 gen.add("id",               int_t,      0,  "ID Number",        0,      0,      64)
 gen.add("inverted",         bool_t,     0, "Inverted",          False)
 gen.add("peak_voltage",     double_t,   0,  "Peak Voltage",     12.0,   0.0,    28)
+gen.add("pot",              bool_t,     0, "Potentiometer",     False)
 
 gen.add("P", double_t, 0, "P", 0, 0, 100)
 gen.add("I", double_t, 0, "I", 0, 0, 100)

robot_motor_control/include/robot_motor_control/TalonNode.h

@@ -24,6 +24,7 @@ namespace robot_motor_control{
         ros::NodeHandle nh;
         std::string _name;
         dynamic_reconfigure::Server<robot_motor_control::TalonConfig> server;
+        TalonConfig _config;
 
         std::unique_ptr<TalonSRX> talon;
 
@@ -42,17 +43,19 @@ namespace robot_motor_control{
         ControlMode _controlMode;
         double _output;
         bool disabled;
+        bool configured;
+        bool not_configured_warned;
 
     public:
-        TalonNode(const ros::NodeHandle& parent, const std::string& name);
+        TalonNode(const ros::NodeHandle& parent, const std::string& name, const TalonConfig& config);
 
         TalonNode& operator=(const TalonNode&) = delete;
 
         ~TalonNode() = default;
 
         void reconfigure(const TalonConfig &config, uint32_t level);
 
-        void updateConfig(const TalonConfig &config);
+        void configure();
 
         void setPercentOutput(std_msgs::Float64 output);
 

robot_motor_control/scripts/camera_motor_control.py

@@ -6,35 +6,42 @@
 
 class AngleListener:
 
-    def __init__(self, pinNumber):
-
-        # Configure GPIO stuff
-        GPIO.setmode(GPIO.BOARD)
-        GPIO.setwarnings(False)
+    def __init__(self, pinNumber, max_angle):
 
         # Set the pin as an output
         self.p = GPIO.setup(pinNumber, GPIO.OUT)
         self.p = GPIO.PWM(pinNumber, 50)
 
+        # Store max angle to use for scaling
+        self.max_angle = max_angle
+
         # Start
         self.p.start(2.5)
 
     def setCameraAngle(self, theta):
 
-        # Convert from range -90 to 90 to range 2.22 to 10.0 (this is the good range for the servo)
-        duty_cycle = ((theta.data + 65) / 130.0) * (7.88) + 2.22
+        rospy.loginfo(theta.data)
+
+        # Convert from range -90 to 90 to range 0 to 100
+        duty_cycle = ((theta.data + self.max_angle) / (self.max_angle * 2)) * (7.5) + 2.5
+
+	rospy.loginfo(duty_cycle)
 
         # Send new duty cycle to servo
         self.p.ChangeDutyCycle(duty_cycle)
 
 
 if __name__ == '__main__':
 
+    # Initialize GPIO
+    GPIO.setmode(GPIO.BOARD)
+    GPIO.setwarnings(False)
+
     # Initialize ROS node
     rospy.init_node('camera_control_node', anonymous=True)
 
     # Create listener object
-    listener = AngleListener(5)
+    listener = AngleListener(18, 135.0 / 2)
 
     # Attach subscriber to servo angle topic
     rospy.Subscriber("aruco/servo_theta", Int32, listener.setCameraAngle)

robot_motor_control/scripts/digging_control.py

@@ -6,7 +6,7 @@
 
 class DigManager:
 
-    def __init__(self):
+    def __init__(self, ):
 
         # Current dig mode - can be "teleop" or "auto"
         self.dig_mode = "teleop"

robot_motor_control/src/MotorControl.cpp

@@ -25,6 +25,8 @@ int main(int argc, char **argv) {
                 config.inverted = (bool)v["inverted"];
             if(v.hasMember("peak_voltage"))
                 config.peak_voltage = (double)v["peak_voltage"];
+            if(v.hasMember("pot"))
+                config.pot = (bool)v["pot"];
             if(v.hasMember("P"))
                 config.P = (double)v["P"];
             if(v.hasMember("I"))
@@ -35,8 +37,7 @@ int main(int argc, char **argv) {
                 config.F = (double)v["F"];
 
             auto node = ros::NodeHandle(nh, name);
-            std::shared_ptr<TalonNode> talon = std::make_shared<TalonNode>(node, name);
-            talon->updateConfig(config);
+            std::shared_ptr<TalonNode> talon = std::make_shared<TalonNode>(node, name, config);
             talons.push_back(talon);
             ROS_INFO("Created Talon with name '%s' and id '%d'", name.c_str(), config.id);
        }else{

robot_motor_control/src/TalonNode.cpp

@@ -10,8 +10,8 @@ using namespace ctre::phoenix::motorcontrol::can;
 
 namespace robot_motor_control {
 
-    TalonNode::TalonNode(const ros::NodeHandle& parent, const std::string& name) :
-            nh(parent), _name(name), server(nh), talon(new TalonSRX(0)),
+    TalonNode::TalonNode(const ros::NodeHandle& parent, const std::string& name, const TalonConfig& config):
+            nh(parent), _name(name), server(nh), _config(config), talon(new TalonSRX(_config.id)),
             tempPub(nh.advertise<std_msgs::Float64>("temperature", 1)),
             busVoltagePub(nh.advertise<std_msgs::Float64>("bus_voltage", 1)),
             outputPercentPub(nh.advertise<std_msgs::Float64>("output_percent", 1)),
@@ -21,10 +21,11 @@ namespace robot_motor_control {
             velPub(nh.advertise<std_msgs::Int32>("velocity", 1)),
             setPercentSub(nh.subscribe("set_percent_output", 1, &TalonNode::setPercentOutput, this)),
             setVelSub(nh.subscribe("set_velocity", 1, &TalonNode::setVelocity, this)),
-            lastUpdate(ros::Time::now()), _controlMode(ControlMode::PercentOutput), _output(0.0), disabled(false){
+            lastUpdate(ros::Time::now()), _controlMode(ControlMode::PercentOutput), _output(0.0), disabled(false),
+            configured(false), not_configured_warned(false){
         server.setCallback(boost::bind(&TalonNode::reconfigure, this, _1, _2));
+        server.updateConfig(config);
         talon->Set(ControlMode::PercentOutput, 0);
-        configureStatusPeriod(*talon);
     }
 
     void TalonNode::setPercentOutput(std_msgs::Float64 output) {
@@ -40,37 +41,61 @@ namespace robot_motor_control {
     }
 
     void TalonNode::reconfigure(const TalonConfig &config, uint32_t level) {
-        if (config.id != 0 && talon->GetDeviceID() != config.id) {
-            ROS_INFO("Resetting TalonNode to new id: %d", config.id);
-            talon = std::make_unique<TalonSRX>(config.id);
-            configureStatusPeriod(*talon);
+        this->_config = config;
+        this->configured = false;
+        this->configure();
+    }
+
+    void TalonNode::configure(){
+        if (talon->GetDeviceID() != _config.id) {
+            ROS_INFO("Resetting TalonNode to new id: %d", _config.id);
+            talon = std::make_unique<TalonSRX>(_config.id);
         }
-        ROS_INFO("Reconfiguring Talon: %s", _name.c_str());
+
+        configureStatusPeriod(*talon);
 
         TalonSRXConfiguration c;
         SlotConfiguration slot;
-        slot.kP = config.P;
-        slot.kI = config.I;
-        slot.kD = config.D;
-        slot.kF = config.F;
+        slot.kP = _config.P;
+        slot.kI = _config.I;
+        slot.kD = _config.D;
+        slot.kF = _config.F;
         c.slot0 = slot;
-        c.voltageCompSaturation = config.peak_voltage;
+        c.voltageCompSaturation = _config.peak_voltage;
         c.pulseWidthPeriod_EdgesPerRot = 4096;
-        talon->ConfigAllSettings(c);
+        ErrorCode error = talon->ConfigAllSettings(c, 50);
+
+        if(_config.pot){
+            talon->ConfigSelectedFeedbackSensor(FeedbackDevice::Analog);
+        }else{
+            talon->ConfigSelectedFeedbackSensor(FeedbackDevice::CTRE_MagEncoder_Relative);
+        }
 
         talon->SelectProfileSlot(0,0);
-        talon->SetInverted(config.inverted);
+        talon->SetInverted(_config.inverted);
         talon->EnableVoltageCompensation(true);
-    }
 
-    void TalonNode::updateConfig(const TalonConfig &config){
-        server.updateConfig(config);
+        if(error != ErrorCode::OK){
+            if(!this->not_configured_warned){
+                ROS_WARN("Reconfiguring Talon %s %d failed!", _name.c_str(), talon->GetDeviceID());
+                this->not_configured_warned = true;
+            }
+            this->configured = false;
+        }else{
+            ROS_INFO("Reconfigured Talon: %s with %d %f %f %f", _name.c_str(), talon->GetDeviceID(),
+                     _config.P, _config.I, _config.D);
+            this->not_configured_warned = false;
+            this->configured = true;
+        }
     }
 
     void TalonNode::update(){
+        if(!this->configured){
+            this->configure();
+        }
         if(ros::Time::now()-lastUpdate > ros::Duration(0.2)){
             // Disable the Talon if we aren't getting commands
-            if(!this->disabled) ROS_INFO("Talon disabled for not receiving updates: %s", _name.c_str());
+            if(!this->disabled) ROS_WARN("Talon disabled for not receiving updates: %s", _name.c_str());
             this->disabled = true;
             this->_controlMode = ControlMode::PercentOutput;
             this->_output = 0.0;