actionlib support in roscs?
Hi, Does ROSCS supports actionlib and also using custom messages?
ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
Hi, Does ROSCS supports actionlib and also using custom messages?
Reading the documentation at http://wiki.ros.org/roscs appears to support custom messages but there does not appear to be actionlib support.
Tully is right. You need to send the msgs, which are autogenerated by actionlib and then you are missing the support of actionlib server and/or client. Here is one messy little class that did the job for us so far.
using System;
using System.Threading;
using System.Collections;
using System.Collections.Generic;
using RosCS;
using RosCS.geometry_msgs;
using RosCS.nav_msgs;
using RosCS.tmc200_msgs;
using RosCS.actionlib_msgs;
using RosCS.move_base_msgs;
using RosCS.Services;
using RosCS.Services.nav_msgs;
using Castor;
public class ArmHelper
{
public static Pose PointOfInterest { get; private set; }
public static bool NewPOI { get; set; }
public static bool ExplorationDone { get; set; }
public static GoalID CurrentGoalID{get; private set;}
public static GoalStatusArray GoalStatus {get; private set;}
public static MoveBaseActionFeedback GoalFeedback {get; private set;}
public static MoveBaseActionResult GoalResult {get; set;}
public static bool MotionEnabled { get; set; }
public static string MotionType { get; set; }
protected static Node rosNode;
protected static int id = 0;
protected static MoveBaseActionGoal MoveBaseGoal{get; private set;}
protected static SystemConfig sc = SystemConfig.LocalInstance;
protected static ArmHelper instance = null;
protected static object lockobject = new object();
public static ArmHelper Get() {
if(instance == null) {
lock(lockobject) {
if(instance == null) {
instance = new ArmHelper();
//Thread.MemoryBarrier();
}
}
}
return instance;
}
protected static Publisher motionPub;
protected static Publisher goalPositionPub;
protected static Publisher mBaseGoalPositionPub;
protected static Publisher mBaseCancelGoalPub;
protected static string GenerateUniqueId()
{
id++;
return "goal_" + id;
}
protected static GoalID GenerateActionLibGoalId()
{
CurrentGoalID = new GoalID();
CurrentGoalID.Stamp = RosSharp.Now();
CurrentGoalID.Id = GenerateUniqueId();
return CurrentGoalID;
}
public static void RosInit()
{
if(rosNode!= null) return;
rosNode = Node.MainNode;
ExplorationDone = false;
MotionEnabled = false;
MotionType = sc["ImperaHelper"].GetString("ImperaHelper.PathHelper.MotionType");
mBaseCancelGoalPub = new Publisher(Node.MainNode, "/move_base/cancel", GoalID.TypeId, 1);
goalPositionPub = new Publisher(Node.MainNode, "/move_base_simple/goal", PoseStamped.TypeId, 1);
mBaseGoalPositionPub = new Publisher(Node.MainNode, "/move_base/goal", MoveBaseActionGoal.TypeId, 1);
motionPub = new Publisher(Node.MainNode,"MotionControl",MotionControl.TypeId,1);
Node.MainNode.Subscribe("/move_base/status", OnGoalStatus);
Node.MainNode.Subscribe("/move_base/feedback", OnGoalFeedback);
Node.MainNode.Subscribe("/move_base/result", OnGoalResult);
Node.MainNode.Subscribe("/PointOfInterest", OnPOI);
Node.MainNode.Subscribe("/cmd_vel", OnTwist);
}
protected ArmHelper()
{
if(rosNode == null) {
RosInit();
}
}
protected static void OnTwist(Twist tc)
{
if (!MotionEnabled)
return;
if(MotionType.Equals("tmc"))
{
MotionControl mc = new MotionControl();
mc.Motion.Angle = Math.Atan2(tc.Linear.Y, tc.Linear.X);
mc.Motion.Translation = 1000*Math.Sqrt(tc.Linear.X * tc.Linear.X + tc.Linear.Y * tc.Linear.Y);
mc.Motion.Rotation = tc.Angular.Z;
//Console.WriteLine("Velocity : " + mc.Motion.Translation + " Turn : " + mc.Motion.Rotation + " Angle : " + mc.Motion.Angle);
motionPub.Send(mc);
}
}
protected static void OnPOI(PoseStamped ps) {
NewPOI = true;
PointOfInterest = ps.Pose;
Console.WriteLine("PH: Got new POI");
}
protected static void OnGoalStatus(GoalStatusArray gStatus)
{
GoalStatus = new GoalStatusArray();
GoalStatus.Header = gStatus.Header;
GoalStatus.Status_list = gStatus.Status_list;
//Console.WriteLine("Total Goal Count: "+GoalStatus.Status_list.Count);
}
protected static void OnGoalFeedback(MoveBaseActionFeedback feedback)
{
GoalFeedback = new MoveBaseActionFeedback();
GoalFeedback.Feedback = feedback.Feedback;
GoalFeedback.Header = feedback.Header;
GoalFeedback.Status = feedback.Status;
//Console.WriteLine("Feedback: GoalID: "+GoalFeedback.Status.Goal_id.Id+ " Status: "+GoalFeedback.Status.Status);
}
protected static void OnGoalResult(MoveBaseActionResult result)
{
GoalResult = new MoveBaseActionResult();
GoalResult.Header = result.Header;
GoalResult.Result = result.Result;
GoalResult.Status = result.Status;
Console.WriteLine("Result: GoalID: "+GoalResult.Status.Goal_id.Id ...
(more)Asked: 2012-07-10 22:58:31 -0600
Seen: 445 times
Last updated: Dec 15 '13
cancelGoal crashes if called before any goal has been sent...
genaction not found in diamondback
Using Smach and SimpleActionState
Action server with more than one action...
Subscribing to action servers' feedback topic
SimpleActionState doesn't abort on timeout