Adds Simulator from OpenQuadruped/spot_mini_mini
This commit is contained in:
Rune Harlyk
@@ -0,0 +1,118 @@
|
||||
#include "mini_ros/spot.hpp"
|
||||
|
||||
namespace spot
|
||||
{
|
||||
|
||||
// Spot Constructor
|
||||
Spot::Spot()
|
||||
{
|
||||
cmd.x_velocity = 0.0;
|
||||
cmd.y_velocity = 0.0;
|
||||
cmd.rate = 0.0;
|
||||
cmd.roll = 0.0;
|
||||
cmd.pitch = 0.0;
|
||||
cmd.yaw = 0.0;
|
||||
cmd.z = 0.0;
|
||||
cmd.motion = Stop;
|
||||
cmd.movement = Viewing;
|
||||
}
|
||||
|
||||
void Spot::update_command(const double & vx, const double & vy, const double & z,
|
||||
const double & w, const double & wx, const double & wy)
|
||||
{
|
||||
// If Command is nearly zero, just give zero
|
||||
if (almost_equal(vx, 0.0) and almost_equal(vy, 0.0) and almost_equal(z, 0.0) and almost_equal(w, 0.0))
|
||||
{
|
||||
cmd.motion = Stop;
|
||||
cmd.x_velocity = 0.0;
|
||||
cmd.y_velocity = 0.0;
|
||||
cmd.rate = 0.0;
|
||||
cmd.roll = 0.0;
|
||||
cmd.pitch = 0.0;
|
||||
cmd.yaw = 0.0;
|
||||
cmd.z = 0.0;
|
||||
cmd.faster = 0.0;
|
||||
cmd.slower = 0.0;
|
||||
} else
|
||||
{
|
||||
cmd.motion = Go;
|
||||
if (cmd.movement == Stepping)
|
||||
{
|
||||
// Stepping Mode, use commands as vx, vy, rate, Z
|
||||
cmd.x_velocity = vx;
|
||||
cmd.y_velocity = vy;
|
||||
cmd.rate = w;
|
||||
cmd.z = z;
|
||||
cmd.roll = 0.0;
|
||||
cmd.pitch = 0.0;
|
||||
cmd.yaw = 0.0;
|
||||
// change clearance height from +- 0-2 * scaling
|
||||
cmd.faster = 1.0 - wx;
|
||||
cmd.slower = -(1.0 - wy);
|
||||
} else
|
||||
{
|
||||
// Viewing Mode, use commands as RPY, Z
|
||||
cmd.x_velocity = 0.0;
|
||||
cmd.y_velocity = 0.0;
|
||||
cmd.rate = 0.0;
|
||||
cmd.roll = vy;
|
||||
cmd.pitch = vx;
|
||||
cmd.yaw = w;
|
||||
cmd.z = z;
|
||||
cmd.faster = 0.0;
|
||||
cmd.slower = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Spot::switch_movement()
|
||||
{
|
||||
if (!almost_equal(cmd.x_velocity, 0.0) and !almost_equal(cmd.y_velocity, 0.0) and !almost_equal(cmd.rate, 0.0))
|
||||
{
|
||||
ROS_WARN("MAKE SURE BOTH LINEAR [%.2f, %.2f] AND ANGULAR VELOCITY [%.2f] ARE AT 0.0 BEFORE SWITCHING!", cmd.x_velocity, cmd.y_velocity, cmd.rate);
|
||||
|
||||
ROS_WARN("STOPPING ROBOT...");
|
||||
|
||||
cmd.motion = Stop;
|
||||
cmd.x_velocity = 0.0;
|
||||
cmd.y_velocity = 0.0;
|
||||
cmd.rate = 0.0;
|
||||
cmd.roll = 0.0;
|
||||
cmd.pitch = 0.0;
|
||||
cmd.yaw = 0.0;
|
||||
cmd.z = 0.0;
|
||||
cmd.faster = 0.0;
|
||||
cmd.slower = 0.0;
|
||||
} else
|
||||
{
|
||||
cmd.x_velocity = 0.0;
|
||||
cmd.y_velocity = 0.0;
|
||||
cmd.rate = 0.0;
|
||||
cmd.roll = 0.0;
|
||||
cmd.pitch = 0.0;
|
||||
cmd.yaw = 0.0;
|
||||
cmd.z = 0.0;
|
||||
cmd.faster = 0.0;
|
||||
cmd.slower = 0.0;
|
||||
if (cmd.movement == Viewing)
|
||||
{
|
||||
ROS_INFO("SWITCHING TO STEPPING MOTION, COMMANDS NOW MAPPED TO VX|VY|W|Z.");
|
||||
|
||||
cmd.movement = Stepping;
|
||||
cmd.motion = Stop;
|
||||
} else
|
||||
{
|
||||
ROS_INFO("SWITCHING TO VIEWING MOTION, COMMANDS NOW MAPPED TO R|P|Y|Z.");
|
||||
|
||||
cmd.movement = Viewing;
|
||||
cmd.motion = Stop;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
SpotCommand Spot::return_command()
|
||||
{
|
||||
return cmd;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
#include "mini_ros/teleop.hpp"
|
||||
|
||||
namespace tele
|
||||
{
|
||||
Teleop::Teleop(const int & linear_x, const int & linear_y, const int & linear_z,
|
||||
const int & angular, const double & l_scale, const double & a_scale,
|
||||
const int & LB, const int & RB, const int & B_scale, const int & LT,
|
||||
const int & RT, const int & UD, const int & LR,
|
||||
const int & sw, const int & es)
|
||||
{
|
||||
// step length or pitch
|
||||
linear_x_ = linear_x;
|
||||
// lateral fraction or roll
|
||||
linear_y_ = linear_y;
|
||||
// height
|
||||
linear_z_ = linear_z;
|
||||
// yaw rate or yaw
|
||||
angular_ = angular;
|
||||
// scale linear axis (usually just 1)
|
||||
l_scale_ = l_scale;
|
||||
// scale angular axis (usually just 1)
|
||||
a_scale_ = a_scale;
|
||||
// for incrementing and decrementing step velocity
|
||||
// Bottom Bumpers
|
||||
RB_ = RB;
|
||||
LB_ = LB;
|
||||
// scale Bottom Trigger axis (usually just 1)
|
||||
B_scale_ = B_scale;
|
||||
// Top Bumpers
|
||||
RT_ = RT;
|
||||
LT_ = LT;
|
||||
// Switch between Stepping and Viewing
|
||||
sw_ = sw;
|
||||
// E-STOP
|
||||
es_ = es;
|
||||
|
||||
// Arrow PAd
|
||||
UD_ = UD;
|
||||
LR_ = LR;
|
||||
|
||||
switch_trigger = false;
|
||||
ESTOP = false;
|
||||
updown = 0;
|
||||
leftright = 0;
|
||||
|
||||
left_bump = false;
|
||||
right_bump = false;
|
||||
}
|
||||
|
||||
void Teleop::joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
|
||||
{
|
||||
twist.linear.x = l_scale_*joy->axes[linear_x_];
|
||||
twist.linear.y = l_scale_*joy->axes[linear_y_];
|
||||
// NOTE: used to control robot height
|
||||
twist.linear.z = -l_scale_*joy->axes[linear_z_];
|
||||
twist.angular.z = a_scale_*joy->axes[angular_];
|
||||
// NOTE: bottom bumpers used for changing step velocity
|
||||
twist.angular.x = B_scale_*joy->axes[RB_];
|
||||
twist.angular.y = B_scale_*joy->axes[LB_];
|
||||
|
||||
|
||||
// Switch Trigger: Button A
|
||||
switch_trigger = joy->buttons[sw_];
|
||||
|
||||
// ESTOP: Button B
|
||||
ESTOP = joy->buttons[es_];
|
||||
|
||||
// Arrow Pad
|
||||
updown = joy->axes[UD_];
|
||||
leftright = -joy->axes[LR_];
|
||||
|
||||
// Top Bumpers
|
||||
left_bump = joy->buttons[LT_];
|
||||
right_bump = joy->buttons[RT_];
|
||||
}
|
||||
|
||||
geometry_msgs::Twist Teleop::return_twist()
|
||||
{
|
||||
return twist;
|
||||
}
|
||||
|
||||
bool Teleop::return_trigger()
|
||||
{
|
||||
return switch_trigger;
|
||||
}
|
||||
|
||||
bool Teleop::return_estop()
|
||||
{
|
||||
return ESTOP;
|
||||
}
|
||||
|
||||
mini_ros::JoyButtons Teleop::return_buttons()
|
||||
{
|
||||
mini_ros::JoyButtons jb;
|
||||
jb.updown = updown;
|
||||
jb.leftright = leftright;
|
||||
jb.left_bump = left_bump;
|
||||
jb.right_bump = right_bump;
|
||||
|
||||
return jb;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,132 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
DESCRIPTION:
|
||||
|
||||
SUBSCRIBERS:
|
||||
"""
|
||||
|
||||
from __future__ import division
|
||||
import rospy
|
||||
from mini_ros.msg import IMUdata, ContactData
|
||||
from std_msgs.msg import String
|
||||
# Used for str(Boolean) --> Boolean
|
||||
from distutils.util import strtobool
|
||||
|
||||
import sys
|
||||
|
||||
import rospkg
|
||||
rospack = rospkg.RosPack()
|
||||
|
||||
sys.path.append(rospack.get_path('mini_ros') + '/../')
|
||||
|
||||
sys.path.append('../../')
|
||||
|
||||
from spot_real.Control.RPi.lib.Teensy_Interface import TeensyInterface
|
||||
from spot_real.Control.RPi.lib.imu import IMU
|
||||
|
||||
|
||||
class SensorInterface():
|
||||
def __init__(self):
|
||||
|
||||
rospy.init_node('SensorInterface', anonymous=True)
|
||||
self.TI = TeensyInterface()
|
||||
self.imu_pub = rospy.Publisher('spot/imu', IMUdata, queue_size=1)
|
||||
self.cnt_pub = rospy.Publisher('spot/contact',
|
||||
ContactData,
|
||||
queue_size=1)
|
||||
self.str_pub = rospy.Publisher('spot/teensydebug',
|
||||
String,
|
||||
queue_size=1)
|
||||
print("PUT SPOT ON THE GROUND, CALIBRATING IMU")
|
||||
self.imu = IMU()
|
||||
|
||||
def read_sensors(self):
|
||||
""" Reads IMU and Contact Sensor data from Teensy 4.0
|
||||
and publishes to respective topics
|
||||
"""
|
||||
while not rospy.is_shutdown():
|
||||
data = self.TI.read_buffer()
|
||||
|
||||
rospy.logdebug(data)
|
||||
|
||||
imu_dat = IMUdata()
|
||||
imu_read = False
|
||||
cnt_dat = ContactData()
|
||||
cnt_read = False
|
||||
|
||||
msg = data.decode().split(",")
|
||||
# REMOVE NEWLINE
|
||||
msg[-1] = msg[-1].rstrip('\r\n')
|
||||
|
||||
# IMU
|
||||
if msg[0] == "IMUDATA":
|
||||
try:
|
||||
imu_msg = [float(x) for x in msg[1:]]
|
||||
# rospy.loginfo(imu_msg)
|
||||
# IMU
|
||||
imu_dat.roll = imu_msg[0]
|
||||
imu_dat.pitch = imu_msg[1]
|
||||
imu_dat.acc_x = imu_msg[2]
|
||||
imu_dat.acc_y = imu_msg[3]
|
||||
imu_dat.acc_z = imu_msg[4]
|
||||
imu_dat.gyro_x = imu_msg[5]
|
||||
imu_dat.gyro_y = imu_msg[6]
|
||||
imu_dat.gyro_z = imu_msg[7]
|
||||
imu_read = True
|
||||
except:
|
||||
rospy.logdebug("bad imu read")
|
||||
|
||||
# CONTACT
|
||||
elif msg[0] == "CONTACT":
|
||||
try:
|
||||
cnt_msg = [strtobool(x) for x in msg[1:]]
|
||||
# rospy.loginfo(cnt_msg)
|
||||
# Contact Sensor
|
||||
cnt_dat.FL = cnt_msg[0]
|
||||
cnt_dat.FR = cnt_msg[1]
|
||||
cnt_dat.BL = cnt_msg[2]
|
||||
cnt_dat.BR = cnt_msg[3]
|
||||
cnt_read = True
|
||||
except:
|
||||
rospy.logdebug("bad contact read")
|
||||
|
||||
# READ IMU
|
||||
self.imu.filter_rpy()
|
||||
imu_dat.roll = self.imu.true_roll
|
||||
imu_dat.pitch = self.imu.true_pitch
|
||||
imu_dat.acc_x = self.imu.imu_data[3]
|
||||
imu_dat.acc_y = self.imu.imu_data[4]
|
||||
imu_dat.acc_z = self.imu.imu_data[5]
|
||||
imu_dat.gyro_x = self.imu.imu_data[0]
|
||||
imu_dat.gyro_y = self.imu.imu_data[1]
|
||||
imu_dat.gyro_z = self.imu.imu_data[2]
|
||||
imu_read = True
|
||||
|
||||
if imu_read:
|
||||
self.imu_pub.publish(imu_dat)
|
||||
# rospy.logdebug("IMU")
|
||||
if cnt_read:
|
||||
self.cnt_pub.publish(cnt_dat)
|
||||
# rospy.logdebug("CONTACT")
|
||||
|
||||
string_msg = String()
|
||||
string_msg.data = data
|
||||
self.str_pub.publish(string_msg)
|
||||
|
||||
|
||||
def main():
|
||||
""" The main() function. """
|
||||
si = SensorInterface()
|
||||
rospy.loginfo("Ready To Log Data!")
|
||||
# rate = rospy.Rate(1.0 / 60.0)
|
||||
while not rospy.is_shutdown():
|
||||
si.read_sensors()
|
||||
# rate.sleep()
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
@@ -0,0 +1,66 @@
|
||||
#!/usr/bin/env python
|
||||
"""
|
||||
DESCRIPTION:
|
||||
|
||||
SUBSCRIBERS:
|
||||
"""
|
||||
|
||||
from __future__ import division
|
||||
import rospy
|
||||
from mini_ros.srv import CalibServo, CalibServoResponse
|
||||
from mini_ros.msg import JointPulse
|
||||
import numpy as np
|
||||
|
||||
import sys
|
||||
|
||||
import rospkg
|
||||
rospack = rospkg.RosPack()
|
||||
|
||||
sys.path.append(rospack.get_path('mini_ros') + '/../')
|
||||
|
||||
sys.path.append('../../')
|
||||
|
||||
|
||||
class ServoCalibrator():
|
||||
def __init__(self):
|
||||
|
||||
rospy.init_node('ServoCalibrator', anonymous=True)
|
||||
|
||||
self.serv = rospy.Service('servo_calibrator', CalibServo,
|
||||
self.calib_service_cb)
|
||||
self.jp_pub = rospy.Publisher('spot/pulse', JointPulse, queue_size=1)
|
||||
|
||||
def calib_service_cb(self, req):
|
||||
""" Requests a servo to be moved to a certain position
|
||||
|
||||
Args: req
|
||||
Returns: response
|
||||
"""
|
||||
try:
|
||||
jp_msg = JointPulse()
|
||||
|
||||
jp_msg.servo_num = req.servo_num
|
||||
jp_msg.servo_pulse = req.servo_pulse
|
||||
|
||||
self.jp_pub.publish(jp_msg)
|
||||
response = "Servo Command Sent."
|
||||
except rospy.ROSInterruptException:
|
||||
response = "FAILED to send Servo Command"
|
||||
return CalibServoResponse(response)
|
||||
|
||||
|
||||
def main():
|
||||
""" The main() function. """
|
||||
srv_calib = ServoCalibrator()
|
||||
rospy.loginfo(
|
||||
"Use The servo_calibrator service (Pulse Width Unit is us (nominal ~500-2500))."
|
||||
)
|
||||
while not rospy.is_shutdown():
|
||||
rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
@@ -0,0 +1,256 @@
|
||||
#!/usr/bin/env python
|
||||
"""
|
||||
DESCRIPTION:
|
||||
|
||||
SUBSCRIBERS:
|
||||
"""
|
||||
|
||||
from __future__ import division
|
||||
import os
|
||||
import rospy
|
||||
import numpy as np
|
||||
from mini_ros.msg import MiniCmd, JoyButtons
|
||||
import copy
|
||||
|
||||
import sys
|
||||
|
||||
import rospkg
|
||||
rospack = rospkg.RosPack()
|
||||
|
||||
|
||||
sys.path.append(rospack.get_path('mini_ros') + '/../')
|
||||
|
||||
sys.path.append('../../')
|
||||
|
||||
from spotmicro.GymEnvs.spot_bezier_env import spotBezierEnv
|
||||
from spotmicro.Kinematics.SpotKinematics import SpotModel
|
||||
from spotmicro.GaitGenerator.Bezier import BezierGait
|
||||
|
||||
# Controller Params
|
||||
STEPLENGTH_SCALE = 0.06
|
||||
Z_SCALE_CTRL = 0.12
|
||||
RPY_SCALE = 0.6
|
||||
SV_SCALE = 0.1
|
||||
CHPD_SCALE = 0.0005
|
||||
YAW_SCALE = 1.5
|
||||
|
||||
# AGENT PARAMS
|
||||
CD_SCALE = 0.05
|
||||
SLV_SCALE = 0.05
|
||||
RESIDUALS_SCALE = 0.03
|
||||
Z_SCALE = 0.05
|
||||
# Filter actions
|
||||
alpha = 0.7
|
||||
# Added this to avoid filtering residuals
|
||||
# -1 for all
|
||||
|
||||
|
||||
class SpotCommander():
|
||||
def __init__(self):
|
||||
|
||||
rospy.init_node('Policies', anonymous=True)
|
||||
self.agents = {}
|
||||
# self.movetypes = [
|
||||
# "Forward", "Backward", "Left", "Right", "CW", "CCW", "Stop"
|
||||
# ]
|
||||
self.movetypes = ["Stop"]
|
||||
self.mini_cmd = MiniCmd()
|
||||
self.jb = JoyButtons()
|
||||
self.mini_cmd.x_velocity = 0.0
|
||||
self.mini_cmd.y_velocity = 0.0
|
||||
self.mini_cmd.rate = 0.0
|
||||
self.mini_cmd.roll = 0.0
|
||||
self.mini_cmd.pitch = 0.0
|
||||
self.mini_cmd.yaw = 0.0
|
||||
self.mini_cmd.z = 0.0
|
||||
self.mini_cmd.motion = "Stop"
|
||||
self.mini_cmd.movement = "Stepping"
|
||||
# FIXED
|
||||
self.BaseStepVelocity = 0.1
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
# Stock, use Bumpers to change
|
||||
self.BaseSwingPeriod = 0.2
|
||||
self.SwingPeriod = self.BaseSwingPeriod
|
||||
# Stock, use arrow pads to change
|
||||
self.BaseClearanceHeight = 0.04
|
||||
self.BasePenetrationDepth = 0.005
|
||||
self.ClearanceHeight = self.BaseClearanceHeight
|
||||
self.PenetrationDepth = self.BasePenetrationDepth
|
||||
|
||||
self.load_spot()
|
||||
# mini_cmd_cb from mini_cmd topic
|
||||
self.sub_cmd = rospy.Subscriber('mini_cmd', MiniCmd, self.mini_cmd_cb)
|
||||
self.sub_jb = rospy.Subscriber('joybuttons', JoyButtons, self.jb_cb)
|
||||
self.time = rospy.get_time()
|
||||
print("READY TO GO!")
|
||||
|
||||
def load_spot(self):
|
||||
|
||||
self.env = spotBezierEnv(render=True,
|
||||
on_rack=False,
|
||||
height_field=False,
|
||||
draw_foot_path=False)
|
||||
|
||||
self.env.reset()
|
||||
|
||||
seed = 0
|
||||
# Set seeds
|
||||
self.env.seed(seed)
|
||||
np.random.seed(seed)
|
||||
|
||||
state_dim = self.env.observation_space.shape[0]
|
||||
print("STATE DIM: {}".format(state_dim))
|
||||
action_dim = self.env.action_space.shape[0]
|
||||
print("ACTION DIM: {}".format(action_dim))
|
||||
max_action = float(self.env.action_space.high[0])
|
||||
print("RECORDED MAX ACTION: {}".format(max_action))
|
||||
|
||||
self.state = self.env.reset()
|
||||
|
||||
# Load Spot Model
|
||||
self.spot = SpotModel()
|
||||
|
||||
self.dt = self.env._time_step
|
||||
|
||||
self.T_bf0 = self.spot.WorldToFoot
|
||||
self.T_bf = copy.deepcopy(self.T_bf0)
|
||||
|
||||
self.bzg = BezierGait(dt=self.env._time_step)
|
||||
|
||||
def mini_cmd_cb(self, mini_cmd):
|
||||
""" Reads the desired Minitaur command and passes it for execution
|
||||
|
||||
Args: mini_cmd
|
||||
"""
|
||||
try:
|
||||
# Update mini_cmd
|
||||
self.mini_cmd = mini_cmd
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(mini_cmd)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def jb_cb(self, jb):
|
||||
""" Reads the desired additional joystick buttons
|
||||
|
||||
Args: jb
|
||||
"""
|
||||
try:
|
||||
# Update jb
|
||||
self.jb = jb
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(jb)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def move(self):
|
||||
""" Turn joystick inputs into commands
|
||||
"""
|
||||
|
||||
if self.mini_cmd.motion != "Stop":
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
self.SwingPeriod = np.clip(
|
||||
self.BaseSwingPeriod +
|
||||
(-self.mini_cmd.faster + -self.mini_cmd.slower) * SV_SCALE,
|
||||
0.1, 0.3)
|
||||
if self.mini_cmd.movement == "Stepping":
|
||||
StepLength = self.mini_cmd.x_velocity + abs(
|
||||
self.mini_cmd.y_velocity * 0.66)
|
||||
StepLength = np.clip(StepLength, -1.0, 1.0)
|
||||
StepLength *= STEPLENGTH_SCALE
|
||||
LateralFraction = self.mini_cmd.y_velocity * np.pi / 2
|
||||
YawRate = self.mini_cmd.rate * YAW_SCALE
|
||||
# x offset
|
||||
pos = np.array(
|
||||
[0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
|
||||
orn = np.array([0.0, 0.0, 0.0])
|
||||
else:
|
||||
StepLength = 0.0
|
||||
LateralFraction = 0.0
|
||||
YawRate = 0.0
|
||||
# RESET
|
||||
self.ClearanceHeight = self.BaseClearanceHeight
|
||||
self.PenetrationDepth = self.BasePenetrationDepth
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
# x offset
|
||||
pos = np.array(
|
||||
[0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
|
||||
orn = np.array([
|
||||
self.mini_cmd.roll * RPY_SCALE,
|
||||
self.mini_cmd.pitch * RPY_SCALE,
|
||||
self.mini_cmd.yaw * RPY_SCALE
|
||||
])
|
||||
else:
|
||||
StepLength = 0.0
|
||||
LateralFraction = 0.0
|
||||
YawRate = 0.0
|
||||
# RESET
|
||||
self.ClearanceHeight = self.BaseClearanceHeight
|
||||
self.PenetrationDepth = self.BasePenetrationDepth
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
self.SwingPeriod = self.BaseSwingPeriod
|
||||
pos = np.array([0.0, 0.0, 0.0])
|
||||
orn = np.array([0.0, 0.0, 0.0])
|
||||
|
||||
# TODO: integrate into controller
|
||||
self.ClearanceHeight += self.jb.updown * CHPD_SCALE
|
||||
self.PenetrationDepth += self.jb.leftright * CHPD_SCALE
|
||||
|
||||
# Manual Reset
|
||||
if self.jb.left_bump or self.jb.right_bump:
|
||||
self.ClearanceHeight = self.BaseClearanceHeight
|
||||
self.PenetrationDepth = self.BasePenetrationDepth
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
self.SwingPeriod = self.BaseSwingPeriod
|
||||
self.env.reset()
|
||||
|
||||
|
||||
# print("SL: {} \tSV: {} \nLAT: {} \tYAW: {}".format(
|
||||
# StepLength, self.StepVelocity, LateralFraction, YawRate))
|
||||
# print("BASE VEL: {}".format(self.BaseStepVelocity))
|
||||
# print("---------------------------------------")
|
||||
|
||||
contacts = self.state[-4:]
|
||||
|
||||
# Time
|
||||
dt = rospy.get_time() - self.time
|
||||
# print("dt: {}".format(dt))
|
||||
self.time = rospy.get_time()
|
||||
|
||||
# Update Step Period
|
||||
self.bzg.Tswing = self.SwingPeriod
|
||||
|
||||
self.T_bf = self.bzg.GenerateTrajectory(StepLength, LateralFraction,
|
||||
YawRate, self.StepVelocity,
|
||||
self.T_bf0, self.T_bf,
|
||||
self.ClearanceHeight,
|
||||
self.PenetrationDepth,
|
||||
contacts, dt)
|
||||
|
||||
joint_angles = self.spot.IK(orn, pos, self.T_bf)
|
||||
self.env.pass_joint_angles(joint_angles.reshape(-1))
|
||||
# Get External Observations
|
||||
# TODO
|
||||
# self.env.spot.GetExternalObservations(bzg, bz_step)
|
||||
# Step
|
||||
action = self.env.action_space.sample()
|
||||
action[:] = 0.0
|
||||
self.state, reward, done, _ = self.env.step(action)
|
||||
|
||||
|
||||
def main():
|
||||
""" The main() function. """
|
||||
mini_commander = SpotCommander()
|
||||
rate = rospy.Rate(600.0)
|
||||
while not rospy.is_shutdown():
|
||||
# This is called continuously. Has timeout functionality too
|
||||
mini_commander.move()
|
||||
rate.sleep()
|
||||
# rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
@@ -0,0 +1,388 @@
|
||||
#!/usr/bin/env python
|
||||
"""
|
||||
DESCRIPTION:
|
||||
|
||||
SUBSCRIBERS:
|
||||
"""
|
||||
|
||||
from __future__ import division
|
||||
import rospy
|
||||
import numpy as np
|
||||
from mini_ros.msg import MiniCmd, JoyButtons, IMUdata, ContactData, AgentData, JointAngles
|
||||
import copy
|
||||
import sys
|
||||
import os
|
||||
import rospkg
|
||||
rospack = rospkg.RosPack()
|
||||
|
||||
sys.path.append(rospack.get_path('mini_ros') + '/../')
|
||||
|
||||
sys.path.append('../../')
|
||||
|
||||
from spotmicro.Kinematics.SpotKinematics import SpotModel
|
||||
from spotmicro.GaitGenerator.Bezier import BezierGait
|
||||
from spot_bullet.src.ars_lib.ars import ARSAgent, Normalizer, Policy
|
||||
from spotmicro.GymEnvs.spot_bezier_env import spotBezierEnv
|
||||
|
||||
# Initialize Node
|
||||
rospy.init_node('Policies', anonymous=True)
|
||||
|
||||
# Controller Params
|
||||
STEPLENGTH_SCALE = rospy.get_param("STEPLENGTH_SCALE")
|
||||
Z_SCALE_CTRL = rospy.get_param("Z_SCALE_CTRL")
|
||||
RPY_SCALE = rospy.get_param("RPY_SCALE")
|
||||
SV_SCALE = rospy.get_param("SV_SCALE")
|
||||
CHPD_SCALE = rospy.get_param("CHPD_SCALE")
|
||||
YAW_SCALE = rospy.get_param("YAW_SCALE")
|
||||
|
||||
# AGENT PARAMS
|
||||
CD_SCALE = rospy.get_param("CD_SCALE")
|
||||
SLV_SCALE = rospy.get_param("SLV_SCALE")
|
||||
RESIDUALS_SCALE = rospy.get_param("RESIDUALS_SCALE")
|
||||
Z_SCALE = rospy.get_param("Z_SCALE")
|
||||
# Filter actions
|
||||
alpha = rospy.get_param("alpha")
|
||||
# Added this to avoid filtering residuals
|
||||
# -1 for all
|
||||
actions_to_filter = rospy.get_param("actions_to_filter")
|
||||
|
||||
|
||||
class SpotCommander():
|
||||
def __init__(self, Agent=True, contacts=False):
|
||||
|
||||
self.Agent = Agent
|
||||
self.agent_num = rospy.get_param("agent_num")
|
||||
self.movetypes = ["Stop"]
|
||||
self.mini_cmd = MiniCmd()
|
||||
self.jb = JoyButtons()
|
||||
self.mini_cmd.x_velocity = 0.0
|
||||
self.mini_cmd.y_velocity = 0.0
|
||||
self.mini_cmd.rate = 0.0
|
||||
self.mini_cmd.roll = 0.0
|
||||
self.mini_cmd.pitch = 0.0
|
||||
self.mini_cmd.yaw = 0.0
|
||||
self.mini_cmd.z = 0.0
|
||||
self.mini_cmd.motion = "Stop"
|
||||
self.mini_cmd.movement = "Stepping"
|
||||
# FIXED
|
||||
self.BaseStepVelocity = rospy.get_param("BaseStepVelocity")
|
||||
self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
|
||||
# Stock, use Bumpers to change
|
||||
self.BaseSwingPeriod = rospy.get_param("Tswing")
|
||||
self.SwingPeriod = copy.deepcopy(self.BaseSwingPeriod)
|
||||
self.SwingPeriod_LIMITS = rospy.get_param("SwingPeriod_LIMITS")
|
||||
# Stock, use arrow pads to change
|
||||
self.BaseClearanceHeight = rospy.get_param("BaseClearanceHeight")
|
||||
self.BasePenetrationDepth = rospy.get_param("BasePenetrationDepth")
|
||||
self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
|
||||
self.PenetrationDepth = copy.deepcopy(self.BasePenetrationDepth)
|
||||
self.ClearanceHeight_LIMITS = rospy.get_param("ClearanceHeight_LIMITS")
|
||||
self.PenetrationDepth_LIMITS = rospy.get_param(
|
||||
"PenetrationDepth_LIMITS")
|
||||
|
||||
# Time
|
||||
self.time = rospy.get_time()
|
||||
|
||||
self.enable_contact = contacts
|
||||
|
||||
# Contacts: FL, FR, BL, BR
|
||||
self.contacts = [0, 0, 0, 0]
|
||||
|
||||
# IMU: R, P, Ax, Ay, Az, Gx, Gy, Gz
|
||||
self.imu = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
|
||||
# See spot_params.yaml in config
|
||||
self.spot = SpotModel(
|
||||
shoulder_length=rospy.get_param("shoulder_length"),
|
||||
elbow_length=rospy.get_param("elbow_length"),
|
||||
wrist_length=rospy.get_param("wrist_length"),
|
||||
hip_x=rospy.get_param("hip_x"),
|
||||
hip_y=rospy.get_param("hip_y"),
|
||||
foot_x=rospy.get_param("foot_x"),
|
||||
foot_y=rospy.get_param("foot_y"),
|
||||
height=rospy.get_param("height"),
|
||||
com_offset=rospy.get_param("com_offset"))
|
||||
|
||||
self.T_bf0 = self.spot.WorldToFoot
|
||||
self.T_bf = copy.deepcopy(self.T_bf0)
|
||||
|
||||
# See spot_params.yaml in config
|
||||
self.bzg = BezierGait(dt=rospy.get_param("dt"),
|
||||
Tswing=rospy.get_param("Tswing"))
|
||||
|
||||
if self.Agent:
|
||||
self.load_spot(contacts, agent_num=self.agent_num)
|
||||
# cmd_cb from mini_cmd topic
|
||||
self.sub_cmd = rospy.Subscriber('mini_cmd',
|
||||
MiniCmd,
|
||||
self.cmd_cb,
|
||||
queue_size=1)
|
||||
self.sub_jb = rospy.Subscriber('joybuttons',
|
||||
JoyButtons,
|
||||
self.jb_cb,
|
||||
queue_size=1)
|
||||
self.sub_imu = rospy.Subscriber('spot/imu',
|
||||
IMUdata,
|
||||
self.imu_cb,
|
||||
queue_size=1)
|
||||
self.sub_cnt = rospy.Subscriber('spot/contact',
|
||||
ContactData,
|
||||
self.cnt_cb,
|
||||
queue_size=1)
|
||||
self.ag_pub = rospy.Publisher('spot/agent', AgentData, queue_size=1)
|
||||
self.ja_pub = rospy.Publisher('spot/joints', JointAngles, queue_size=1)
|
||||
print("READY TO GO!")
|
||||
|
||||
def load_spot(self, contacts, state_dim=12, action_dim=14, agent_num=0):
|
||||
self.policy = Policy(state_dim=state_dim, action_dim=action_dim)
|
||||
self.normalizer = Normalizer(state_dim=state_dim)
|
||||
env = spotBezierEnv(render=False,
|
||||
on_rack=False,
|
||||
height_field=False,
|
||||
draw_foot_path=False)
|
||||
agent = ARSAgent(self.normalizer, self.policy, env)
|
||||
my_path = os.path.abspath(os.path.dirname(__file__))
|
||||
if contacts:
|
||||
models_path = os.path.join(my_path,
|
||||
"../../spot_bullet/models/contact")
|
||||
else:
|
||||
models_path = os.path.join(my_path,
|
||||
"../../spot_bullet/models/no_contact")
|
||||
|
||||
print("MODEL PATH: {}".format(my_path))
|
||||
file_name = "spot_ars_"
|
||||
if os.path.exists(models_path + "/" + file_name + str(agent_num) +
|
||||
"_policy"):
|
||||
print("Loading Existing agent: {}".format(agent_num))
|
||||
agent.load(models_path + "/" + file_name + str(agent_num))
|
||||
agent.policy.episode_steps = np.inf
|
||||
self.policy = agent.policy
|
||||
|
||||
self.action = np.zeros(action_dim)
|
||||
self.old_act = self.action[:actions_to_filter]
|
||||
|
||||
def imu_cb(self, imu):
|
||||
""" Reads the IMU
|
||||
|
||||
Args: imu
|
||||
"""
|
||||
try:
|
||||
# Update imu
|
||||
self.imu = [
|
||||
imu.roll, imu.pitch,
|
||||
np.radians(imu.gyro_x),
|
||||
np.radians(imu.gyro_y),
|
||||
np.radians(imu.gyro_z), imu.acc_x, imu.acc_y, imu.acc_z - 9.81
|
||||
]
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(imu)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def cnt_cb(self, cnt):
|
||||
""" Reads the Contact Sensors
|
||||
|
||||
Args: cnt
|
||||
"""
|
||||
try:
|
||||
# Update contacts
|
||||
self.contacts = [cnt.FL, cnt.FR, cnt.BL, cnt.BR]
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(cnt)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def cmd_cb(self, mini_cmd):
|
||||
""" Reads the desired Minitaur command and passes it for execution
|
||||
|
||||
Args: mini_cmd
|
||||
"""
|
||||
try:
|
||||
# Update mini_cmd
|
||||
self.mini_cmd = mini_cmd
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(mini_cmd)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def jb_cb(self, jb):
|
||||
""" Reads the desired additional joystick buttons
|
||||
|
||||
Args: jb
|
||||
"""
|
||||
try:
|
||||
# Update jb
|
||||
self.jb = jb
|
||||
# log input data as debug-level message
|
||||
rospy.logdebug(jb)
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
def move(self):
|
||||
""" Turn joystick inputs into commands
|
||||
"""
|
||||
|
||||
# Move Type
|
||||
if self.mini_cmd.movement == "Stepping":
|
||||
step_or_view = False
|
||||
else:
|
||||
step_or_view = True
|
||||
|
||||
if self.mini_cmd.motion != "Stop":
|
||||
self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
|
||||
self.SwingPeriod = np.clip(
|
||||
copy.deepcopy(self.BaseSwingPeriod) +
|
||||
(-self.mini_cmd.faster + -self.mini_cmd.slower) * SV_SCALE,
|
||||
self.SwingPeriod_LIMITS[0], self.SwingPeriod_LIMITS[1])
|
||||
if self.mini_cmd.movement == "Stepping":
|
||||
# Only take 2/3 of lateral step length or it will be too great.
|
||||
StepLength = self.mini_cmd.x_velocity + abs(
|
||||
self.mini_cmd.y_velocity * 0.66)
|
||||
StepLength = np.clip(StepLength, -1.0, 1.0)
|
||||
StepLength *= STEPLENGTH_SCALE
|
||||
# Convert lateral joystick action into poolar coordinates
|
||||
# for lateral movement
|
||||
LateralFraction = self.mini_cmd.y_velocity * np.pi / 2
|
||||
YawRate = self.mini_cmd.rate * YAW_SCALE
|
||||
# NOTE: NO HEIGHT MOD DURING WALK
|
||||
pos = np.array([0.0, 0.0, 0.0])
|
||||
orn = np.array([0.0, 0.0, 0.0])
|
||||
else:
|
||||
StepLength = 0.0
|
||||
LateralFraction = 0.0
|
||||
YawRate = 0.0
|
||||
# RESET
|
||||
self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
|
||||
self.PenetrationDepth = copy.deepcopy(
|
||||
self.BasePenetrationDepth)
|
||||
self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
|
||||
# x offset
|
||||
pos = np.array([0.0, 0.0, self.mini_cmd.z * Z_SCALE_CTRL])
|
||||
orn = np.array([
|
||||
self.mini_cmd.roll * RPY_SCALE,
|
||||
self.mini_cmd.pitch * RPY_SCALE,
|
||||
self.mini_cmd.yaw * RPY_SCALE
|
||||
])
|
||||
else:
|
||||
StepLength = 0.0
|
||||
LateralFraction = 0.0
|
||||
YawRate = 0.0
|
||||
# RESET
|
||||
self.ClearanceHeight = self.BaseClearanceHeight
|
||||
self.PenetrationDepth = self.BasePenetrationDepth
|
||||
self.StepVelocity = self.BaseStepVelocity
|
||||
self.SwingPeriod = self.BaseSwingPeriod
|
||||
pos = np.array([0.0, 0.0, 0.0])
|
||||
orn = np.array([0.0, 0.0, 0.0])
|
||||
|
||||
# TODO: integrate into controller
|
||||
self.ClearanceHeight += self.jb.updown * CHPD_SCALE
|
||||
self.PenetrationDepth += self.jb.leftright * CHPD_SCALE
|
||||
|
||||
# Manual Reset
|
||||
if self.jb.left_bump or self.jb.right_bump:
|
||||
self.ClearanceHeight = copy.deepcopy(self.BaseClearanceHeight)
|
||||
self.PenetrationDepth = copy.deepcopy(self.BasePenetrationDepth)
|
||||
self.StepVelocity = copy.deepcopy(self.BaseStepVelocity)
|
||||
self.SwingPeriod = copy.deepcopy(self.BaseSwingPeriod)
|
||||
|
||||
# OPTIONAL: Agent
|
||||
if self.Agent and self.mini_cmd.motion != "Stop":
|
||||
phases = copy.deepcopy(self.bzg.Phases)
|
||||
# Total 12
|
||||
state = []
|
||||
# r, p, gz, gy, gz, ax, ay, az (8)
|
||||
state.extend(self.imu)
|
||||
# FL, FR, BL, BR (4)
|
||||
state.extend(phases)
|
||||
# FL, FR, BL, BR (4)
|
||||
if self.enable_contact:
|
||||
state.extend(self.contacts)
|
||||
self.normalizer.observe(state)
|
||||
# Don't normalize contacts
|
||||
state[:-4] = self.normalizer.normalize(state)[:-4]
|
||||
self.action = self.policy.evaluate(state, None, None)
|
||||
self.action = np.tanh(self.action)
|
||||
# EXP FILTER
|
||||
self.action[:actions_to_filter] = alpha * self.old_act + (
|
||||
1.0 - alpha) * self.action[:actions_to_filter]
|
||||
self.old_act = self.action[:actions_to_filter]
|
||||
|
||||
self.ClearanceHeight += self.action[0] * CD_SCALE
|
||||
|
||||
# Time
|
||||
dt = rospy.get_time() - self.time
|
||||
# print("dt: {}".format(dt))
|
||||
self.time = rospy.get_time()
|
||||
|
||||
# Update Step Period
|
||||
self.bzg.Tswing = self.SwingPeriod
|
||||
|
||||
# CLIP
|
||||
self.ClearanceHeight = np.clip(self.ClearanceHeight,
|
||||
self.ClearanceHeight_LIMITS[0],
|
||||
self.ClearanceHeight_LIMITS[1])
|
||||
self.PenetrationDepth = np.clip(self.PenetrationDepth,
|
||||
self.PenetrationDepth_LIMITS[0],
|
||||
self.PenetrationDepth_LIMITS[1])
|
||||
|
||||
self.T_bf = self.bzg.GenerateTrajectory(StepLength, LateralFraction,
|
||||
YawRate, self.StepVelocity,
|
||||
self.T_bf0, self.T_bf,
|
||||
self.ClearanceHeight,
|
||||
self.PenetrationDepth,
|
||||
self.contacts, dt)
|
||||
|
||||
T_bf_copy = copy.deepcopy(self.T_bf)
|
||||
# OPTIONAL: Agent
|
||||
if self.Agent and self.mini_cmd.motion != "Stop":
|
||||
self.action[2:] *= RESIDUALS_SCALE
|
||||
# Add DELTA to XYZ Foot Poses
|
||||
T_bf_copy["FL"][:3, 3] += self.action[2:5]
|
||||
T_bf_copy["FR"][:3, 3] += self.action[5:8]
|
||||
T_bf_copy["BL"][:3, 3] += self.action[8:11]
|
||||
T_bf_copy["BR"][:3, 3] += self.action[11:14]
|
||||
pos[2] += abs(self.action[1]) * Z_SCALE
|
||||
|
||||
joint_angles = self.spot.IK(orn, pos, T_bf_copy)
|
||||
|
||||
ja_msg = JointAngles()
|
||||
|
||||
ja_msg.fls = np.degrees(joint_angles[0][0])
|
||||
ja_msg.fle = np.degrees(joint_angles[0][1])
|
||||
ja_msg.flw = np.degrees(joint_angles[0][2])
|
||||
|
||||
ja_msg.frs = np.degrees(joint_angles[1][0])
|
||||
ja_msg.fre = np.degrees(joint_angles[1][1])
|
||||
ja_msg.frw = np.degrees(joint_angles[1][2])
|
||||
|
||||
ja_msg.bls = np.degrees(joint_angles[2][0])
|
||||
ja_msg.ble = np.degrees(joint_angles[2][1])
|
||||
ja_msg.blw = np.degrees(joint_angles[2][2])
|
||||
|
||||
ja_msg.brs = np.degrees(joint_angles[3][0])
|
||||
ja_msg.bre = np.degrees(joint_angles[3][1])
|
||||
ja_msg.brw = np.degrees(joint_angles[3][2])
|
||||
|
||||
# Move Type
|
||||
ja_msg.step_or_view = step_or_view
|
||||
|
||||
self.ja_pub.publish(ja_msg)
|
||||
|
||||
|
||||
def main():
|
||||
""" The main() function. """
|
||||
mini_commander = SpotCommander()
|
||||
rate = rospy.Rate(600.0)
|
||||
while not rospy.is_shutdown():
|
||||
# This is called continuously. Has timeout functionality too
|
||||
mini_commander.move()
|
||||
rate.sleep()
|
||||
# rospy.spin()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
@@ -0,0 +1,184 @@
|
||||
/// \file
|
||||
/// \brief Draws Each Obstacle in RViz using MarkerArrays
|
||||
///
|
||||
/// PARAMETERS:
|
||||
/// PUBLISHES:
|
||||
/// SUBSCRIBES:
|
||||
/// FUNCTIONS:
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <mini_ros/spot.hpp>
|
||||
#include <mini_ros/teleop.hpp>
|
||||
#include "mini_ros/MiniCmd.h"
|
||||
#include "std_srvs/Empty.h"
|
||||
#include "std_msgs/Bool.h"
|
||||
|
||||
// Global Vars
|
||||
spot::Spot spot_mini = spot::Spot();
|
||||
bool teleop_flag = false;
|
||||
bool motion_flag = false;
|
||||
bool ESTOP = false;
|
||||
// Init Time
|
||||
ros::Time current_time;
|
||||
ros::Time last_time;
|
||||
|
||||
|
||||
void teleop_callback(const geometry_msgs::Twist &tw)
|
||||
{
|
||||
/// \brief cmd_vel subscriber callback. Records commanded twist
|
||||
///
|
||||
/// \param tw (geometry_msgs::Twist): the commanded linear and angular velocity
|
||||
/**
|
||||
* This function runs every time we get a geometry_msgs::Twist message on the "cmd_vel" topic.
|
||||
* We generally use the const <message>ConstPtr &msg syntax to prevent our node from accidentally
|
||||
* changing the message, in the case that another node is also listening to it.
|
||||
*/
|
||||
|
||||
spot_mini.update_command(tw.linear.x, tw.linear.y, tw.linear.z, tw.angular.z, tw.angular.x, tw.angular.y);
|
||||
}
|
||||
|
||||
void estop_callback(const std_msgs::Bool &estop)
|
||||
{
|
||||
if (estop.data)
|
||||
{
|
||||
spot_mini.update_command(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
motion_flag = true;
|
||||
if (!ESTOP)
|
||||
{
|
||||
ROS_ERROR("ENGAGING MANUAL E-STOP!");
|
||||
ESTOP = true;
|
||||
} else
|
||||
{
|
||||
ROS_WARN("DIS-ENGAGING MANUAL E-STOP!");
|
||||
ESTOP = false;
|
||||
}
|
||||
}
|
||||
|
||||
last_time = ros::Time::now();
|
||||
}
|
||||
|
||||
|
||||
bool swm_callback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
||||
/// Switches the Movement mode from FB (Forward/Backward) to LR (Left/Right)
|
||||
/// and vice versa
|
||||
{
|
||||
spot_mini.switch_movement();
|
||||
motion_flag = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
/// The Main Function ///
|
||||
{
|
||||
ROS_INFO("STARTING NODE: spot_mini State Machine");
|
||||
|
||||
// Vars
|
||||
double frequency = 5;
|
||||
// Seconds for timeout
|
||||
double timeout = 1.0;
|
||||
|
||||
ros::init(argc, argv, "mini_sm_node"); // register the node on ROS
|
||||
ros::NodeHandle nh; // get a handle to ROS
|
||||
ros::NodeHandle nh_("~"); // get a handle to ROS
|
||||
// Parameters
|
||||
nh_.getParam("frequency", frequency);
|
||||
|
||||
// Init Subscriber
|
||||
ros::Subscriber teleop_sub = nh.subscribe("teleop", 1, teleop_callback);
|
||||
ros::Subscriber estop_sub = nh.subscribe("estop", 1, estop_callback);
|
||||
// Init Command Publisher
|
||||
ros::Publisher mini_pub = nh.advertise<mini_ros::MiniCmd>("mini_cmd", 1);
|
||||
|
||||
// Init Switch Movement Service Server
|
||||
ros::ServiceServer switch_movement_server = nh.advertiseService("switch_movement", swm_callback);
|
||||
|
||||
// Init MiniCmd
|
||||
mini_ros::MiniCmd mini_cmd;
|
||||
// Placeholder
|
||||
mini_cmd.x_velocity = 0.0;
|
||||
mini_cmd.y_velocity = 0.0;
|
||||
mini_cmd.rate = 0.0;
|
||||
mini_cmd.roll = 0.0;
|
||||
mini_cmd.pitch = 0.0;
|
||||
mini_cmd.yaw = 0.0;
|
||||
mini_cmd.z = 0.0;
|
||||
mini_cmd.faster = 0.0;
|
||||
mini_cmd.slower = 0.0;
|
||||
mini_cmd.motion = "Stop";
|
||||
mini_cmd.movement = "Stepping";
|
||||
|
||||
ros::Rate rate(frequency);
|
||||
current_time = ros::Time::now();
|
||||
last_time = ros::Time::now();
|
||||
// Main While
|
||||
while (ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
current_time = ros::Time::now();
|
||||
|
||||
spot::SpotCommand cmd = spot_mini.return_command();
|
||||
|
||||
// Condition for sending non-stop command
|
||||
if (!motion_flag and !(current_time.toSec() - last_time.toSec() > timeout) and !ESTOP)
|
||||
{
|
||||
mini_cmd.x_velocity = cmd.x_velocity;
|
||||
mini_cmd.y_velocity = cmd.y_velocity;
|
||||
mini_cmd.rate = cmd.rate;
|
||||
mini_cmd.roll = cmd.roll;
|
||||
mini_cmd.pitch = cmd.pitch;
|
||||
mini_cmd.yaw = cmd.yaw;
|
||||
mini_cmd.z = cmd.z;
|
||||
mini_cmd.faster = cmd.faster;
|
||||
mini_cmd.slower = cmd.slower;
|
||||
// Now convert enum to string
|
||||
// Motion
|
||||
if (cmd.motion == spot::Go)
|
||||
{
|
||||
mini_cmd.motion = "Go";
|
||||
} else
|
||||
{
|
||||
mini_cmd.motion = "Stop";
|
||||
}
|
||||
// Movement
|
||||
if (cmd.movement == spot::Stepping)
|
||||
{
|
||||
mini_cmd.movement = "Stepping";
|
||||
} else
|
||||
{
|
||||
mini_cmd.movement = "Viewing";
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
mini_cmd.x_velocity = 0.0;
|
||||
mini_cmd.y_velocity = 0.0;
|
||||
mini_cmd.rate = 0.0;
|
||||
mini_cmd.roll = 0.0;
|
||||
mini_cmd.pitch = 0.0;
|
||||
mini_cmd.yaw = 0.0;
|
||||
mini_cmd.z = 0.0;
|
||||
mini_cmd.faster = 0.0;
|
||||
mini_cmd.slower = 0.0;
|
||||
mini_cmd.motion = "Stop";
|
||||
}
|
||||
|
||||
if (current_time.toSec() - last_time.toSec() > timeout)
|
||||
{
|
||||
ROS_ERROR("TIMEOUT...ENGAGING E-STOP!");
|
||||
}
|
||||
|
||||
// Now publish
|
||||
mini_pub.publish(mini_cmd);
|
||||
motion_flag = false;
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
/// \file
|
||||
/// \brief Draws Each Obstacle in RViz using MarkerArrays
|
||||
///
|
||||
/// PARAMETERS:
|
||||
/// PUBLISHES:
|
||||
/// SUBSCRIBES:
|
||||
/// FUNCTIONS:
|
||||
|
||||
#include <ros/ros.h>
|
||||
|
||||
#include <math.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include <mini_ros/spot.hpp>
|
||||
#include <mini_ros/teleop.hpp>
|
||||
#include "mini_ros/MiniCmd.h"
|
||||
#include "std_msgs/Bool.h"
|
||||
#include "std_srvs/Empty.h"
|
||||
#include "mini_ros/JoyButtons.h"
|
||||
|
||||
#include <functional> // To use std::bind
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
/// The Main Function ///
|
||||
{
|
||||
ROS_INFO("STARTING NODE: Teleoperation");
|
||||
|
||||
// Vars
|
||||
double frequency = 60;
|
||||
int linear_x = 4;
|
||||
int linear_y = 3;
|
||||
int linear_z = 1;
|
||||
int angular = 0;
|
||||
int sw = 0;
|
||||
int es = 1;
|
||||
int RB = 5;
|
||||
int LB = 2;
|
||||
int RT = 5;
|
||||
int LT = 4;
|
||||
int UD = 7;
|
||||
int LR = 6;
|
||||
double l_scale = 1.0;
|
||||
double a_scale = 1.0;
|
||||
double B_scale = 1.0;
|
||||
double debounce_thresh = 0.15; // sec
|
||||
|
||||
ros::init(argc, argv, "teleop_node"); // register the node on ROS
|
||||
ros::NodeHandle nh; // get a handle to ROS
|
||||
ros::NodeHandle nh_("~"); // get a handle to ROS
|
||||
// Parameters
|
||||
nh_.getParam("frequency", frequency);
|
||||
nh_.getParam("axis_linear_x", linear_x);
|
||||
nh_.getParam("axis_linear_y", linear_y);
|
||||
nh_.getParam("axis_linear_z", linear_z);
|
||||
nh_.getParam("axis_angular", angular);
|
||||
nh_.getParam("scale_linear", l_scale);
|
||||
nh_.getParam("scale_angular", a_scale);
|
||||
nh_.getParam("scale_bumper", B_scale);
|
||||
nh_.getParam("button_switch", sw);
|
||||
nh_.getParam("button_estop", es);
|
||||
nh_.getParam("rb", RB);
|
||||
nh_.getParam("lb", LB);
|
||||
nh_.getParam("rt", RT);
|
||||
nh_.getParam("lt", LT);
|
||||
nh_.getParam("updown", UD);
|
||||
nh_.getParam("leftright", LR);
|
||||
nh_.getParam("debounce_thresh", debounce_thresh);
|
||||
|
||||
tele::Teleop teleop = tele::Teleop(linear_x, linear_y, linear_z, angular,
|
||||
l_scale, a_scale, LB, RB, B_scale, LT,
|
||||
RT, UD, LR, sw, es);
|
||||
|
||||
// Init Switch Movement Server
|
||||
ros::ServiceClient switch_movement_client = nh.serviceClient<std_srvs::Empty>("switch_movement");
|
||||
ros::service::waitForService("switch_movement", -1);
|
||||
|
||||
// Init ESTOP Publisher
|
||||
ros::Publisher estop_pub = nh.advertise<std_msgs::Bool>("estop", 1);
|
||||
// Init Command Publisher
|
||||
ros::Publisher vel_pub = nh.advertise<geometry_msgs::Twist>("teleop", 1);
|
||||
// Init Joy Button Publisher
|
||||
ros::Publisher jb_pub = nh.advertise<mini_ros::JoyButtons>("joybuttons", 1);
|
||||
|
||||
// Init Subscriber (also handles pub)
|
||||
// TODO: Figure out how to use std::bind properly
|
||||
// ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 1, std::bind(&tele::Teleop::joyCallback, std::placeholders::_1, vel_pub), &teleop);
|
||||
ros::Subscriber joy_sub = nh.subscribe<sensor_msgs::Joy>("joy", 1, &tele::Teleop::joyCallback, &teleop);
|
||||
|
||||
ros::Rate rate(frequency);
|
||||
|
||||
// Record time for debouncing buttons
|
||||
ros::Time current_time = ros::Time::now();
|
||||
ros::Time last_time = ros::Time::now();
|
||||
|
||||
|
||||
// Main While
|
||||
while (ros::ok())
|
||||
{
|
||||
ros::spinOnce();
|
||||
current_time = ros::Time::now();
|
||||
|
||||
std_msgs::Bool estop;
|
||||
estop.data = teleop.return_estop();
|
||||
|
||||
if (estop.data and current_time.toSec() - last_time.toSec() >= debounce_thresh)
|
||||
{
|
||||
ROS_INFO("SENDING E-STOP COMMAND!");
|
||||
last_time = ros::Time::now();
|
||||
} else if (!teleop.return_trigger())
|
||||
{
|
||||
// Send Twist
|
||||
vel_pub.publish(teleop.return_twist());
|
||||
estop.data = 0;
|
||||
} else if (current_time.toSec() - last_time.toSec() >= debounce_thresh)
|
||||
{
|
||||
// Call Switch Service
|
||||
std_srvs::Empty e;
|
||||
switch_movement_client.call(e);
|
||||
estop.data = 0;
|
||||
last_time = ros::Time::now();
|
||||
}
|
||||
// pub buttons
|
||||
jb_pub.publish(teleop.return_buttons());
|
||||
|
||||
estop_pub.publish(estop);
|
||||
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user