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

Add full support for gripper

Browse Source
This commit is contained in:
Damian Barański 2015-03-03 10:04:26 +01:00
parent dec06e1ea1
commit 4a1ee2f368
1 changed files with 589 additions and 536 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
RosAria.cpp
View File

@ -2,9 +2,9 @@
#include <stdio.h> #include <stdio.h>
#include <math.h> #include <math.h>
#ifdef ADEPT_PKG #ifdef ADEPT_PKG
#include <Aria.h> #include <Aria.h>
#else #else
#include </usr/local/Aria/include/Aria.h> #include </usr/local/Aria/include/Aria.h>
//Aria/Aria.h> //Aria/Aria.h>
#endif #endif
#include "ros/ros.h" #include "ros/ros.h"
@ -37,11 +37,11 @@
// information, tutorials and documentation. // information, tutorials and documentation.
class RosAriaNode class RosAriaNode
{ {
public: public:
RosAriaNode(ros::NodeHandle n); RosAriaNode(ros::NodeHandle n);
virtual ~RosAriaNode(); virtual ~RosAriaNode();
public: public:
int Setup(); int Setup();
void cmdvel_cb( const geometry_msgs::TwistConstPtr &); void cmdvel_cb( const geometry_msgs::TwistConstPtr &);
//void cmd_enable_motors_cb(); //void cmd_enable_motors_cb();
@ -52,7 +52,7 @@ class RosAriaNode
void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level); void dynamic_reconfigureCB(rosaria::RosAriaConfig &config, uint32_t level);
void readParameters(); void readParameters();
protected: protected:
int WATCHDOG; int WATCHDOG;
ros::NodeHandle n; ros::NodeHandle n;
ros::Publisher pose_pub; ros::Publisher pose_pub;
@ -83,6 +83,17 @@ class RosAriaNode
bool stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response); 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); bool start_motors_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
//Gripper
ros::ServiceServer gripper_open_srv;
ros::ServiceServer gripper_close_srv;
ros::ServiceServer gripper_up_srv;
ros::ServiceServer gripper_down_srv;
bool gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool gripper_close_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool gripper_up_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
bool gripper_down_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
ros::Time veltime; ros::Time veltime;
std::string serial_port; std::string serial_port;
@ -348,6 +359,12 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this); enable_srv = n.advertiseService("enable_motors", &RosAriaNode::enable_motors_cb, this);
disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this); disable_srv = n.advertiseService("disable_motors", &RosAriaNode::disable_motors_cb, this);
//Damian Gripper
gripper_open_srv = n.advertiseService("gripper_open", &RosAriaNode::gripper_open_cb, this);
gripper_close_srv = n.advertiseService("gripper_close", &RosAriaNode::gripper_close_cb, this);
gripper_up_srv = n.advertiseService("gripper_up", &RosAriaNode::gripper_up_cb, this);
gripper_down_srv = n.advertiseService("gripper_down", &RosAriaNode::gripper_down_cb, this);
//Damian usługi stopu awaryjnego //Damian usługi stopu awaryjnego
stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this); stop_motors_srv = n.advertiseService("stop_motors", &RosAriaNode::stop_motors_cb, this);
start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this); start_motors_srv = n.advertiseService("start_motors", &RosAriaNode::start_motors_cb, this);
@ -355,7 +372,7 @@ RosAriaNode::RosAriaNode(ros::NodeHandle nh) :
stop_motors_msg.data=false; stop_motors_msg.data=false;
veltime = ros::Time::now(); veltime = ros::Time::now();
gripper->gripOpen();
} }
RosAriaNode::~RosAriaNode() RosAriaNode::~RosAriaNode()
@ -376,6 +393,7 @@ int RosAriaNode::Setup()
// called once per instance, and these objects need to persist until the process terminates. // called once per instance, and these objects need to persist until the process terminates.
robot = new ArRobot(); robot = new ArRobot();
gripper = new ArGripper(robot);
ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed ArArgumentBuilder *args = new ArArgumentBuilder(); // never freed
ArArgumentParser *argparser = new ArArgumentParser(args); // Warning 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) argparser->loadDefaultArguments(); // adds any arguments given in /etc/Aria.args. Useful on robots with unusual serial port or baud rate (e.g. pioneer lx)
@ -506,7 +524,6 @@ int RosAriaNode::Setup()
// Run ArRobot background processing thread // Run ArRobot background processing thread
robot->runAsync(true); robot->runAsync(true);
return 0; return 0;
} }
@ -749,8 +766,44 @@ bool RosAriaNode::stop_motors_cb(std_srvs::Empty::Request& request, std_srvs::Em
robot->unlock(); robot->unlock();
return true; return true;
} }
void bool RosAriaNode::gripper_open_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg) {
robot->lock();
gripper->gripOpen();
ROS_INFO("RosAria: Gripper opening.");
robot->unlock();
return true;
}
bool RosAriaNode::gripper_close_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
gripper->gripClose();
ROS_INFO("RosAria: Gripper closing.");
robot->unlock();
return true;
}
bool RosAriaNode::gripper_up_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
gripper->liftUp();
ROS_INFO("RosAria: Gripper moving up");
robot->unlock();
return true;
}
bool RosAriaNode::gripper_down_cb(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
{
robot->lock();
gripper->liftDown();
ROS_INFO("RosAria: Gripper moving down.");
robot->unlock();
return true;
}
void RosAriaNode::cmdvel_cb( const geometry_msgs::TwistConstPtr &msg)
{ {
veltime = ros::Time::now(); veltime = ros::Time::now();
watchdog= ros::Time::now(); watchdog= ros::Time::now();