commit ec1ab02895bed0a4d52abd9abb4bd4dc6bbe5641 Author: Damian Barański Date: Wed Feb 18 11:55:51 2015 +0100 Inicjalizacja diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..77da5e3 --- /dev/null +++ b/CMakeLists.txt @@ -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} +) diff --git a/RosAria.cpp b/RosAria.cpp new file mode 100644 index 0000000..8c9bbf7 --- /dev/null +++ b/RosAria.cpp @@ -0,0 +1,766 @@ +#include +#include +#ifdef ADEPT_PKG + #include +#else + #include +//Aria/Aria.h> +#endif +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/PoseStamped.h" +#include //for sonar data +#include +#include // 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 +#include "tf/transform_datatypes.h" +#include +#include +#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 + + +// 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 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 *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 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("pose",1000); + bumpers_pub = n.advertise("bumper_state",1000); + sonar_pub = n.advertise("sonar", 50, + boost::bind(&RosAriaNode::sonarConnectCb, this), + boost::bind(&RosAriaNode::sonarConnectCb, this)); + sonar_pointcloud2_pub = n.advertise("sonar_pointcloud2", 50, + boost::bind(&RosAriaNode::sonarConnectCb, this), + boost::bind(&RosAriaNode::sonarConnectCb, this)); + + voltage_pub = n.advertise("battery_voltage", 1000); + recharge_state_pub = n.advertise("battery_recharge_state", 5, true /*latch*/ ); + recharge_state.data = -2; + state_of_charge_pub = n.advertise("battery_state_of_charge", 100); + + motors_state_pub = n.advertise("motors_state", 5, true /*latch*/ ); + stop_motors_pub = n.advertise("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 ) + 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; + + // 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; igetNumFrontBumpers(); 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; igetRealBatteryVoltageNow(); + 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; + +} diff --git a/cfg/RosAria.cfg b/cfg/RosAria.cfg new file mode 100755 index 0000000..309e332 --- /dev/null +++ b/cfg/RosAria.cfg @@ -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")) diff --git a/lab1_5@10.104.16.131 b/lab1_5@10.104.16.131 new file mode 100644 index 0000000..4fcd9d6 --- /dev/null +++ b/lab1_5@10.104.16.131 @@ -0,0 +1,752 @@ +#include +#include +#ifdef ADEPT_PKG + #include +#else + #include +//Aria/Aria.h> +#endif +#include "ros/ros.h" +#include "geometry_msgs/Twist.h" +#include "geometry_msgs/Pose.h" +#include "geometry_msgs/PoseStamped.h" +#include //for sonar data +#include +#include // 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 +#include "tf/transform_datatypes.h" +#include +#include +#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 + + +// 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 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 *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 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("pose",1000); + bumpers_pub = n.advertise("bumper_state",1000); + sonar_pub = n.advertise("sonar", 50, + boost::bind(&RosAriaNode::sonarConnectCb, this), + boost::bind(&RosAriaNode::sonarConnectCb, this)); + sonar_pointcloud2_pub = n.advertise("sonar_pointcloud2", 50, + boost::bind(&RosAriaNode::sonarConnectCb, this), + boost::bind(&RosAriaNode::sonarConnectCb, this)); + + voltage_pub = n.advertise("battery_voltage", 1000); + recharge_state_pub = n.advertise("battery_recharge_state", 5, true /*latch*/ ); + recharge_state.data = -2; + state_of_charge_pub = n.advertise("battery_state_of_charge", 100); + + motors_state_pub = n.advertise("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 ) + 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; + + // 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; igetNumFrontBumpers(); 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; igetRealBatteryVoltageNow(); + 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; + +} diff --git a/libaria.rdmanifest b/libaria.rdmanifest new file mode 100644 index 0000000..b08c930 --- /dev/null +++ b/libaria.rdmanifest @@ -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 ] diff --git a/mainpage.dox b/mainpage.dox new file mode 100644 index 0000000..02e374e --- /dev/null +++ b/mainpage.dox @@ -0,0 +1,30 @@ +/** +\mainpage +\htmlinclude manifest.html + +\b ROSARIA is ... + + + + +\section codeapi Code API + + + + +*/ \ No newline at end of file diff --git a/msg/BumperState.msg b/msg/BumperState.msg new file mode 100644 index 0000000..65dfcc1 --- /dev/null +++ b/msg/BumperState.msg @@ -0,0 +1,3 @@ +Header header +bool[] front_bumpers +bool[] rear_bumpers diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..65de483 --- /dev/null +++ b/package.xml @@ -0,0 +1,56 @@ + + rosaria + 0.9.0 + + ROSARIA 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 RosAria 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. + + Srećko Jurić-Kavelj + Ivan Marković + Reed Hedges + + GPLv2 + + http://www.ros.org/wiki/ROSARIA + https://github.com/amor-ros-pkg/rosaria/issues + + Srećko Jurić-Kavelj + + catkin + + message_generation + libaria + roscpp + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + tf + dynamic_reconfigure + libaria + roscpp + nav_msgs + geometry_msgs + sensor_msgs + std_msgs + tf + dynamic_reconfigure + + + + + +