33 n(nh), gripper_dirty_(false),
34 batt_pub_( n.advertise<p2os_msgs::BatteryState>(
"battery_state",1000),
36 diagnostic_updater::FrequencyStatusParam( &desired_freq, &desired_freq, 0.1),
37 diagnostic_updater::TimeStampStatusParam() ),
46 ros::NodeHandle n_private(
"~");
47 n_private.param(
"use_sonar",
use_sonar_,
false);
51 n_private.param(
"bumpstall",
bumpstall, -1 );
53 n_private.param(
"pulse",
pulse, -1.0 );
55 n_private.param(
"rot_kp",
rot_kp, -1 );
57 n_private.param(
"rot_kv",
rot_kv, -1 );
59 n_private.param(
"rot_ki",
rot_ki, -1 );
61 n_private.param(
"trans_kp",
trans_kp, -1 );
63 n_private.param(
"trans_kv",
trans_kv, -1 );
65 n_private.param(
"trans_ki",
trans_ki, -1 );
77 n_private.param(
"joystick",
joystick, 0 );
88 n_private.param(
"max_xaccel", spd, 0.0);
91 n_private.param(
"max_xdecel", spd, 0.0);
94 n_private.param(
"max_yawaccel", spd, 0.0);
97 n_private.param(
"max_yawdecel", spd, 0.0);
103 pose_pub_ =
n.advertise<nav_msgs::Odometry>(
"pose", 1000);
105 mstate_pub_ =
n.advertise<p2os_msgs::MotorState>(
"motor_state",1000);
106 grip_state_pub_ =
n.advertise<p2os_msgs::GripperState>(
"gripper_state",1000);
108 sonar_pub_ =
n.advertise<p2os_msgs::SonarArray>(
"sonar", 1000);
109 aio_pub_ =
n.advertise<p2os_msgs::AIO>(
"aio", 1000);
110 dio_pub_ =
n.advertise<p2os_msgs::DIO>(
"dio", 1000);
146 unsigned char command[4];
152 packet.Build(command,4);
165 unsigned char grip_val = (
unsigned char)
gripper_state_.grip.state;
166 unsigned char grip_command[4];
170 grip_command[2] = grip_val;
172 grip_packet.Build(grip_command,4);
176 unsigned char lift_val = (
unsigned char)
gripper_state_.lift.state;
177 unsigned char lift_command[4];
181 lift_command[2] = lift_val;
183 lift_packet.Build(lift_command,4);
190 if( fabs( msg->linear.x -
cmdvel_.linear.x ) > 0.01 || fabs( msg->angular.z-
cmdvel_.angular.z) > 0.01 )
193 ROS_DEBUG(
"new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z,
veltime.toSec() );
199 ros::Duration veldur = ros::Time::now() -
veltime;
200 if( veldur.toSec() > 2.0 && ((fabs(
cmdvel_.linear.x) > 0.01) || (fabs(
cmdvel_.angular.z) > 0.01)) )
202 ROS_DEBUG(
"maintaining old speed: %0.3f|%0.3f",
veltime.toSec(), ros::Time::now().toSec() );
214 ROS_DEBUG(
"setting vel: [%0.2f,%0.2f]",
cmdvel_.linear.x,
cmdvel_.angular.z);
217 unsigned short absSpeedDemand, absturnRateDemand;
218 unsigned char motorcommand[4];
221 int vx = (int) (
cmdvel_.linear.x*1e3);
226 motorcommand[0] =
VEL;
227 if( vx >= 0 ) motorcommand[1] =
ARGINT;
228 else motorcommand[1] =
ARGNINT;
230 absSpeedDemand = (
unsigned short)abs(vx);
233 motorcommand[2] = absSpeedDemand & 0x00FF;
234 motorcommand[3] = (absSpeedDemand & 0xFF00) >> 8;
238 ROS_WARN(
"speed demand thresholded! (true: %u, max: %u)", absSpeedDemand, motor_max_speed );
239 motorcommand[2] = motor_max_speed & 0x00FF;
240 motorcommand[3] = (motor_max_speed & 0xFF00) >> 8;
242 motorpacket.
Build(motorcommand, 4);
245 motorcommand[0] =
RVEL;
246 if( va >= 0 ) motorcommand[1] =
ARGINT;
247 else motorcommand[1] =
ARGNINT;
249 absturnRateDemand = (
unsigned short)abs(va);
252 motorcommand[2] = absturnRateDemand & 0x00FF;
253 motorcommand[3] = (absturnRateDemand & 0xFF00) >> 8;
257 ROS_WARN(
"Turn rate demand threshholded!");
262 motorpacket.
Build(motorcommand,4);
276 int bauds[] = {B9600, B38400, B19200, B115200, B57600};
277 int numbauds =
sizeof(bauds);
283 unsigned char command;
286 bool sent_close =
false;
295 psos_state = NO_SYNC;
297 char name[20], type[20], subtype[20];
303 ROS_INFO(
"P2OS connection opening serial port %s...",
psos_serial_port.c_str());
306 O_RDWR | O_SYNC | O_NONBLOCK, S_IRUSR | S_IWUSR )) < 0 )
308 ROS_ERROR(
"P2OS::Setup():open():");
312 if(tcgetattr( this->
psos_fd, &term ) < 0 )
314 ROS_ERROR(
"P2OS::Setup():tcgetattr():");
321 cfsetispeed(&term, bauds[currbaud]);
322 cfsetospeed(&term, bauds[currbaud]);
324 if(tcsetattr(this->
psos_fd, TCSAFLUSH, &term ) < 0)
326 ROS_ERROR(
"P2OS::Setup():tcsetattr():");
332 if(tcflush(this->
psos_fd, TCIOFLUSH ) < 0)
334 ROS_ERROR(
"P2OS::Setup():tcflush():");
340 if((flags = fcntl(this->
psos_fd, F_GETFL)) < 0)
342 ROS_ERROR(
"P2OS::Setup():fcntl()");
349 int num_sync_attempts = 3;
350 while(psos_state != READY)
356 packet.
Build(&command, 1);
360 case AFTER_FIRST_SYNC:
361 ROS_INFO(
"turning off NONBLOCK mode...");
362 if(fcntl(this->
psos_fd, F_SETFL, flags ^ O_NONBLOCK) < 0)
364 ROS_ERROR(
"P2OS::Setup():fcntl()");
370 packet.
Build(&command, 1);
373 case AFTER_SECOND_SYNC:
375 packet.
Build(&command, 1);
379 ROS_WARN(
"P2OS::Setup():shouldn't be here...");
384 if(receivedpacket.
Receive(this->psos_fd))
386 if((psos_state == NO_SYNC) && (num_sync_attempts >= 0))
395 if(++currbaud < numbauds)
397 cfsetispeed(&term, bauds[currbaud]);
398 cfsetospeed(&term, bauds[currbaud]);
399 if( tcsetattr(this->
psos_fd, TCSAFLUSH, &term ) < 0 )
401 ROS_ERROR(
"P2OS::Setup():tcsetattr():");
407 if(tcflush(this->
psos_fd, TCIOFLUSH ) < 0 )
409 ROS_ERROR(
"P2OS::Setup():tcflush():");
414 num_sync_attempts = 3;
424 switch(receivedpacket.
packet[3])
428 psos_state = AFTER_FIRST_SYNC;
432 psos_state = AFTER_SECOND_SYNC;
443 ROS_DEBUG(
"sending CLOSE");
445 packet.
Build( &command, 1);
449 tcflush(this->
psos_fd,TCIFLUSH);
450 psos_state = NO_SYNC;
456 if(psos_state != READY)
459 ROS_INFO(
"Couldn't synchronize with P2OS.\n"
460 " Most likely because the robot is not connected %s %s",
461 this->
psos_use_tcp ?
"to the ethernet-serial bridge device " :
"to the serial port",
468 cnt += snprintf(name,
sizeof(name),
"%s", &receivedpacket.
packet[cnt]);
470 cnt += snprintf(type,
sizeof(type),
"%s", &receivedpacket.
packet[cnt]);
472 cnt += snprintf(subtype,
sizeof(subtype),
"%s", &receivedpacket.
packet[cnt]);
475 std::string hwID = std::string( name ) + std::string(
": ") + std::string(type) + std::string(
"/") + std::string( subtype );
479 packet.
Build(&command, 1);
483 packet.
Build(&command, 1);
487 ROS_INFO(
"Done.\n Connected to %s, a %s %s", name, type, subtype);
499 if(i == PLAYER_NUM_ROBOT_TYPES)
501 ROS_WARN(
"P2OS: Warning: couldn't find parameters for this robot; "
524 unsigned char accel_command[4];
527 accel_command[0] =
SETA;
528 accel_command[1] =
ARGINT;
531 accel_packet.
Build(accel_command, 4);
537 accel_command[0] =
SETA;
541 accel_packet.
Build(accel_command, 4);
546 accel_command[0] =
SETRA;
547 accel_command[1] =
ARGINT;
550 accel_packet.
Build(accel_command, 4);
555 accel_command[0] =
SETRA;
559 accel_packet.
Build(accel_command, 4);
566 unsigned char pid_command[4];
569 pid_command[0] =
ROTKP;
571 pid_command[2] = this->
rot_kp & 0x00FF;
572 pid_command[3] = (this->
rot_kp & 0xFF00) >> 8;
573 pid_packet.
Build(pid_command, 4);
578 pid_command[0] =
ROTKV;
580 pid_command[2] = this->
rot_kv & 0x00FF;
581 pid_command[3] = (this->
rot_kv & 0xFF00) >> 8;
582 pid_packet.
Build(pid_command, 4);
587 pid_command[0] =
ROTKI;
589 pid_command[2] = this->
rot_ki & 0x00FF;
590 pid_command[3] = (this->
rot_ki & 0xFF00) >> 8;
591 pid_packet.
Build(pid_command, 4);
598 pid_command[2] = this->
trans_kp & 0x00FF;
599 pid_command[3] = (this->
trans_kp & 0xFF00) >> 8;
600 pid_packet.
Build(pid_command, 4);
607 pid_command[2] = this->
trans_kv & 0x00FF;
608 pid_command[3] = (this->
trans_kv & 0xFF00) >> 8;
609 pid_packet.
Build(pid_command, 4);
616 pid_command[2] = this->
trans_ki & 0x00FF;
617 pid_command[3] = (this->
trans_ki & 0xFF00) >> 8;
618 pid_packet.
Build(pid_command, 4);
631 ROS_INFO(
"ignoring bumpstall value %d; should be 0, 1, 2, or 3",
635 ROS_INFO(
"setting bumpstall to %d", this->
bumpstall);
637 unsigned char bumpstall_command[4];
639 bumpstall_command[1] =
ARGINT;
640 bumpstall_command[2] = (
unsigned char)this->
bumpstall;
641 bumpstall_command[3] = 0;
642 bumpstall_packet.
Build(bumpstall_command, 4);
650 ROS_DEBUG(
"Sonar array powered on.");
659 unsigned char command[20],buffer[20];
673 packet.
Build(command, 1);
678 packet.
Build(command, 1);
684 ROS_INFO(
"P2OS has been shutdown");
734 pthread_testcancel();
735 if(packet.
Receive(this->psos_fd))
737 ROS_ERROR(
"RunPsosThread(): Receive errored");
741 if(packet.
packet[0] == 0xFA && packet.
packet[1] == 0xFB &&
742 (packet.
packet[3] == 0x30 || packet.
packet[3] == 0x31 ||
744 packet.
packet[3] == 0x34))
754 else if(packet.
packet[0] == 0xFA && packet.
packet[1] == 0xFB &&
760 int len = packet.
packet[2] - 3;
763 ROS_ERROR(
"PTZ got a message, but alread has the complete packet.");
767 for (
int i=4; i < 4+len; ++i)
776 ROS_ERROR(
"Received other packet!");
794 stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
"battery voltage critically low" );
796 else if( voltage < 11.75 )
798 stat.summary( diagnostic_msgs::DiagnosticStatus::WARN,
"battery voltage getting low" );
801 else stat.summary( diagnostic_msgs::DiagnosticStatus::OK,
"battery voltage OK" );
803 stat.add(
"voltage", voltage );
810 stat.summary( diagnostic_msgs::DiagnosticStatus::ERROR,
"wheel stalled" );
812 else stat.summary( diagnostic_msgs::DiagnosticStatus::OK,
"no wheel stall" );
821 unsigned char p2oscommand[4];
829 p2oscommand[0] =
SETO;
831 pkt.
Build(p2oscommand, 2);
833 ROS_INFO(
"resetting raw positions" );
840 unsigned char command[4];
847 packet.
Build(command, 4);
858 unsigned char command[4];
866 packet.
Build(command, 4);
884 result = result * pos;
885 if ((joint >= 0) && (joint <= 2))
902 if ((joint >= 0) && (joint <= 2))
906 if (val < sippacket->armJoints[joint].min)
911 return static_cast<int> (round (val));
924 unsigned char result =
static_cast<unsigned char> (
DegreesToTicks (joint,
RTOD (rads)));
931 double degs =
RTOD (speed);
933 double ticksPerSec = degs * ticksPerDeg;
934 double secsPerTick = 1000.0f / ticksPerSec;
936 if (secsPerTick > 127)
938 else if (secsPerTick < 1)
946 double ticksPerSec = 1.0 / (
static_cast<double> (msecs) / 1000.0);
948 double degs = ticksPerSec / ticksPerDeg;
949 double rads =
DTOR (degs);
956 unsigned char command;
960 packet.
Build(&command, 1);
std::string psos_tcp_host
double TicksToDegrees(int joint, unsigned char ticks)
Convert ticks to degrees.
short motor_max_trans_decel
Minimum translational acceleration in Meters per second per second.
diagnostic_updater::Updater diagnostic_
int motor_max_speed
Maximum motor speed in Meters per second.
p2os_msgs::PTZState getCurrentState()
#define DEFAULT_P2OS_TCP_REMOTE_HOST
void ToggleMotorPower(unsigned char val)
Toggle motors on/off.
ros::Publisher mstate_pub_
int SendReceive(P2OSPacket *pkt, bool publish_data=true)
p2os_msgs::MotorState cmdmotor_state_
Motor state publisher.
p2os_msgs::SonarArray sonar
Container for sonar data.
double RadsPerSectoSecsPerTick(int joint, double speed)
Convert radians per second to radians per encoder tick.
#define DEFAULT_P2OS_TCP_REMOTE_PORT
bool use_sonar_
Use the sonar array?
unsigned char packet[PACKET_LEN]
unsigned char RadiansToTicks(int joint, double rads)
Convert radians to ticks.
p2os_msgs::AIO aio
Analog In/Out.
ros::NodeHandle n
Node Handler used for publication of data.
void check_voltage(diagnostic_updater::DiagnosticStatusWrapper &stat)
#define PLAYER_NUM_ROBOT_TYPES
std::string psos_serial_port
int joystick
Use Joystick?
void gripperCallback(const p2os_msgs::GripperStateConstPtr &msg)
p2os_msgs::GripperState gripper
Provides the state of the gripper.
geometry_msgs::TransformStamped odom_trans
Transformed odometry frame.
ros::Publisher sonar_pub_
void check_and_set_gripper_state()
tf::TransformBroadcaster odom_broadcaster
#define MOTOR_DEF_MAX_SPEED
RobotParams_t PlayerRobotParams[]
void ParseStandard(unsigned char *buffer)
void putOnBuf(unsigned char c)
diagnostic_updater::DiagnosedPublisher< p2os_msgs::BatteryState > batt_pub_
int Build(unsigned char *data, unsigned char datasize)
int bumpstall
Stall I hit a wall?
short motor_max_rot_accel
Maximum rotational acceleration in radians per second per second.
p2os_msgs::GripperState gripper_state_
Gripper state publisher.
void callback(const p2os_msgs::PTZStateConstPtr &msg)
ros::Publisher grip_state_pub_
int Setup()
Setup the robot for use. Communicates with the robot directly.
P2OSNode(ros::NodeHandle n)
P2OS robot driver node.
ros::Subscriber ptz_cmd_sub_
void cmdmotor_state(const p2os_msgs::MotorStateConstPtr &)
ros::Subscriber cmdvel_sub_
p2os_msgs::BatteryState batt
Provides the battery voltage.
int motor_max_turnspeed
Maximum turn speed in radians per second.
int Shutdown()
Prepare for shutdown.
double lastPulseTime
Last time the node received or sent a pulse.
void initialize_robot_params(void)
void FillStandard(ros_p2os_data_t *data)
#define P2OS_CYCLETIME_USEC
ros::Subscriber gripper_sub_
unsigned char armNumJoints
ros::Subscriber cmdmstate_sub_
void StandardSIPPutData(ros::Time ts)
void ToggleSonarPower(unsigned char val)
short motor_max_trans_accel
Maximum translational acceleration in Meters per second per second.
double TicksToRadians(int joint, unsigned char ticks)
Convert ticks to radians.
short motor_max_rot_decel
Minimum rotational acceleration in Meters per second per second.
ros_p2os_data_t p2os_data
sensor data container
geometry_msgs::Twist cmdvel_
Command Velocity subscriber.
#define DEFAULT_P2OS_PORT
p2os_msgs::MotorState motors
Provides the state of the motors (enabled or disabled)
double SecsPerTicktoRadsPerSec(int joint, double secs)
Convert Seconds per encoder tick to radians per second.
unsigned char DegreesToTicks(int joint, double degrees)
convert degrees to ticks
void check_and_set_motor_state()
void cmdvel_cb(const geometry_msgs::TwistConstPtr &)
ros::Publisher ptz_state_pub_
nav_msgs::Odometry position
Provides the position of the robot.
int direct_wheel_vel_control
Control wheel velocities individually?
#define MOTOR_DEF_MAX_TURNSPEED
void check_stall(diagnostic_updater::DiagnosticStatusWrapper &stat)
p2os_msgs::DIO dio
Digital In/Out.