/ - Diff - Colony Scout OS - Robotics Club

Revision 75a7571a

ID	75a7571a58c8e315beec4bf6a966437fdc0765b7

Added by Priya over 12 years ago

Finished skeleton of cliffsensor node. Compiles.

scout/cliffsensor/CMakeLists.txt
29	29	#rosbuild_add_executable(example examples/example.cpp)
30	30	#target_link_libraries(example ${PROJECT_NAME})
31	31
32		rosbuild_add_executable(motors_node src/motors.cpp)
	32	rosbuild_add_executable(cliffsensor_node src/cliffsensor.cpp)

     static bool is_cliff; /**< Boolean to represent whether or not there is a cliff. */
     /**
      * @brief Sets motor speed
      * @brief Changes cliff sensor status
+     *
      * Sets the motor speeds based on subscription to the set_motors topic.
      * Changes cliff sensor status based on subscription to topic cliff_status_changed
+     *
      * @param msg The message from the set_motors topic, containing speeds and
      *  motor configuration settings.
      * @param msg The message from the cliff_status_changed topic, containing
      * status of all cliff sensors.
      */
     void motors_set(const motors::set_motors::ConstPtr& msg)
     void changed_cliff_status(const cliffsensor::cliff_status_changed::ConstPtr& msg)
+    {
         /** @todo Edit to only set requested motors, not all */
         int which = msg->which;
         if(which & MOTOR_FL_REV)
+        {
           motor_fl_speed = -1 * motors_rel_to_abs(msg->fl_speed, msg->units);
+        }
         if(which & MOTOR_FR_REV)
+        {
           motor_fr_speed = -1 * motors_rel_to_abs(msg->fr_speed, msg->units);
+        }
         if(which & MOTOR_BL_REV)
+        {
           motor_bl_speed = -1 * motors_rel_to_abs(msg->bl_speed, msg->units);
+        }
         if(which & MOTOR_BR_REV)
+        {
           motor_br_speed = -1 * motors_rel_to_abs(msg->br_speed, msg->units);
+        }
         if(which & MOTOR_FL)
+        {
           motor_fl_speed = motors_rel_to_abs(msg->fl_speed, msg->units);
+        }
         if(which & MOTOR_FR)
+        {
           motor_fr_speed = motors_rel_to_abs(msg->fr_speed, msg->units);
+        }
         if(which & MOTOR_BL)
+        {
           motor_bl_speed = motors_rel_to_abs(msg->bl_speed, msg->units);
+        }
         if(which & MOTOR_BR)
+        {
           motor_br_speed = motors_rel_to_abs(msg->br_speed, msg->units);
+        }
         /* Write speeds to hardware */
         /** @todo Add code to write speeds to hardware */
       return;
+    }
     /**
      * @brief Outputs current motor speeds
      * @brief Outputs status of cliffsensors
+     *
      * Serves the service query_motors by responding to service requests with the
      * speeds of the motors.
      * Serves the service query_cliff by responding to service requests with the
      * status of the cliff sensors.
      * @param req The request. The only field is the units requested.
      * @param res The response. The fields will be filled with values.
      */
     bool motors_query(motors::query_motors::Request &req,
                       motors::query_motors::Response &res)
     bool cliff_query(cliffsensor::query_cliff::Request &req,
                       cliffsensor::query_cliff::Response &res)
+    {
         int units = req.units;
         res.fl_speed = motors_abs_to_rel(motor_fl_speed, units);
         res.fr_speed = motors_abs_to_rel(motor_fr_speed, units);
         res.bl_speed = motors_abs_to_rel(motor_bl_speed, units);
         res.br_speed = motors_abs_to_rel(motor_br_speed, units);
         int threshold = 500; //TODO fix after figuring out what sensors give us
         res.front_raw = front_raw;
         res.left_raw = left_raw;
         res.right_raw = back_raw;
         res.cliff_status = is_cliff;
         ROS_DEBUG("Motor speeds queried");
         ROS_DEBUG("Cliffsensor status queried");
         return true;
+    }
     /**
      * @brief Converts set speeds (of various units) to absolute speeds.
+     *
      * @param speed The speed expressed in the desired units
      * @param units The units the desired speed is measured in
      * @return The absolute speed of the motor
      **/
     int motors_rel_to_abs(int rel_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return rel_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return rel_speed * MAXSPEED / 100;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return rel_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return rel_speed;
+        }
+    }
     /**
      * @brief Convert absolute speeds to speeds of various units.
+     *
      * @param speed The speed expressed in absolute units.
      * @param units The units the desired speed is measured in.
      * @return The relative speed of the motor in desired units.
      **/
     int motors_abs_to_rel(int abs_speed, int units)
+    {
         switch(units)
+        {
           case MOTOR_ABSOLUTE:/* Speed given as absolute */
             return abs_speed;
           case MOTOR_PERCENT:/* Convert from percentage */
             return abs_speed * 100 / MAXSPEED;
           case MOTOR_MMS:/* Convert from mm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           case MOTOR_CMS:/* Convert from cm/s */
             /** @todo Make math to do this conversion **/
             return abs_speed;
           default: /* The units aren't recognized */
             /** @todo Decide on default case. Either percent or absolute. **/
             return abs_speed;
+        }
+    }
     /**
      * @brief Motors driver. This is a ROS node that controls motor speeds.
      * @brief Cliffsensor driver. This is a ROS node that controls cliffsensor status.
+     *
      * This is the main function for the motors node. It is run when the node
      * starts and initializes the motors. It then subscribes to the
      * set_motors, and set_motor_speeds topics, and advertises the
      * query_motors service.
      * This is the main function for the cliffsensors node. It is run when the node
      * starts and initializes the cliffsensors. It then subscribes to the
      * cliff_status_changed topics, and advertises the
      * query_cliff service.
+     *
      * @param argc The number of command line arguments (should be 1)
      * @param argv The array of command line arguments
      **/
     int main(int argc, char **argv)
+    {
         /* Initialize in ROS the motors driver node */
         ros::init(argc, argv, "motors_driver");
         /* Initialize in ROS the cliffsensor driver node */
         ros::init(argc, argv, "cliffsensor_driver");
         /* Advertise that this serves the query_motors service */
         /* Advertise that this serves the query_cliff service */
         ros::NodeHandle n;
         ros::ServiceServer service = n.advertiseService("query_motors",
                                                         motors_query);
         ros::ServiceServer service = n.advertiseService("query_cliff",
                                                         cliff_query);
         /* Subscribe to the set_motors topic */
         ros::Subscriber sub0 = n.subscribe("set_motors", 4, motors_set);
         /* Subscribe to the cliff_status_changed topic */
         ros::Subscriber sub0 = n.subscribe("cliff_status_changed",
 , changed_cliff_status);
         /* Initialize hardware for motors */
         /* Initialize hardware for cliffsensors */
         // Hardware init functions here
         ROS_INFO("Ready to set motors.");
         ROS_INFO("Ready to query cliffsensors.");
         ros::spin();
         return 0;

Also available in: Unified diff

Project

General

Profile

Colony » Colony Scout » Colony Scout OS

Revision 75a7571a