Skip to content

Commit

Permalink
makes more rflex parameters configurable
Browse files Browse the repository at this point in the history 
Now torque, acceleration are configurable.
Lots of changes in rflex_driver to make this happen,
changes api, so makes b21 not compatible, only works for atrvjr.
  • Loading branch information
Mikhail Medvedev committed Nov 9, 2012
1 parent ff3be64 commit 2c79372
Show file tree
Hide file tree
Showing 8 changed files with 170 additions and 100 deletions.
5 changes: 4 additions & 1 deletion atrvjr_drivers/rflex/cfg/AtrvjrParams.cfg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,9 @@ gen = ParameterGenerator()
# Name Type Level Description Default Min Max
gen.add("odo_distance_conversion", int_t, 0, "Arbitrary units per meter.", 93810, 1, 188000)
gen.add("odo_angle_conversion", int_t, 0, "Arbitrary units per radian.", 38500, 1, 78000)
gen.add("acceleration", double_t, 0, "How fast to change translational speed, m/s^2.", 0.7, 0, 2)
gen.add("trans_acceleration", double_t, 0, "How fast to change translational speed, m/s^2.", 0.7, 0, 4)
gen.add("trans_torque", double_t, 0, "Translational torque , N*m.", 0.3, 0, 2)
gen.add("rot_acceleration", double_t, 0, "How fast to change rotational speed, rad/s^2.", 2.6, 0, 4)
gen.add("rot_torque", double_t, 0, "Rotational torque , N*m.", 0.9, 0, 2)

exit(gen.generate(PACKAGE, "rflex", "AtrvjrParams"))
1 change: 0 additions & 1 deletion atrvjr_drivers/rflex/include/rflex/atrvjr_config.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,6 @@ const float SONAR_RING_HEIGHT[] = {0.4};

// Digital IO constants
// ====================
#define HOME_BEARING -32500

//#define BUMPER_COUNT 14
//#define BUMPER_ADDRESS_STYLE 0
Expand Down
20 changes: 1 addition & 19 deletions atrvjr_drivers/rflex/include/rflex/atrvjr_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,10 +35,6 @@ class ATRVJR : public RFLEX {
ATRVJR();
virtual ~ATRVJR();
void setSonarPower(bool);
float getDistance();
float getBearing();
float getTranslationalVelocity() const;
float getRotationalVelocity() const;
float getVoltage() const;
bool isPluggedIn() const;
int getNumBodySonars() const;
Expand All @@ -58,12 +54,6 @@ class ATRVJR : public RFLEX {
* \return number of active bump sensors */
// int getBaseBumps(sensor_msgs::PointCloud* cloud) const;

/** Sets the motion of the robot
* \param tvel Translational velocity (in m/s)
* \param rvel Rotational velocity (in radian/s)
* \param acceleration Translational acceleration (in m/s/s) */
void setMovement(float tvel, float rvel, float acceleration);

/** Processes the DIO packets - called from RFflex Driver
* \param address origin
* \param data values */
Expand All @@ -76,9 +66,6 @@ class ATRVJR : public RFLEX {
return odomReady==3;
}

void setOdoAngleConversion(int);
void setOdoDistanceConversion(int);

// SimpleSignal bumpsUpdateSignal;
private:
/**\param ringi BODY_INDEX or BASE_INDEX
Expand All @@ -94,13 +81,8 @@ class ATRVJR : public RFLEX {
*/
// int getBumps(const int index, sensor_msgs::PointCloud* cloud) const;

int first_distance;
bool found_distance;
int home_bearing; ///< Last home bearing (arbitrary units)
// int home_bearing; ///< Last home bearing (arbitrary units)

// Odometery Settings
int odo_distance_conversion_; ///< Arbitrary units per meter
int odo_angle_conversion_; ///< Arbitrary units per radian
// int** bumps;

// Not allowed to use these
Expand Down
119 changes: 109 additions & 10 deletions atrvjr_drivers/rflex/include/rflex/rflex_driver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,95 @@ class SimpleSignal{
boost::function<void()> callback;
};

/**
* Holds low-level configuration parameters for RFLEX.
* These used to be constant definitions in rflex_info.h
*/
class RFLEXConfig {
private:
unsigned long odo_distance_conversion;
unsigned long odo_angle_conversion;

double trans_acc;///< In m/s/s
double rot_acc;///< In m/s/s
double trans_torque; ///< In N*m
double rot_torque; ///< In N*m

public:
int home_bearing;

public:
RFLEXConfig() :
odo_distance_conversion(93810), //
odo_angle_conversion(38500), //
trans_acc(0.7), //
rot_acc(2.6), //
trans_torque(0.3), //
rot_torque(0.9), //
home_bearing(-32500)
{
}

/**
* Convert the real translational value into RFLEX units.
* @param value
*/
unsigned long realTrans2driver(double value) const {
return value * odo_distance_conversion;
}
double driverTrans2real(int value) const{
return value / (float) odo_distance_conversion;
}

/**
* Convert the real angular value into RFLEX units.
* @param value
*/
unsigned long realAngle2driver(double value) const {
return value * odo_angle_conversion;
}
double driverAngle2real(int value) const{
return value / (float) odo_angle_conversion;
}

unsigned long getAdjustedTransAcc(){
return realTrans2driver(trans_acc);
}
unsigned long getAdjustedRotAcc(){
return realAngle2driver(rot_acc);
}
unsigned long getAdjustedTransTorque(){
return realTrans2driver(trans_torque);
}
unsigned long getAdjustedRotTorque(){
return realAngle2driver(rot_torque);
}

void setOdoAngleConversion(unsigned long odoAngleConversion) {
odo_angle_conversion = odoAngleConversion;
}

void setOdoDistanceConversion(unsigned long odoDistanceConversion) {
odo_distance_conversion = odoDistanceConversion;
}

void setRotAcc(double rotAcc) {
rot_acc = rotAcc;
}

void setRotTorque(double rotTorque) {
rot_torque = rotTorque;
}

void setTransAcc(double transAcc) {
trans_acc = transAcc;
}

void setTransTorque(double transTorque) {
trans_torque = transTorque;
}
};

class RFLEX {
public:
RFLEX();
Expand Down Expand Up @@ -107,31 +196,31 @@ class RFLEX {
}

/** Sets the velocity
* \param transVelocity Translational velocity in arbitrary units
* \param rotVelocity Rotational velocity in arbitrary units
* \param acceleration Acceleration (also in arbitrary units) */
void setVelocity(const long transVelocity, const long rotVelocity,
const long acceleration);
* \param transVelocity Translational velocity in m/s
* \param rotVelocity Rotational velocity in rad/s
*/
void setVelocity(const float transVelocity, const float rotVelocity);

/** Sends a system status command to the device.
* Updates the brake and battery status. */
void sendSystemStatusCommand();

double getDistance();///< Translational odometry, m.
double getBearing();///< Rotational odometry, rad.
double getTransVelocity();///< Translational velocity, m/s.
double getRotVelocity();///< Rotational velocity, rad/s.

/// Signals
SimpleSignal systemStatusUpdateSignal;
SimpleSignal motorUpdateSignal;
SimpleSignal sonarUpdateSignal;

RFLEXConfig config;

protected:

virtual void processDioEvent(unsigned char address, unsigned short data);

int distance; ///< Raw translational odometry
int bearing; ///< Raw rotational odometry
int transVelocity; ///< Raw translational velocity
int rotVelocity; ///< Raw rotational velocity

int sonar_ranges[SONAR_MAX_COUNT]; ///< Raw Sonar readings (including unconnected ports)
long voltage; ///< Raw voltage reading
bool brake; ///< Brake Status
Expand All @@ -147,6 +236,15 @@ class RFLEX {
int odomReady;

private:

int distance; ///< Raw translational odometry
int bearing; ///< Raw rotational odometry
int transVelocity; ///< Raw translational velocity
int rotVelocity; ///< Raw rotational velocity

int first_distance; ///< Raw odometry reading at start time
bool found_distance; ///< True when the first_distance was initialized

void parsePacket(const unsigned char* buffer);
void parseMotReport(const unsigned char* buffer);
void parseDioReport(const unsigned char* buffer);
Expand All @@ -155,6 +253,7 @@ class RFLEX {
void parseSonarReport(const unsigned char* buffer);
void parseJoyReport(const unsigned char* buffer);


// IO Stuff
int fd; ///< File descriptor for serial port
pthread_t thread; ///< Thread which reads input upon arrival
Expand Down
4 changes: 0 additions & 4 deletions atrvjr_drivers/rflex/include/rflex/rflex_info.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,10 +40,6 @@ static const int PACKET_DATA_START_BYTE = 6;
#define MAX_COMMAND_LENGTH 256
#define BUFFER_SIZE 1024

#define STD_TRANS_TORQUE 30000
#define STD_ROT_ACC 100000
#define STD_ROT_TORQUE 35000

#define SYS_PORT 1
#define MOT_PORT 2
#define JSTK_PORT 3
Expand Down
51 changes: 6 additions & 45 deletions atrvjr_drivers/rflex/src/atrvjr_driver.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,11 +29,11 @@
#include <math.h>
#include <cstdio>

ATRVJR::ATRVJR():
odo_distance_conversion_(93810),
odo_angle_conversion_(38500)
ATRVJR::ATRVJR()
//:
// odo_distance_conversion_(93810),
// odo_angle_conversion_(38500)
{
found_distance = false;
// bumps = new int*[2];
//
// for (int index=0;index<2;index++) {
Expand All @@ -51,27 +51,6 @@ ATRVJR::~ATRVJR() {
// delete bumps;
}

float ATRVJR::getDistance() {
if (!found_distance && isOdomReady()) {
first_distance = distance;
found_distance = true;
}

return (distance-first_distance) / (float) odo_distance_conversion_;
}

float ATRVJR::getBearing() {
return (bearing-HOME_BEARING) / (float) odo_angle_conversion_;
}

float ATRVJR::getTranslationalVelocity() const {
return transVelocity / (float) odo_distance_conversion_;
}

float ATRVJR::getRotationalVelocity() const {
return rotVelocity / (float) odo_angle_conversion_;
}

float ATRVJR::getVoltage() const {
if (voltage==0.0)
return 0.0;
Expand Down Expand Up @@ -117,14 +96,6 @@ void ATRVJR::setSonarPower(bool on) {
}


void ATRVJR::setMovement( float tvel, float rvel,
float acceleration ) {
setVelocity(tvel * odo_distance_conversion_,
rvel * odo_angle_conversion_,
acceleration * odo_distance_conversion_);
}


void ATRVJR::getSonarReadings(const int ringi, float* adjusted_ranges) const {
int i = 0;
for (int x = SONAR_RING_BANK_BOUND[ringi];x<SONAR_RING_BANK_BOUND[ringi+1];x++) {
Expand Down Expand Up @@ -198,8 +169,8 @@ void ATRVJR::getSonarPoints(const int ringi, sensor_msgs::PointCloud* cloud) con
void ATRVJR::processDioEvent(unsigned char address, unsigned short data) {

if (address == HEADING_HOME_ADDRESS) {
home_bearing = bearing;
printf("ATRVJR Home %f \n", home_bearing / (float) odo_angle_conversion_);
// home_bearing = bearing;
// printf("ATRVJR Home %f \n", config.driverAngle2real(home_bearing));
// }// check if the dio packet came from a bumper packet
// else if ((address >= BUMPER_ADDRESS) && (address < (BUMPER_ADDRESS+BUMPER_COUNT))) {
// int index =0, rot = address - BUMPER_ADDRESS;
Expand All @@ -213,13 +184,3 @@ void ATRVJR::processDioEvent(unsigned char address, unsigned short data) {
printf("ATRVJR DIO: address 0x%02x (%d) value 0x%02x (%d)\n", address, address, data, data);
}
}


void ATRVJR::setOdoAngleConversion(int odo_angle_conversion) {
odo_angle_conversion_ = odo_angle_conversion;
}


void ATRVJR::setOdoDistanceConversion(int odo_distance_conversion) {
odo_distance_conversion_ = odo_distance_conversion;
}
33 changes: 21 additions & 12 deletions atrvjr_drivers/rflex/src/atrvjr_node.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,6 @@ class ATRVJRNode {
tf::TransformBroadcaster broadcaster; ///< Transform Broadcaster (for odom)

bool isSonarOn;
double acceleration;
float last_distance, last_bearing;
float x_odo, y_odo, a_odo;
bool initialized;
Expand Down Expand Up @@ -90,11 +89,18 @@ ATRVJRNode::ATRVJRNode() : n ("~") {

int param;
n.param("odo_distance_conversion", param, 93810);
driver.setOdoDistanceConversion(param);
driver.config.setOdoDistanceConversion(param);
n.param("odo_angle_conversion", param, 38500);
driver.setOdoAngleConversion(param);
n.param("acceleration", acceleration, 0.7);

driver.config.setOdoAngleConversion(param);
double d_param;
n.param("trans_acceleration", d_param, 0.7);
driver.config.setTransAcc(d_param);
n.param("trans_torque", d_param, 0.3);
driver.config.setTransTorque(d_param);
n.param("rot_acceleration", d_param, 2.6);
driver.config.setRotAcc(d_param);
n.param("rot_torque", d_param, 0.9);
driver.config.setRotTorque(d_param);

// Setup dynamic reconfigure
reconfigure_srv_.setCallback(boost::bind(&ATRVJRNode::reconfigureCb, this, _1, _2));
Expand Down Expand Up @@ -125,9 +131,12 @@ ATRVJRNode::ATRVJRNode() : n ("~") {

void ATRVJRNode::reconfigureCb(rflex::AtrvjrParamsConfig& config,
uint32_t level) {
driver.setOdoDistanceConversion(config.odo_distance_conversion);
driver.setOdoAngleConversion(config.odo_angle_conversion);
acceleration = config.acceleration;
driver.config.setOdoDistanceConversion(config.odo_distance_conversion);
driver.config.setOdoAngleConversion(config.odo_angle_conversion);
driver.config.setTransAcc(config.trans_acceleration);
driver.config.setTransTorque(config.trans_torque);
driver.config.setRotAcc(config.rot_acceleration);
driver.config.setRotTorque(config.rot_torque);
}

void ATRVJRNode::RFLEXSystemStatusUpdateCb(){
Expand Down Expand Up @@ -178,12 +187,12 @@ ATRVJRNode::~ATRVJRNode() {

/// cmd_vel callback
void ATRVJRNode::NewCommand(const geometry_msgs::Twist::ConstPtr& msg) {
driver.setMovement(msg->linear.x, msg->angular.z, acceleration);
driver.setVelocity(msg->linear.x, msg->angular.z);
}

/// cmd_acceleration callback
void ATRVJRNode::SetAcceleration (const std_msgs::Float32::ConstPtr& msg) {
acceleration = msg->data;
driver.config.setTransAcc(msg->data);
}

/// cmd_sonar_power callback
Expand Down Expand Up @@ -264,10 +273,10 @@ void ATRVJRNode::publishOdometry() {

//set the velocity
odom.child_frame_id = "base_link";
float tvel = driver.getTranslationalVelocity();
float tvel = driver.getTransVelocity();
odom.twist.twist.linear.x = tvel*cos(a_odo);
odom.twist.twist.linear.y = tvel*sin(a_odo);
odom.twist.twist.angular.z = driver.getRotationalVelocity();
odom.twist.twist.angular.z = driver.getRotVelocity();

//publish the message
odom_pub.publish(odom);
Expand Down
Loading

0 comments on commit 2c79372

Please sign in to comment.