L1.5/rosaria
6
0
Fork 0
Code Issues 1 Pull Requests Projects Releases Wiki Activity

Inicjalizacja

Browse Source
This commit is contained in:
Damian Barański 2015-02-18 11:55:51 +01:00
commit ec1ab02895
8 changed files with 1734 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
CMakeLists.txt Normal file
View File

@ -0,0 +1,80 @@
# http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html
cmake_minimum_required(VERSION 2.8.3)
project(rosaria)
# Load catkin and all dependencies required for this package
# TODO: remove all from COMPONENTS that are not catkin packages.
find_package(catkin REQUIRED COMPONENTS message_generation roscpp nav_msgs geometry_msgs sensor_msgs tf
dynamic_reconfigure)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
#set the default path for built executables to the "bin" directory
#set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
#set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
add_message_files(
FILES
BumperState.msg
)
#uncomment if you have defined services
#add_service_files(
# FILES
# # TODO: List your msg files here
#)
generate_dynamic_reconfigure_options(cfg/RosAria.cfg)
#common commands for building c++ executables and libraries
#add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
find_package(Boost REQUIRED COMPONENTS thread)
include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} )
# The installation package provided by Adept doesn't follow Debian policies
if(EXISTS "/usr/local/Aria/include/Aria.h")
add_definitions(-DADEPT_PKG)
include_directories( /usr/local/Aria/include)
link_directories(/usr/local/Aria/lib)
endif()
add_executable(RosAria RosAria.cpp)
add_dependencies(RosAria rosaria_gencfg)
add_dependencies(RosAria rosaria_gencpp)
target_link_libraries(RosAria ${catkin_LIBRARIES} ${Boost_LIBRARIES} Aria pthread dl rt)
set_target_properties(RosAria PROPERTIES COMPILE_FLAGS "-fPIC")
#set_target_properties(RosAria PROPERTIES LINK_FLAGS "-Wl")
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES geometry_msgs std_msgs
)
# TODO: fill in what other packages will need to use this package
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
DEPENDS roscpp nav_msgs geometry_msgs sensor_msgs tf
)
#############
## Install ##
#############
## Mark executables and/or libraries for installation
install(
TARGETS
RosAria
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

766
RosAria.cpp Normal file
View File

@ -0,0 +1,766 @@
#include <stdio.h>
#include <math.h>
#ifdef ADEPT_PKG
#include <Aria.h>
#else
#include </usr/local/Aria/include/Aria.h>
//Aria/Aria.h>
#endif
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <sensor_msgs/PointCloud.h> //for sonar data
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
#include "nav_msgs/Odometry.h"
#include "rosaria/BumperState.h"
#include "tf/tf.h"
#include "tf/transform_listener.h" //for tf::getPrefixParam
#include <tf/transform_broadcaster.h>
#include "tf/transform_datatypes.h"
#include <dynamic_reconfigure/server.h>
#include <rosaria/RosAriaConfig.h>
#include "std_msgs/Float64.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int8.h"
#include "std_msgs/Bool.h"
#include "std_srvs/Empty.h"
#include <sstream>
// Node that interfaces between ROS and mobile robot base features via ARIA library.
//
// RosAria uses the roscpp client library, see http://www.ros.org/wiki/roscpp for
// information, tutorials and documentation.
class RosAriaNode
{
public:
RosAriaNode(ros::NodeHandle n);
virtual ~RosAriaNode();
public:
int Setup();
void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
//void cmd_enable_motors_cb();
//void cmd_disable_motors_cb();
void spin();
void publish();
void sonarConnectCb();
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
void readParameters();
protected:
ros::NodeHandle n;
ros::Publisher pose_pub;
ros::Publisher bumpers_pub;
ros::Publisher sonar_pub;
ros::Publisher sonar_pointcloud2_pub;
ros::Publisher voltage_pub;
ros::Publisher recharge_state_pub;
std_msgs::Int8 recharge_state;
ros::Publisher state_of_charge_pub;
ros::Publisher motors_state_pub;
std_msgs::Bool motors_state;
bool published_motors_state;
ros::Subscriber cmdvel_sub;
ros::ServiceServer enable_srv;
ros::ServiceServer disable_srv;
bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::Publisher stop_motors_pub;
ros::ServiceServer stop_motors_srv;
ros::ServiceServer start_motors_srv;
bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::Time veltime;
std::string serial_port;
int serial_baud;
ArRobotConnector *conn;
ArRobot *robot;
nav_msgs::Odometry position;
rosaria::BumperState bumpers;
ArPose pos;
ArFunctorC<RosAriaNode> myPublishCB;
//ArRobot::ChargeState batteryCharge;
//for odom->base_link transform
tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_trans;
//for resolving tf names.
std::string tf_prefix;
std::string frame_id_odom;
std::string frame_id_base_link;
std::string frame_id_bumper;
std::string frame_id_sonar;
// flag indicating whether sonar was enabled or disabled on the robot
bool sonar_enabled;
// enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects.
bool publish_sonar;
bool publish_sonar_pointcloud2;
// Debug Aria
bool debug_aria;
std::string aria_log_filename;
// Robot Parameters
int TicksMM, DriftFactor, RevCount; // Odometry Calibration Settings
// dynamic_reconfigure
dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
//Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
bool stop_robot;
std_msgs::Bool stop_motors_msg;
};
void RosAriaNode::readParameters()
{
// Robot Parameters
robot->lock();
ros::NodeHandle n_("~");
if (n_.hasParam("TicksMM"))
{
n_.getParam( "TicksMM", TicksMM);
ROS_INFO("Setting TicksMM from ROS Parameter: %d", TicksMM);
robot->comInt(93, TicksMM);
}
else
{
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
n_.setParam( "TicksMM", TicksMM);
ROS_INFO("Setting TicksMM from robot controller stored configuration: %d", TicksMM);
}
if (n_.hasParam("DriftFactor"))
{
n_.getParam( "DriftFactor", DriftFactor);
ROS_INFO("Setting DriftFactor from ROS Parameter: %d", DriftFactor);
robot->comInt(89, DriftFactor);
}
else
{
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
n_.setParam( "DriftFactor", DriftFactor);
ROS_INFO("Setting DriftFactor from robot controller stored configuration: %d", DriftFactor);
}
if (n_.hasParam("RevCount"))
{
n_.getParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from ROS Parameter: %d", RevCount);
robot->comInt(88, RevCount);
}
else
{
RevCount = robot->getOrigRobotConfig()->getRevCount();
n_.setParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount);
}
robot->unlock();
}
void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
{
//
// Odometry Settings
//
robot->lock();
if(TicksMM != config.TicksMM and config.TicksMM > 0)
{
ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
TicksMM = config.TicksMM;
robot->comInt(93, TicksMM);
}
if(DriftFactor != config.DriftFactor)
{
ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
DriftFactor = config.DriftFactor;
robot->comInt(89, DriftFactor);
}
if(RevCount != config.RevCount and config.RevCount > 0)
{
ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
RevCount = config.RevCount;
robot->comInt(88, RevCount);
}
//
// Acceleration Parameters
//
int value;
value = config.trans_accel * 1000;
if(value != robot->getTransAccel() and value > 0)
{
ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
robot->setTransAccel(value);
}
value = config.trans_decel * 1000;
if(value != robot->getTransDecel() and value > 0)
{
ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
robot->setTransDecel(value);
}
value = config.lat_accel * 1000;
if(value != robot->getLatAccel() and value > 0)
{
ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatAccel() > 0 )
robot->setLatAccel(value);
}
value = config.lat_decel * 1000;
if(value != robot->getLatDecel() and value > 0)
{
ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatDecel() > 0 )
robot->setLatDecel(value);
}
value = config.rot_accel * 180/M_PI;
if(value != robot->getRotAccel() and value > 0)
{
ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
robot->setRotAccel(value);
}
value = config.rot_decel * 180/M_PI;
if(value != robot->getRotDecel() and value > 0)
{
ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
robot->setRotDecel(value);
}
robot->unlock();
}
void RosAriaNode::sonarConnectCb()
{
publish_sonar = (sonar_pub.getNumSubscribers() > 0);
publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
robot->lock();
if (publish_sonar || publish_sonar_pointcloud2)
{
robot->enableSonar();
sonar_enabled = false;
}
else if(!publish_sonar && !publish_sonar_pointcloud2)
{
robot->disableSonar();
sonar_enabled = true;
}
robot->unlock();
}
RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
myPublishCB(this, &RosAriaNode::publish), serial_port(""), serial_baud(0),
sonar_enabled(false), publish_sonar(false), publish_sonar_pointcloud2(false)
{
// read in runtime parameters
n = nh;
// port and baud
n.param( "port", serial_port, std::string("/dev/ttyUSB0") );
ROS_INFO( "RosAria: using port: [%s]", serial_port.c_str() );
n.param("baud", serial_baud, 0);
if(serial_baud != 0)
ROS_INFO("RosAria: using serial port baud rate %d", serial_baud);
// handle debugging more elegantly
n.param( "debug_aria", debug_aria, false ); // default not to debug
n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
// Figure out what frame_id's to use. if a tf_prefix param is specified,
// it will be added to the beginning of the frame_ids.
//
// e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
// roslaunch files)
// will result in the frame_ids being set to /MyRobot/odom etc,
// rather than /odom. This is useful for Multi Robot Systems.
// See ROS Wiki for further details.
tf_prefix = tf::getPrefixParam(n);
frame_id_odom = tf::resolve(tf_prefix, "odom");
frame_id_base_link = tf::resolve(tf_prefix, "base_link");
frame_id_bumper = tf::resolve(tf_prefix, "bumpers_frame");
frame_id_sonar = tf::resolve(tf_prefix, "sonar_frame");
// advertise services for data topics
// second argument to advertise() is queue size.
// other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new
// subscribers when they subscribe).
// See ros::NodeHandle API docs.
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50,
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
stop_motors_pub = n.advertise<std_msgs::Bool>("stop_motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
//Damian usługi stopu awaryjnego
stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
stop_robot=false;
stop_motors_msg.data=false;
veltime = ros::Time::now();
}
RosAriaNode::~RosAriaNode()
{
// disable motors and sonar.
robot->disableMotors();
robot->disableSonar();
robot->stopRunning();
robot->waitForRunExit();
Aria::shutdown();
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
readParameters();
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
rosaria::RosAriaConfig dynConf_default;
dynConf_default.trans_accel = robot->getTransAccel() / 1000;
dynConf_default.trans_decel = robot->getTransDecel() / 1000;
dynConf_default.lat_accel = robot->getLatAccel() / 1000;
dynConf_default.lat_decel = robot->getLatDecel() / 1000;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180;
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_max);
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// Enable the motors
robot->enableMotors();
// disable sonars on startup
robot->disableSonar();
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
void RosAriaNode::spin()
{
ros::spin();
}
void RosAriaNode::publish()
{
// Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
pos = robot->getPose();
tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s.
position.twist.twist.linear.y = robot->getLatVel()/1000.0;
position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
position.header.frame_id = frame_id_odom;
position.child_frame_id = frame_id_base_link;
position.header.stamp = ros::Time::now();
pose_pub.publish(position);
ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f",
position.header.stamp.toSec(),
(double)position.pose.pose.position.x,
(double)position.pose.pose.position.y,
(double)position.pose.pose.orientation.w,
(double) position.twist.twist.linear.x,
(double) position.twist.twist.linear.y,
(double) position.twist.twist.angular.z
);
// publishing transform odom->base_link
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = frame_id_odom;
odom_trans.child_frame_id = frame_id_base_link;
odom_trans.transform.translation.x = pos.getX()/1000;
odom_trans.transform.translation.y = pos.getY()/1000;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
odom_broadcaster.sendTransform(odom_trans);
// getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
int stall = robot->getStallValue();
unsigned char front_bumpers = (unsigned char)(stall >> 8);
unsigned char rear_bumpers = (unsigned char)(stall);
bumpers.header.frame_id = frame_id_bumper;
bumpers.header.stamp = ros::Time::now();
std::stringstream bumper_info(std::stringstream::out);
// Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
{
bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
bumper_info << " " << (front_bumpers & (1 << (i+1)));
}
ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
bumper_info.str("");
// Rear bumpers have reverse order (rightmost is LSB)
unsigned int numRearBumpers = robot->getNumRearBumpers();
for (unsigned int i=0; i<numRearBumpers; i++)
{
bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
}
ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
bumpers_pub.publish(bumpers);
//Publish battery information
// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
std_msgs::Float64 batteryVoltage;
batteryVoltage.data = robot->getRealBatteryVoltageNow();
voltage_pub.publish(batteryVoltage);
if(robot->haveStateOfCharge())
{
std_msgs::Float32 soc;
soc.data = robot->getStateOfCharge()/100.0;
state_of_charge_pub.publish(soc);
}
// publish recharge state if changed
char s = robot->getChargeState();
if(s != recharge_state.data)
{
ROS_INFO("RosAria: publishing new recharge state %d.", s);
recharge_state.data = s;
recharge_state_pub.publish(recharge_state);
}
// publish motors state if changed
bool e = robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO("RosAria: publishing stop motors state %d.", e);
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
//Publikuje czy zatrzymane silniki
if(stop_robot)
{
ROS_INFO("RosAria: publishing new motors state %d.", stop_motors_msg.data);
stop_motors_pub.publish(stop_motors_msg);
stop_robot = false;
}
// Publish sonar information, if enabled.
if (publish_sonar || publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
std::stringstream sonar_debug_info; // Log debugging info
sonar_debug_info << "Sonar readings: ";
for (int i = 0; i < robot->getNumSonar(); i++) {
ArSensorReading* reading = NULL;
reading = robot->getSonarReading(i);
if(!reading) {
ROS_WARN("RosAria: Did not receive a sonar reading.");
continue;
}
// getRange() will return an integer between 0 and 5000 (5m)
sonar_debug_info << reading->getRange() << " ";
// local (x,y). Appears to be from the centre of the robot, since values may
// exceed 5000. This is good, since it means we only need 1 transform.
// x & y seem to be swapped though, i.e. if the robot is driving north
// x is north/south and y is east/west.
//
//ArPose sensor = reading->getSensorPosition(); //position of sensor.
// sonar_debug_info << "(" << reading->getLocalX()
// << ", " << reading->getLocalY()
// << ") from (" << sensor.getX() << ", "
// << sensor.getY() << ") ;; " ;
//add sonar readings (robot-local coordinate frame) to cloud
geometry_msgs::Point32 p;
p.x = reading->getLocalX() / 1000.0;
p.y = reading->getLocalY() / 1000.0;
p.z = 0.0;
cloud.points.push_back(p);
}
ROS_DEBUG_STREAM(sonar_debug_info.str());
// publish topic(s)
if(publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud2 cloud2;
if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
{
ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
}
else
{
sonar_pointcloud2_pub.publish(cloud2);
}
}
if(publish_sonar)
{
sonar_pub.publish(cloud);
}
} // end if sonar_enabled
}
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Enable motors request.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
robot->enableMotors();
robot->unlock();
// todo could wait and see if motors do become enabled, and send a response with an error flag if not
return true;
}
bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Disable motors request.");
robot->lock();
robot->disableMotors();
robot->unlock();
// todo could wait and see if motors do become disabled, and send a response with an error flag if not
return true;
}
//Damian Metody odpowiedzialne za awaryjne ztrzymanie silników
bool RosAriaNode::start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
stop_robot=true;
stop_motors_msg.data=false;
ROS_INFO("RosAria: Disable motors brakes.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
robot->unlock();
return true;
}
bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
stop_robot=true;
stop_motors_msg.data=true;
//Zatrzymuje robota
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
robot->comInt(ArCommands::ESTOP,0);
ROS_INFO("RosAria: Enable motors brakes.");
robot->unlock();
return true;
}
void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
veltime = ros::Time::now();
if(stop_motors_msg.data)
{
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
return;
}
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
robot->lock();
robot->setVel(msg->linear.x*1e3);
if(robot->hasLatVel())
robot->setLatVel(msg->linear.y*1e3);
robot->setRotVel(msg->angular.z*180/M_PI);
robot->unlock();
ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
(double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
int main( int argc, char** argv )
{
ros::init(argc,argv, "RosAria");
ros::NodeHandle n(std::string("~"));
Aria::init();
RosAriaNode *node = new RosAriaNode(n);
if( node->Setup() != 0 )
{
ROS_FATAL( "RosAria: ROS node setup failed... \n" );
return -1;
}
node->spin();
delete node;
ROS_INFO( "RosAria: Quitting... \n" );
return 0;
}

26
cfg/RosAria.cfg Executable file
View File

@ -0,0 +1,26 @@
#!/usr/bin/env python
PACKAGE = "rosaria"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
gen.add("trans_accel", double_t, 0, "Translational acceleration (m/s^2)")
gen.add("trans_decel", double_t, 0, "Translational deceleration (m/s^2)" )
gen.add("lat_accel" , double_t, 0, "Lateral acceleration (m/s^2)" )
gen.add("lat_decel" , double_t, 0, "Lateral deceleration (m/s^2)" )
gen.add("rot_accel" , double_t, 0, "Rotational acceleration (rad/s^2)" )
gen.add("rot_decel" , double_t, 0, "Rotational deceleration (rad/s^2)" )
# Default values are from the Pioneer Manual p.53
gen.add("TicksMM" , int_t, 0, "Encoder ticks/mm" )
gen.add("DriftFactor", int_t, 0, "Value in 1/8192 increments to be added or " +
"subtracted from the left encoder ticks in " +
"order to compensate for tire differences." )
gen.add("RevCount" , int_t, 0, "The number of differential encoder ticks " +
"for a 180-degree revolution of the robot." )
exit(gen.generate(PACKAGE, "RosAria", "RosAria"))

752
lab1_5@10.104.16.131 Normal file
View File

@ -0,0 +1,752 @@
#include <stdio.h>
#include <math.h>
#ifdef ADEPT_PKG
#include <Aria.h>
#else
#include </usr/local/Aria/include/Aria.h>
//Aria/Aria.h>
#endif
#include "ros/ros.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include <sensor_msgs/PointCloud.h> //for sonar data
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud_conversion.h> // can optionally publish sonar as new type pointcloud2
#include "nav_msgs/Odometry.h"
#include "rosaria/BumperState.h"
#include "tf/tf.h"
#include "tf/transform_listener.h" //for tf::getPrefixParam
#include <tf/transform_broadcaster.h>
#include "tf/transform_datatypes.h"
#include <dynamic_reconfigure/server.h>
#include <rosaria/RosAriaConfig.h>
#include "std_msgs/Float64.h"
#include "std_msgs/Float32.h"
#include "std_msgs/Int8.h"
#include "std_msgs/Bool.h"
#include "std_srvs/Empty.h"
#include <sstream>
// Node that interfaces between ROS and mobile robot base features via ARIA library.
//
// RosAria uses the roscpp client library, see http://www.ros.org/wiki/roscpp for
// information, tutorials and documentation.
class RosAriaNode
{
public:
RosAriaNode(ros::NodeHandle n);
virtual ~RosAriaNode();
public:
int Setup();
void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
//void cmd_enable_motors_cb();
//void cmd_disable_motors_cb();
void spin();
void publish();
void sonarConnectCb();
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
void readParameters();
protected:
ros::NodeHandle n;
ros::Publisher pose_pub;
ros::Publisher bumpers_pub;
ros::Publisher sonar_pub;
ros::Publisher sonar_pointcloud2_pub;
ros::Publisher voltage_pub;
ros::Publisher recharge_state_pub;
std_msgs::Int8 recharge_state;
ros::Publisher state_of_charge_pub;
ros::Publisher motors_state_pub;
std_msgs::Bool motors_state;
bool published_motors_state;
ros::Subscriber cmdvel_sub;
ros::ServiceServer enable_srv;
ros::ServiceServer disable_srv;
bool enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::ServiceServer stop_motors_srv;
ros::ServiceServer start_motors_srv;
bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::Time veltime;
std::string serial_port;
int serial_baud;
ArRobotConnector *conn;
ArRobot *robot;
nav_msgs::Odometry position;
rosaria::BumperState bumpers;
ArPose pos;
ArFunctorC<RosAriaNode> myPublishCB;
//ArRobot::ChargeState batteryCharge;
//for odom->base_link transform
tf::TransformBroadcaster odom_broadcaster;
geometry_msgs::TransformStamped odom_trans;
//for resolving tf names.
std::string tf_prefix;
std::string frame_id_odom;
std::string frame_id_base_link;
std::string frame_id_bumper;
std::string frame_id_sonar;
// flag indicating whether sonar was enabled or disabled on the robot
bool sonar_enabled;
// enable and publish sonar topics. set to true when first subscriber connects, set to false when last subscriber disconnects.
bool publish_sonar;
bool publish_sonar_pointcloud2;
// Debug Aria
bool debug_aria;
std::string aria_log_filename;
// Robot Parameters
int TicksMM, DriftFactor, RevCount; // Odometry Calibration Settings
// dynamic_reconfigure
dynamic_reconfigure::Server<rosaria::RosAriaConfig> *dynamic_reconfigure_server;
//Damian zmienna mówiąca czy robot jest zatrzymany awaryjnie
bool stop_robot;
};
void RosAriaNode::readParameters()
{
// Robot Parameters
robot->lock();
ros::NodeHandle n_("~");
if (n_.hasParam("TicksMM"))
{
n_.getParam( "TicksMM", TicksMM);
ROS_INFO("Setting TicksMM from ROS Parameter: %d", TicksMM);
robot->comInt(93, TicksMM);
}
else
{
TicksMM = robot->getOrigRobotConfig()->getTicksMM();
n_.setParam( "TicksMM", TicksMM);
ROS_INFO("Setting TicksMM from robot controller stored configuration: %d", TicksMM);
}
if (n_.hasParam("DriftFactor"))
{
n_.getParam( "DriftFactor", DriftFactor);
ROS_INFO("Setting DriftFactor from ROS Parameter: %d", DriftFactor);
robot->comInt(89, DriftFactor);
}
else
{
DriftFactor = robot->getOrigRobotConfig()->getDriftFactor();
n_.setParam( "DriftFactor", DriftFactor);
ROS_INFO("Setting DriftFactor from robot controller stored configuration: %d", DriftFactor);
}
if (n_.hasParam("RevCount"))
{
n_.getParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from ROS Parameter: %d", RevCount);
robot->comInt(88, RevCount);
}
else
{
RevCount = robot->getOrigRobotConfig()->getRevCount();
n_.setParam( "RevCount", RevCount);
ROS_INFO("Setting RevCount from robot controller stored configuration: %d", RevCount);
}
robot->unlock();
}
void RosAriaNode::dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level)
{
//
// Odometry Settings
//
robot->lock();
if(TicksMM != config.TicksMM and config.TicksMM > 0)
{
ROS_INFO("Setting TicksMM from Dynamic Reconfigure: %d -> %d ", TicksMM, config.TicksMM);
TicksMM = config.TicksMM;
robot->comInt(93, TicksMM);
}
if(DriftFactor != config.DriftFactor)
{
ROS_INFO("Setting DriftFactor from Dynamic Reconfigure: %d -> %d ", DriftFactor, config.DriftFactor);
DriftFactor = config.DriftFactor;
robot->comInt(89, DriftFactor);
}
if(RevCount != config.RevCount and config.RevCount > 0)
{
ROS_INFO("Setting RevCount from Dynamic Reconfigure: %d -> %d ", RevCount, config.RevCount);
RevCount = config.RevCount;
robot->comInt(88, RevCount);
}
//
// Acceleration Parameters
//
int value;
value = config.trans_accel * 1000;
if(value != robot->getTransAccel() and value > 0)
{
ROS_INFO("Setting TransAccel from Dynamic Reconfigure: %d", value);
robot->setTransAccel(value);
}
value = config.trans_decel * 1000;
if(value != robot->getTransDecel() and value > 0)
{
ROS_INFO("Setting TransDecel from Dynamic Reconfigure: %d", value);
robot->setTransDecel(value);
}
value = config.lat_accel * 1000;
if(value != robot->getLatAccel() and value > 0)
{
ROS_INFO("Setting LatAccel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatAccel() > 0 )
robot->setLatAccel(value);
}
value = config.lat_decel * 1000;
if(value != robot->getLatDecel() and value > 0)
{
ROS_INFO("Setting LatDecel from Dynamic Reconfigure: %d", value);
if (robot->getAbsoluteMaxLatDecel() > 0 )
robot->setLatDecel(value);
}
value = config.rot_accel * 180/M_PI;
if(value != robot->getRotAccel() and value > 0)
{
ROS_INFO("Setting RotAccel from Dynamic Reconfigure: %d", value);
robot->setRotAccel(value);
}
value = config.rot_decel * 180/M_PI;
if(value != robot->getRotDecel() and value > 0)
{
ROS_INFO("Setting RotDecel from Dynamic Reconfigure: %d", value);
robot->setRotDecel(value);
}
robot->unlock();
}
void RosAriaNode::sonarConnectCb()
{
publish_sonar = (sonar_pub.getNumSubscribers() > 0);
publish_sonar_pointcloud2 = (sonar_pointcloud2_pub.getNumSubscribers() > 0);
robot->lock();
if (publish_sonar || publish_sonar_pointcloud2)
{
robot->enableSonar();
sonar_enabled = false;
}
else if(!publish_sonar && !publish_sonar_pointcloud2)
{
robot->disableSonar();
sonar_enabled = true;
}
robot->unlock();
}
RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
myPublishCB(this, &RosAriaNode::publish), serial_port(""), serial_baud(0),
sonar_enabled(false), publish_sonar(false), publish_sonar_pointcloud2(false)
{
// read in runtime parameters
n = nh;
// port and baud
n.param( "port", serial_port, std::string("/dev/ttyUSB0") );
ROS_INFO( "RosAria: using port: [%s]", serial_port.c_str() );
n.param("baud", serial_baud, 0);
if(serial_baud != 0)
ROS_INFO("RosAria: using serial port baud rate %d", serial_baud);
// handle debugging more elegantly
n.param( "debug_aria", debug_aria, false ); // default not to debug
n.param( "aria_log_filename", aria_log_filename, std::string("Aria.log") );
// Figure out what frame_id's to use. if a tf_prefix param is specified,
// it will be added to the beginning of the frame_ids.
//
// e.g. rosrun ... _tf_prefix:=MyRobot (or equivalently using <param>s in
// roslaunch files)
// will result in the frame_ids being set to /MyRobot/odom etc,
// rather than /odom. This is useful for Multi Robot Systems.
// See ROS Wiki for further details.
tf_prefix = tf::getPrefixParam(n);
frame_id_odom = tf::resolve(tf_prefix, "odom");
frame_id_base_link = tf::resolve(tf_prefix, "base_link");
frame_id_bumper = tf::resolve(tf_prefix, "bumpers_frame");
frame_id_sonar = tf::resolve(tf_prefix, "sonar_frame");
// advertise services for data topics
// second argument to advertise() is queue size.
// other argmuments (optional) are callbacks, or a boolean "latch" flag (whether to send current data to new
// subscribers when they subscribe).
// See ros::NodeHandle API docs.
pose_pub = n.advertise<nav_msgs::Odometry>("pose",1000);
bumpers_pub = n.advertise<rosaria::BumperState>("bumper_state",1000);
sonar_pub = n.advertise<sensor_msgs::PointCloud>("sonar", 50,
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
sonar_pointcloud2_pub = n.advertise<sensor_msgs::PointCloud2>("sonar_pointcloud2", 50,
boost::bind(&RosAriaNode::sonarConnectCb, this),
boost::bind(&RosAriaNode::sonarConnectCb, this));
voltage_pub = n.advertise<std_msgs::Float64>("battery_voltage", 1000);
recharge_state_pub = n.advertise<std_msgs::Int8>("battery_recharge_state", 5, true /*latch*/ );
recharge_state.data = -2;
state_of_charge_pub = n.advertise<std_msgs::Float32>("battery_state_of_charge", 100);
motors_state_pub = n.advertise<std_msgs::Bool>("motors_state", 5, true /*latch*/ );
motors_state.data = false;
published_motors_state = false;
// subscribe to services
cmdvel_sub = n.subscribe( "cmd_vel", 1, (boost::function <void(const geometry_msgs::TwistConstPtr&)>)
boost::bind(&RosAriaNode::cmdvel_cb, this, _1 ));
// advertise enable/disable services
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
//Damian usługi stopu awaryjnego
stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
stop_robot=false;
veltime = ros::Time::now();
}
RosAriaNode::~RosAriaNode()
{
// disable motors and sonar.
robot->disableMotors();
robot->disableSonar();
robot->stopRunning();
robot->waitForRunExit();
Aria::shutdown();
}
int RosAriaNode::Setup()
{
// Note, various objects are allocated here which are never deleted (freed), since Setup() is only supposed to be
// called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot();
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning never freed
argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
// Now add any parameters given via ros params (see RosAriaNode constructor):
// if serial port parameter contains a ':' character, then interpret it as hostname:tcpport
// for wireless serial connection. Otherwise, interpret it as a serial port name.
size_t colon_pos = serial_port.find(":");
if (colon_pos != std::string::npos)
{
args->add("-remoteHost"); // pass robot's hostname/IP address to Aria
args->add(serial_port.substr(0, colon_pos).c_str());
args->add("-remoteRobotTcpPort"); // pass robot's TCP port to Aria
args->add(serial_port.substr(colon_pos+1).c_str());
}
else
{
args->add("-robotPort"); // pass robot's serial port to Aria
args->add(serial_port.c_str());
}
// if a baud rate was specified in baud parameter
if(serial_baud != 0)
{
args->add("-robotBaud");
char tmp[100];
snprintf(tmp, 100, "%d", serial_baud);
args->add(tmp);
}
if( debug_aria )
{
// turn on all ARIA debugging
args->add("-robotLogPacketsReceived"); // log received packets
args->add("-robotLogPacketsSent"); // log sent packets
args->add("-robotLogVelocitiesReceived"); // log received velocities
args->add("-robotLogMovementSent");
args->add("-robotLogMovementReceived");
ArLog::init(ArLog::File, ArLog::Verbose, aria_log_filename.c_str(), true);
}
// Connect to the robot
conn = new ArRobotConnector(argparser, robot); // warning never freed
if (!conn->connectRobot()) {
ROS_ERROR("RosAria: ARIA could not connect to robot! (Check ~port parameter is correct, and permissions on port device.)");
return 1;
}
// causes ARIA to load various robot-specific hardware parameters from the robot parameter file in /usr/local/Aria/params
if(!Aria::parseArgs())
{
ROS_ERROR("RosAria: ARIA error parsing ARIA startup parameters!");
return 1;
}
readParameters();
// Start dynamic_reconfigure server
dynamic_reconfigure_server = new dynamic_reconfigure::Server<rosaria::RosAriaConfig>;
// Setup Parameter Minimums
rosaria::RosAriaConfig dynConf_min;
dynConf_min.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_min.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_min.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_min.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_min.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_min.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_min.TicksMM = 10;
dynConf_min.DriftFactor = -200;
dynConf_min.RevCount = -32760;
dynamic_reconfigure_server->setConfigMin(dynConf_min);
rosaria::RosAriaConfig dynConf_max;
dynConf_max.trans_accel = robot->getAbsoluteMaxTransAccel() / 1000;
dynConf_max.trans_decel = robot->getAbsoluteMaxTransDecel() / 1000;
// TODO: Fix rqt dynamic_reconfigure gui to handle empty intervals
// Until then, set unit length interval.
dynConf_max.lat_accel = ((robot->getAbsoluteMaxLatAccel() > 0.0) ? robot->getAbsoluteMaxLatAccel() : 0.1) / 1000;
dynConf_max.lat_decel = ((robot->getAbsoluteMaxLatDecel() > 0.0) ? robot->getAbsoluteMaxLatDecel() : 0.1) / 1000;
dynConf_max.rot_accel = robot->getAbsoluteMaxRotAccel() * M_PI/180;
dynConf_max.rot_decel = robot->getAbsoluteMaxRotDecel() * M_PI/180;
// I'm setting these upper bounds relitivly arbitrarily, feel free to increase them.
dynConf_max.TicksMM = 200;
dynConf_max.DriftFactor = 200;
dynConf_max.RevCount = 32760;
dynamic_reconfigure_server->setConfigMax(dynConf_max);
rosaria::RosAriaConfig dynConf_default;
dynConf_default.trans_accel = robot->getTransAccel() / 1000;
dynConf_default.trans_decel = robot->getTransDecel() / 1000;
dynConf_default.lat_accel = robot->getLatAccel() / 1000;
dynConf_default.lat_decel = robot->getLatDecel() / 1000;
dynConf_default.rot_accel = robot->getRotAccel() * M_PI/180;
dynConf_default.rot_decel = robot->getRotDecel() * M_PI/180;
dynConf_default.TicksMM = TicksMM;
dynConf_default.DriftFactor = DriftFactor;
dynConf_default.RevCount = RevCount;
dynamic_reconfigure_server->setConfigDefault(dynConf_max);
dynamic_reconfigure_server->setCallback(boost::bind(&RosAriaNode::dynamic_reconfigureCB, this, _1, _2));
// Enable the motors
robot->enableMotors();
// disable sonars on startup
robot->disableSonar();
// callback will be called by ArRobot background processing thread for every SIP data packet received from robot
robot->addSensorInterpTask("ROSPublishingTask", 100, &myPublishCB);
// Initialize bumpers with robot number of bumpers
bumpers.front_bumpers.resize(robot->getNumFrontBumpers());
bumpers.rear_bumpers.resize(robot->getNumRearBumpers());
// Run ArRobot background processing thread
robot->runAsync(true);
return 0;
}
void RosAriaNode::spin()
{
ros::spin();
}
void RosAriaNode::publish()
{
// Note, this is called via SensorInterpTask callback (myPublishCB, named "ROSPublishingTask"). ArRobot object 'robot' sholud not be locked or unlocked.
pos = robot->getPose();
tf::poseTFToMsg(tf::Transform(tf::createQuaternionFromYaw(pos.getTh()*M_PI/180), tf::Vector3(pos.getX()/1000,
pos.getY()/1000, 0)), position.pose.pose); //Aria returns pose in mm.
position.twist.twist.linear.x = robot->getVel()/1000; //Aria returns velocity in mm/s.
position.twist.twist.linear.y = robot->getLatVel()/1000.0;
position.twist.twist.angular.z = robot->getRotVel()*M_PI/180;
position.header.frame_id = frame_id_odom;
position.child_frame_id = frame_id_base_link;
position.header.stamp = ros::Time::now();
pose_pub.publish(position);
ROS_DEBUG("RosAria: publish: (time %f) pose x: %f, y: %f, angle: %f; linear vel x: %f, y: %f; angular vel z: %f",
position.header.stamp.toSec(),
(double)position.pose.pose.position.x,
(double)position.pose.pose.position.y,
(double)position.pose.pose.orientation.w,
(double) position.twist.twist.linear.x,
(double) position.twist.twist.linear.y,
(double) position.twist.twist.angular.z
);
// publishing transform odom->base_link
odom_trans.header.stamp = ros::Time::now();
odom_trans.header.frame_id = frame_id_odom;
odom_trans.child_frame_id = frame_id_base_link;
odom_trans.transform.translation.x = pos.getX()/1000;
odom_trans.transform.translation.y = pos.getY()/1000;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(pos.getTh()*M_PI/180);
odom_broadcaster.sendTransform(odom_trans);
// getStallValue returns 2 bytes with stall bit and bumper bits, packed as (00 00 FrontBumpers RearBumpers)
int stall = robot->getStallValue();
unsigned char front_bumpers = (unsigned char)(stall >> 8);
unsigned char rear_bumpers = (unsigned char)(stall);
bumpers.header.frame_id = frame_id_bumper;
bumpers.header.stamp = ros::Time::now();
std::stringstream bumper_info(std::stringstream::out);
// Bit 0 is for stall, next bits are for bumpers (leftmost is LSB)
for (unsigned int i=0; i<robot->getNumFrontBumpers(); i++)
{
bumpers.front_bumpers[i] = (front_bumpers & (1 << (i+1))) == 0 ? 0 : 1;
bumper_info << " " << (front_bumpers & (1 << (i+1)));
}
ROS_DEBUG("RosAria: Front bumpers:%s", bumper_info.str().c_str());
bumper_info.str("");
// Rear bumpers have reverse order (rightmost is LSB)
unsigned int numRearBumpers = robot->getNumRearBumpers();
for (unsigned int i=0; i<numRearBumpers; i++)
{
bumpers.rear_bumpers[i] = (rear_bumpers & (1 << (numRearBumpers-i))) == 0 ? 0 : 1;
bumper_info << " " << (rear_bumpers & (1 << (numRearBumpers-i)));
}
ROS_DEBUG("RosAria: Rear bumpers:%s", bumper_info.str().c_str());
bumpers_pub.publish(bumpers);
//Publish battery information
// TODO: Decide if BatteryVoltageNow (normalized to (0,12)V) is a better option
std_msgs::Float64 batteryVoltage;
batteryVoltage.data = robot->getRealBatteryVoltageNow();
voltage_pub.publish(batteryVoltage);
if(robot->haveStateOfCharge())
{
std_msgs::Float32 soc;
soc.data = robot->getStateOfCharge()/100.0;
state_of_charge_pub.publish(soc);
}
// publish recharge state if changed
char s = robot->getChargeState();
if(s != recharge_state.data)
{
ROS_INFO("RosAria: publishing new recharge state %d.", s);
recharge_state.data = s;
recharge_state_pub.publish(recharge_state);
}
// publish motors state if changed
bool e = robot->areMotorsEnabled();
if(e != motors_state.data || !published_motors_state)
{
ROS_INFO("RosAria: publishing new motors state %d.", e);
motors_state.data = e;
motors_state_pub.publish(motors_state);
published_motors_state = true;
}
// Publish sonar information, if enabled.
if (publish_sonar || publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud cloud; //sonar readings.
cloud.header.stamp = position.header.stamp; //copy time.
// sonar sensors relative to base_link
cloud.header.frame_id = frame_id_sonar;
std::stringstream sonar_debug_info; // Log debugging info
sonar_debug_info << "Sonar readings: ";
for (int i = 0; i < robot->getNumSonar(); i++) {
ArSensorReading* reading = NULL;
reading = robot->getSonarReading(i);
if(!reading) {
ROS_WARN("RosAria: Did not receive a sonar reading.");
continue;
}
// getRange() will return an integer between 0 and 5000 (5m)
sonar_debug_info << reading->getRange() << " ";
// local (x,y). Appears to be from the centre of the robot, since values may
// exceed 5000. This is good, since it means we only need 1 transform.
// x & y seem to be swapped though, i.e. if the robot is driving north
// x is north/south and y is east/west.
//
//ArPose sensor = reading->getSensorPosition(); //position of sensor.
// sonar_debug_info << "(" << reading->getLocalX()
// << ", " << reading->getLocalY()
// << ") from (" << sensor.getX() << ", "
// << sensor.getY() << ") ;; " ;
//add sonar readings (robot-local coordinate frame) to cloud
geometry_msgs::Point32 p;
p.x = reading->getLocalX() / 1000.0;
p.y = reading->getLocalY() / 1000.0;
p.z = 0.0;
cloud.points.push_back(p);
}
ROS_DEBUG_STREAM(sonar_debug_info.str());
// publish topic(s)
if(publish_sonar_pointcloud2)
{
sensor_msgs::PointCloud2 cloud2;
if(!sensor_msgs::convertPointCloudToPointCloud2(cloud, cloud2))
{
ROS_WARN("Error converting sonar point cloud message to point_cloud2 type before publishing! Not publishing this time.");
}
else
{
sonar_pointcloud2_pub.publish(cloud2);
}
}
if(publish_sonar)
{
sonar_pub.publish(cloud);
}
} // end if sonar_enabled
}
bool RosAriaNode::enable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Enable motors request.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
robot->enableMotors();
robot->unlock();
// todo could wait and see if motors do become enabled, and send a response with an error flag if not
return true;
}
bool RosAriaNode::disable_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
ROS_INFO("RosAria: Disable motors request.");
robot->lock();
robot->disableMotors();
robot->unlock();
// todo could wait and see if motors do become disabled, and send a response with an error flag if not
return true;
}
//Damian Metody odpowiedzialne za awaryjne ztrzymanie silników
bool RosAriaNode::start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
stop_robot=false;
ROS_INFO("RosAria: Disable motors brakes.");
robot->lock();
if(robot->isEStopPressed())
ROS_WARN("RosAria: Warning: Enable motors requested, but robot also has E-Stop button pressed. Motors will not enable.");
//robot->enableMotors();
robot->unlock();
return true;
}
bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
stop_robot=true;
//Zatrzymuje robota
robot->setVel(0);
if(robot->hasLatVel())
robot->setLatVel(0);
robot->setRotVel(0);
robot->comInt(ArCommands::ESTOP,0);
ROS_INFO("RosAria: Enable motors brakes.");
robot->unlock();
return true;
}
void
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{
veltime = ros::Time::now();
if(stop_robot)
{
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f) but robot is stop", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
return;
}
ROS_INFO( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, veltime.toSec() );
robot->lock();
robot->setVel(msg->linear.x*1e3);
if(robot->hasLatVel())
robot->setLatVel(msg->linear.y*1e3);
robot->setRotVel(msg->angular.z*180/M_PI);
robot->unlock();
ROS_DEBUG("RosAria: sent vels to to aria (time %f): x vel %f mm/s, y vel %f mm/s, ang vel %f deg/s", veltime.toSec(),
(double) msg->linear.x * 1e3, (double) msg->linear.y * 1.3, (double) msg->angular.z * 180/M_PI);
}
int main( int argc, char** argv )
{
ros::init(argc,argv, "RosAria");
ros::NodeHandle n(std::string("~"));
Aria::init();
RosAriaNode *node = new RosAriaNode(n);
if( node->Setup() != 0 )
{
ROS_FATAL( "RosAria: ROS node setup failed... \n" );
return -1;
}
node->spin();
delete node;
ROS_INFO( "RosAria: Quitting... \n" );
return 0;
}

21
libaria.rdmanifest Normal file
View File

@ -0,0 +1,21 @@
uri: 'http://http.debian.net/debian/pool/main/liba/libaria/libaria_2.8.0+repack.orig.tar.bz2'
md5sum: d4adcc4e01e211ee3559d807f400f7d9
install-script: |
#!/bin/bash
set -o errexit
make -j $(nproc)
LD_LIBRARY_PATH=lib make params
echo "Uninstall previous version of libaria-sourcedep"
sudo dpkg -P libaria-sourcedep
echo "About to run checkinstall make install"
sudo checkinstall -y --nodoc --pkgname=libaria-sourcedep --pkgversion=2.8.0 make --ignore-errors install
check-presence-script: |
#!/bin/bash
if test "x`dpkg-query -W -f='${Package} ${Status} ${Version}\n' libaria-sourcedep`" != 'xlibaria-sourcedep install ok installed 2.8.0-1'; then
echo "libaria-sourcedep not installed"
exit 1
else
exit 0
fi
exec-path: libaria-2.8.0.orig
depends: [checkinstall ]

30
mainpage.dox Normal file
View File

@ -0,0 +1,30 @@
/**
\mainpage
\htmlinclude manifest.html
\b ROSARIA is ...
<!--
In addition to providing an overview of your package,
this is the section where the specification and design/architecture
should be detailed. While the original specification may be done on the
wiki, it should be transferred here once your package starts to take shape.
You can then link to this documentation page from the Wiki.
-->
\section codeapi Code API
<!--
Provide links to specific auto-generated API documentation within your
package that is of particular interest to a reader. Doxygen will
document pretty much every part of your code, so do your best here to
point the reader to the actual API.
If your codebase is fairly large or has different sets of APIs, you
should use the doxygen 'group' tag to keep these APIs together. For
example, the roscpp documentation has 'libros' group.
-->
*/

3
msg/BumperState.msg Normal file
View File

@ -0,0 +1,3 @@
Header header
bool[] front_bumpers
bool[] rear_bumpers

56
package.xml Normal file
View File

@ -0,0 +1,56 @@
<package>
<name>rosaria</name>
<version>0.9.0</version>
<description>
<tt>ROSARIA</tt> provides a ROS interface for most Adept MobileRobots,
MobileRobots Inc., and ActivMedia mobile robot bases including
Pioneer 2, Pioneer 3, AmigoBot, PeopleBot, PowerBot, PatrolBot, Seekur,
Seekur Jr., Pioneer LX,
and any other past, current or future robot base supported by Adept MobileRobot's
open source ARIA library.
Information from the robot base, and velocity and acceleration control, is implemented
via a <tt>RosAria</tt> node, which publishes topics providing data recieved from
the robot's embedded controller by ARIA, and sets desired velocity, acceleration and
other commands in ARIA when new commands are received from command topics.
</description>
<maintainer email="srecko.juric-kavelj@fer.hr">Srećko Jurić-Kavelj</maintainer>
<maintainer email="ivan.markovic@fer.hr">Ivan Marković</maintainer>
<maintainer email="reed.hedges@aedpt.com">Reed Hedges</maintainer>
<license>GPLv2</license>
<url type="website">http://www.ros.org/wiki/ROSARIA</url>
<url type="bugtracker">https://github.com/amor-ros-pkg/rosaria/issues</url>
<author>Srećko Jurić-Kavelj</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>libaria</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<run_depend>libaria</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<build_depend>std_msgs</build_depend>
<run_depend>tf</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<!-- <test_depend>Aria</test_depend>
<test_depend>roscpp</test_depend>
<test_depend>nav_msgs</test_depend>
<test_depend>geometry_msgs</test_depend>
<test_depend>sensor_msgs</test_depend>
<test_depend>tf</test_depend> -->
<export>
</export>
</package>