Revision 0121ead7
Added the ROS stack with the libscout, motors, and sonar packages to the reposititory, and associated files. There are probably still things missing. Libscout won't run properly, but it and motors will compile. Sonar is probably broken, but nothing depends on it yet, so this shouldn't be an issue.
| scout/libscout/src/libmotors.cpp | ||
|---|---|---|
| 1 | 1 |
/** |
| 2 |
* Copyright (c) 2007 Colony Project
|
|
| 2 |
* Copyright (c) 2011 Colony Project
|
|
| 3 | 3 |
* |
| 4 | 4 |
* Permission is hereby granted, free of charge, to any person |
| 5 | 5 |
* obtaining a copy of this software and associated documentation |
| ... | ... | |
| 39 | 39 |
|
| 40 | 40 |
#include "libmotors.h" |
| 41 | 41 |
|
| 42 |
/* ROS node created in libscout.cpp */ |
|
| 43 |
extern ros::NodeHandle libscout_node; |
|
| 44 |
|
|
| 45 |
/** ROS publisher and client declaration */ |
|
| 46 |
ros::Publisher set_motors_publisher; |
|
| 47 |
ros::ServiceClient query_motors_client; |
|
| 48 |
|
|
| 49 |
/*! |
|
| 50 |
* \brief Initialize the motors module of libscout. |
|
| 51 |
* |
|
| 52 |
* Initialize the libscout node as a publisher of set_motors and a client of |
|
| 53 |
* query_motors. |
|
| 54 |
*/ |
|
| 42 | 55 |
void libmotors_init(){
|
| 43 | 56 |
set_motors_publisher = libscout_node.advertise<motors::set_motors>("libmotors", 10);
|
| 44 | 57 |
query_motors_client = libscout_node.serviceClient<motors::query_motors>("libmotors");
|
| 45 | 58 |
} |
| 46 | 59 |
|
| 47 |
int motors_set(int speed, int which){
|
|
| 60 |
/*! |
|
| 61 |
* \brief Set motor speeds |
|
| 62 |
* Sets the speeds of the motors as a percentage of top speed. Can selectively |
|
| 63 |
* select which motors to set, and which to remain at previous speed. |
|
| 64 |
* \param speed The speed the motors should be set to |
|
| 65 |
* \param which A bitmask of which motors should be set |
|
| 66 |
* \return Function status |
|
| 67 |
*/ |
|
| 68 |
int motors_set(int speed, int which, char units=MOTOR_PERCENT){
|
|
| 48 | 69 |
/** \todo Set fields of the message based on params */ |
| 49 | 70 |
|
| 50 | 71 |
if(!ros::ok()){
|
| 51 | 72 |
return LIB_ERROR; |
| 52 | 73 |
} |
| 53 |
|
|
| 74 |
|
|
| 75 |
/* Set the speed for each motor according to the which bitmask*/ |
|
| 54 | 76 |
motors::set_motors msg; |
| 55 |
msg.fl_speed = speed; |
|
| 56 |
msg.fr_speed = speed; |
|
| 57 |
msg.bl_speed = speed; |
|
| 58 |
msg.br_speed = speed; |
|
| 77 |
if(which & MOTOR_FL_REV){
|
|
| 78 |
msg.fl_speed = -1 * speed; |
|
| 79 |
} |
|
| 80 |
if(which & MOTOR_FR_REV){
|
|
| 81 |
msg.fr_speed = -1 * speed; |
|
| 82 |
} |
|
| 83 |
if(which & MOTOR_BL_REV){
|
|
| 84 |
msg.bl_speed = -1 * speed; |
|
| 85 |
} |
|
| 86 |
if(which & MOTOR_BR_REV){
|
|
| 87 |
msg.br_speed = -1 * speed; |
|
| 88 |
}if(which & MOTOR_FL){
|
|
| 89 |
msg.fl_speed = speed; |
|
| 90 |
} |
|
| 91 |
if(which & MOTOR_FR){
|
|
| 92 |
msg.fr_speed = speed; |
|
| 93 |
} |
|
| 94 |
if(which & MOTOR_BL){
|
|
| 95 |
msg.bl_speed = speed; |
|
| 96 |
} |
|
| 97 |
if(which & MOTOR_BR){
|
|
| 98 |
msg.br_speed = speed; |
|
| 99 |
} |
|
| 100 |
|
|
| 101 |
/* Specify which units the speeds are given in */ |
|
| 102 |
msg.units = units; |
|
| 59 | 103 |
|
| 60 |
// set_motors_publisher.publish(msg); |
|
| 104 |
/* Publishes message to set_motors topic */ |
|
| 105 |
set_motors_publisher.publish(msg); |
|
| 61 | 106 |
ros::spinOnce(); |
| 62 | 107 |
|
| 63 | 108 |
return LIB_OK; |
| 64 | 109 |
} |
| 65 | 110 |
|
| 66 |
int motors_speed_set(int speed, int which){
|
|
| 67 |
return LIB_OK; |
|
| 68 |
} |
|
| 111 |
/*! |
|
| 112 |
* \brief Query the current speeds of the motors |
|
| 113 |
* |
|
| 114 |
* Sends a request to the query_motors service which will reply with the |
|
| 115 |
* current speed of each motor. |
|
| 116 |
* |
|
| 117 |
* \param which A bitmask that will specify which motor speed should be |
|
| 118 |
* returned |
|
| 119 |
* \return The speed of the selected motor, or LIB_ERR if no motor selected |
|
| 120 |
*/ |
|
| 69 | 121 |
|
| 70 | 122 |
int motors_query(int which){
|
| 71 | 123 |
motors::query_motors srv; |
| ... | ... | |
| 79 | 131 |
return srv.response.bl_speed; |
| 80 | 132 |
case MOTOR_BR: |
| 81 | 133 |
return srv.response.br_speed; |
| 82 |
default:{
|
|
| 134 |
default: |
|
| 83 | 135 |
ROS_WARN("Bad WHICH in motors_query.");
|
| 84 | 136 |
return LIB_ERROR; |
| 85 |
} |
|
| 86 | 137 |
} |
| 87 | 138 |
}else{
|
| 88 | 139 |
ROS_ERROR("Failed to call service query_motors");
|
Also available in: Unified diff